diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/Featherstone')
24 files changed, 0 insertions, 8341 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp deleted file mode 100644 index 62865e0c78..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ /dev/null @@ -1,2043 +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 - -///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals -bool gJointFeedbackInWorldSpace = false; -bool gJointFeedbackInJointFrame = false; - -namespace { - const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) - const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds -} - -namespace { - void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame - const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates - const btVector3 &top_in, // top part of input vector - const btVector3 &bottom_in, // bottom part of input vector - btVector3 &top_out, // top part of output vector - btVector3 &bottom_out) // bottom part of output vector - { - top_out = rotation_matrix * top_in; - bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; - } - -#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 - -} - - -// -// Implementation of class btMultiBody -// - -btMultiBody::btMultiBody(int n_links, - btScalar mass, - const btVector3 &inertia, - bool fixedBase, - bool canSleep, - bool /*deprecatedUseMultiDof*/) - : - m_baseCollider(0), - m_baseName(0), - m_basePos(0,0,0), - m_baseQuat(0, 0, 0, 1), - m_baseMass(mass), - m_baseInertia(inertia), - - m_fixedBase(fixedBase), - m_awake(true), - m_canSleep(canSleep), - m_sleepTimer(0), - m_userObjectPointer(0), - m_userIndex2(-1), - m_userIndex(-1), - m_linearDamping(0.04f), - m_angularDamping(0.04f), - m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), - m_maxCoordinateVelocity(100.f), - m_hasSelfCollision(true), - __posUpdated(false), - m_dofCount(0), - m_posVarCnt(0), - m_useRK4(false), - m_useGlobalVelocities(false), - m_internalNeedsJointFeedback(false) -{ - m_cachedInertiaTopLeft.setValue(0,0,0,0,0,0,0,0,0); - m_cachedInertiaTopRight.setValue(0,0,0,0,0,0,0,0,0); - m_cachedInertiaLowerLeft.setValue(0,0,0,0,0,0,0,0,0); - m_cachedInertiaLowerRight.setValue(0,0,0,0,0,0,0,0,0); - m_cachedInertiaValid=false; - - m_links.resize(n_links); - m_matrixBuf.resize(n_links + 1); - - m_baseForce.setValue(0, 0, 0); - m_baseTorque.setValue(0, 0, 0); -} - -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(); -} - -void btMultiBody::finalizeMultiDof() -{ - m_deltaV.resize(0); - m_deltaV.resize(6 + m_dofCount); - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) - for (int i=0;i<m_vectorBuf.size();i++) - { - m_vectorBuf[i].setValue(0,0,0); - } - updateLinksDofOffsets(); -} - -int btMultiBody::getParent(int i) const -{ - return m_links[i].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, btScalar *q) -{ - for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos) - m_links[i].m_jointPos[pos] = 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, btScalar *qdot) -{ - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - m_realBuf[6 + m_links[i].m_dofOffset + dof] = 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; -} - -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 - omega[0] = quatRotate(m_baseQuat ,getBaseOmega()); - vel[0] = quatRotate(m_baseQuat ,getBaseVel()); - - for (int i = 0; i < num_links; ++i) - { - const int parent = m_links[i].m_parent; - - // transform parent vel into this frame, store in omega[i+1], vel[i+1] - SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector, - omega[parent+1], vel[parent+1], - omega[i+1], vel[i+1]); - - // now add qidot * shat_i - //only supported for revolute/prismatic joints, todo: spherical and planar joints - switch(m_links[i].m_jointType) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - btVector3 axisTop = m_links[i].getAxisTop(0); - btVector3 axisBottom = m_links[i].getAxisBottom(0); - btScalar jointVel = getJointVel(i); - omega[i+1] += jointVel * axisTop; - vel[i+1] += jointVel * axisBottom; - break; - } - default: - { - } - } - } -} - -btScalar btMultiBody::getKineticEnergy() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1); - btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - // we will do the factor of 0.5 at the end - btScalar result = m_baseMass * vel[0].dot(vel[0]); - result += omega[0].dot(m_baseInertia * omega[0]); - - for (int i = 0; i < num_links; ++i) { - result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]); - result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]); - } - - return 0.5f * result; -} - -btVector3 btMultiBody::getAngularMomentum() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1); - btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1); - btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - rot_from_world[0] = m_baseQuat; - btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0])); - - for (int i = 0; i < num_links; ++i) { - rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1]; - result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1]))); - } - - return result; -} - -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]; -} - -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) -{ - // 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; - - btVector3 base_vel = getBaseVel(); - 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 + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount - scratch_v.resize(8*num_links + 6); - scratch_m.resize(4*num_links + 4); - - //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 (m_fixedBase) - { - zeroAccSpatFrc[0].setZero(); - } - else - { - btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; - btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; - //external forces - zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); - - //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().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 - // - //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())); - - // 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]) - ); - // - //p += vhat x Ihat vhat - done in a simpler way - if(m_useGyroTerm) - zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular())); - // - zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); - //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); - ////clamp parent's omega - //btScalar parOmegaMod = temp.length(); - //btScalar parOmegaModMax = 1000; - //if(parOmegaMod > parOmegaModMax) - // temp *= parOmegaModMax / parOmegaMod; - //zeroAccSpatFrc[i+1].addLinear(temp); - //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); - //temp = spatCoriolisAcc[i].getLinear(); - //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); - - - - //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); - //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); - //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) - { - 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) - { - 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: - { - invDi[0] = 1.0f / D[0]; - break; - } - case btMultibodyLink::eSpherical: - case btMultibodyLink::ePlanar: - { - btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); - btMatrix3x3 invD3x3; 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) - { - 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) - { - 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) - { - 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 (m_fixedBase) - { - 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]); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); - } - - 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 (gJointFeedbackInJointFrame) - { - //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 (gJointFeedbackInWorldSpace) - { - 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. - btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); - output[0] = omegadot_out[0]; - output[1] = omegadot_out[1]; - output[2] = omegadot_out[2]; - - btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); - 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 - result[0] = rhs_bot[0] / m_baseInertia[0]; - result[1] = rhs_bot[1] / m_baseInertia[1]; - result[2] = rhs_bot[2] / m_baseInertia[2]; - result[3] = rhs_top[0] / m_baseMass; - result[4] = rhs_top[1] / m_baseMass; - result[5] = rhs_top[2] / m_baseMass; - } else - { - if (!m_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 - result.setAngular(rhs.getAngular() / m_baseInertia); - result.setLinear(rhs.getLinear() / m_baseMass); - } 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(btScalar *pA, 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 (m_fixedBase) - { - 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) - { - 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 (m_fixedBase) - { - 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) - { - 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::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) -{ - int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - // - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; - - /////////////////////////////// - //local functor for quaternion integration (to avoid error prone redundancy) - struct - { - //"exponential map" based on btTransformUtil::integrateTransform(..) - void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) - { - //baseBody => quat is alias and omega is global coor - //!baseBody => quat is alibi and omega is local coor - - btVector3 axis; - btVector3 angvel; - - if(!baseBody) - angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok - else - angvel = omega; - - btScalar fAngle = angvel.length(); - //limit the angular motion - if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) - { - fAngle = btScalar(0.5)*SIMD_HALF_PI / dt; - } - - if ( fAngle < btScalar(0.001) ) - { - // use Taylor's expansions of sync function - axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle ); - } - else - { - // sync(fAngle) = sin(c*fAngle)/t - axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle ); - } - - if(!baseBody) - quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat; - else - quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) )); - //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); - - quat.normalize(); - } - } pQuatUpdateFun; - /////////////////////////////// - - //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); - // - btScalar *pBaseQuat = 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) - { - btScalar *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: - { - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - 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_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m) const -{ - // temporary space - int num_links = getNumLinks(); - int m_dofCount = getNumDofs(); - scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang - scratch_m.resize(num_links + 1); - - btVector3 * v_ptr = &scratch_v[0]; - btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1; - btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - - scratch_r.resize(m_dofCount); - btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0; - - btMatrix3x3 * rot_from_world = &scratch_m[0]; - - 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: speed this up -- don't calculate for m_links we don't need. - // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, - // which is resulting in repeated work being done...) - - // calculate required normals & positions in the local frames. - for (int i = 0; i < num_links; ++i) { - - // 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_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 < SLEEP_EPSILON) { - m_sleepTimer += timestep; - if (m_sleepTimer > SLEEP_TIMEOUT) { - goToSleep(); - } - } else { - m_sleepTimer = 0; - if (!m_awake) - wakeUp(); - } -} - - -void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin) -{ - - 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); - - } - - 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); - } - } -} - -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; - getBaseWorldTransform().serialize(mbd->m_baseWorldTransform); - 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); - 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_parentComToThisComOffset); - getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); - getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); - btAssert(memPtr->m_dofCount<=3); - for (int dof = 0;dof<getLink(i).m_dofCount;dof++) - { - getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]); - getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]); - - memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof]; - memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof]; - - } - 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; -} diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.h deleted file mode 100644 index 655165ac18..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.h +++ /dev/null @@ -1,814 +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 linkIndex, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true); - - - void setupPrismatic(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision); - - void setupRevolute(int linkIndex, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parentIndex, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &jointAxis, // in my frame - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - void setupSpherical(int linkIndex, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - void setupPlanar(int i, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &rotationAxis, - const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame - bool disableParentCollision=false); - - const btMultibodyLink& getLink(int index) const - { - 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; - } - - 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; - } // rotates world vectors into base frame - btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame - - void setBasePos(const btVector3 &pos) - { - m_basePos = pos; - } - - void 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 setBaseVel(const btVector3 &vel) - { - - m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; - } - void setWorldToBaseRot(const btQuaternion &rot) - { - m_baseQuat = rot; //m_baseQuat asumed to ba alias!? - } - void setBaseOmega(const btVector3 &omega) - { - m_realBuf[0]=omega[0]; - m_realBuf[1]=omega[1]; - m_realBuf[2]=omega[2]; - } - - - // - // get/set pos/vel for child m_links (i = 0 to num_links-1) - // - - 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, btScalar *q); - void setJointVelMultiDof(int i, btScalar *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]; - } -/* 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. - - - // - // transform vectors in local frame of link i to world frame (or vice versa) - // - btVector3 localPosToWorld(int i, const btVector3 &vec) const; - btVector3 localDirToWorld(int i, const btVector3 &vec) const; - btVector3 worldPosToLocal(int i, const btVector3 &vec) const; - btVector3 worldDirToLocal(int i, const btVector3 &vec) const; - - // - // transform a frame in local coordinate to a frame in world coordinate - // - btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const; - - // - // calculate kinetic energy and angular momentum - // useful for debugging. - // - - btScalar getKineticEnergy() const; - btVector3 getAngularMomentum() const; - - - // - // set external forces and torques. Note all external forces/torques are given in the WORLD frame. - // - - 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=false - ); - -///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead - void stepVelocitiesMultiDof(btScalar dt, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m, - bool isConstraintPass=false) - { - computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass); - } - - // calcAccelerationDeltasMultiDof - // input: force vector (in same format as jacobian, i.e.: - // 3 torque values, 3 force values, num_links joint torque values) - // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values - // (existing contents of output array are replaced) - // calcAccelerationDeltasMultiDof must have been called first. - 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 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); - - - // - // 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) - { - m_canSleep = canSleep; - } - - bool getCanSleep()const - { - return m_canSleep; - } - - bool isAwake() const { return m_awake; } - void wakeUp(); - void goToSleep(); - void checkMotionAndSleepIfRequired(btScalar timestep); - - bool hasFixedBase() const - { - return m_fixedBase; - } - - 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>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); - - void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; - - void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); - - 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; - } - -private: - btMultiBody(const btMultiBody &); // not implemented - void operator=(const btMultiBody &); // not implemented - - - void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const; - void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; - - void updateLinksDofOffsets() - { - int dofOffset = 0, cfgOffset = 0; - for(int bidx = 0; bidx < m_links.size(); ++bidx) - { - m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; - dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; - } - } - - void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; - - -private: - - btMultiBodyLinkCollider* m_baseCollider;//can be NULL - const char* m_baseName;//memory needs to be manager by user! - - btVector3 m_basePos; // position of COM of base (world frame) - btQuaternion m_baseQuat; // rotates world points into base frame - - btScalar m_baseMass; // mass of the base - btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) - - btVector3 m_baseForce; // external force applied to base. World frame. - btVector3 m_baseTorque; // external torque applied to base. World frame. - - btVector3 m_baseConstraintForce; // external force applied to base. World frame. - btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. - - btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1. - - - // - // realBuf: - // offset size array - // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] - // 6+num_links num_links D - // - // vectorBuf: - // offset size array - // 0 num_links h_top - // num_links num_links h_bottom - // - // matrixBuf: - // offset size array - // 0 num_links+1 rot_from_parent - // - btAlignedObjectArray<btScalar> m_deltaV; - btAlignedObjectArray<btScalar> m_realBuf; - btAlignedObjectArray<btVector3> m_vectorBuf; - btAlignedObjectArray<btMatrix3x3> m_matrixBuf; - - - btMatrix3x3 m_cachedInertiaTopLeft; - btMatrix3x3 m_cachedInertiaTopRight; - btMatrix3x3 m_cachedInertiaLowerLeft; - btMatrix3x3 m_cachedInertiaLowerRight; - bool m_cachedInertiaValid; - - bool m_fixedBase; - - // Sleep parameters. - bool m_awake; - bool m_canSleep; - btScalar m_sleepTimer; - - void* m_userObjectPointer; - int m_userIndex2; - int m_userIndex; - - int m_companionId; - btScalar m_linearDamping; - btScalar m_angularDamping; - bool m_useGyroTerm; - btScalar m_maxAppliedImpulse; - btScalar m_maxCoordinateVelocity; - bool m_hasSelfCollision; - - bool __posUpdated; - int m_dofCount, m_posVarCnt; - bool m_useRK4, m_useGlobalVelocities; - - ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only - bool m_internalNeedsJointFeedback; -}; - -struct btMultiBodyLinkDoubleData -{ - btQuaternionDoubleData m_zeroRotParentToThis; - btVector3DoubleData m_parentComToThisComOffset; - btVector3DoubleData m_thisPivotToThisComOffset; - btVector3DoubleData m_jointAxisTop[6]; - btVector3DoubleData m_jointAxisBottom[6]; - - btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) - double m_linkMass; - int m_parentIndex; - int m_jointType; - - int m_dofCount; - int m_posVarCount; - double m_jointPos[7]; - double m_jointVel[6]; - double m_jointTorque[6]; - - double m_jointDamping; - double m_jointFriction; - double m_jointLowerLimit; - double m_jointUpperLimit; - double m_jointMaxForce; - double m_jointMaxVelocity; - - char *m_linkName; - char *m_jointName; - btCollisionObjectDoubleData *m_linkCollider; - char *m_paddingPtr; - -}; - -struct btMultiBodyLinkFloatData -{ - btQuaternionFloatData m_zeroRotParentToThis; - btVector3FloatData m_parentComToThisComOffset; - btVector3FloatData m_thisPivotToThisComOffset; - btVector3FloatData m_jointAxisTop[6]; - btVector3FloatData m_jointAxisBottom[6]; - btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) - int m_dofCount; - float m_linkMass; - int m_parentIndex; - int m_jointType; - - - - float m_jointPos[7]; - float m_jointVel[6]; - float m_jointTorque[6]; - int m_posVarCount; - 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 -{ - btTransformDoubleData m_baseWorldTransform; - btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) - double m_baseMass; - - char *m_baseName; - btMultiBodyLinkDoubleData *m_links; - btCollisionObjectDoubleData *m_baseCollider; - char *m_paddingPtr; - int m_numLinks; - char m_padding[4]; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btMultiBodyFloatData -{ - char *m_baseName; - btMultiBodyLinkFloatData *m_links; - btCollisionObjectFloatData *m_baseCollider; - btTransformFloatData m_baseWorldTransform; - btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal) - - float m_baseMass; - int m_numLinks; -}; - - - -#endif diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp deleted file mode 100644 index d52852dd8e..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ /dev/null @@ -1,417 +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) - :m_bodyA(bodyA), - m_bodyB(bodyB), - m_linkA(linkA), - m_linkB(linkB), - m_numRows(numRows), - m_jacSizeA(0), - m_jacSizeBoth(0), - m_isUnilateral(isUnilateral), - m_numDofsFinalized(-1), - m_maxAppliedImpulse(100) -{ - -} - -void btMultiBodyConstraint::updateJacobianSizes() -{ - if(m_bodyA) - { - 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) -{ - 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 = rb0->getInvMass() + constraintNormalAng.dot(vec); - } - 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 = rb1->getInvMass() + constraintNormalAng.dot(vec); - } - 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->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - 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->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - } - - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - - btScalar 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/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h deleted file mode 100644 index 83521b9501..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.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_MULTIBODY_CONSTRAINT_H -#define BT_MULTIBODY_CONSTRAINT_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btMultiBody.h" - -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_numRows; - int m_jacSizeA; - int m_jacSizeBoth; - int m_posOffset; - - bool m_isUnilateral; - int m_numDofsFinalized; - btScalar m_maxAppliedImpulse; - - - // warning: the data block lay out is not consistent for all constraints - // data block laid out as follows: - // cached impulses. (one per row.) - // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) - // positions. (one per row.) - btAlignedObjectArray<btScalar> m_data; - - void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - - btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& constraintNormalAng, - - const btVector3& constraintNormalLin, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - bool angConstraint = false, - - btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); - virtual ~btMultiBodyConstraint(); - - void updateJacobianSizes(); - void allocateJacobiansMultiDof(); - - //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; - } - - 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/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp deleted file mode 100644 index 1e2d074096..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ /dev/null @@ -1,1429 +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" - -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 - - //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - - 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); - leastSquaredResidual += residual*residual; - - if(constraint.m_multiBodyA) - constraint.m_multiBodyA->setPosUpdated(false); - if(constraint.m_multiBodyB) - constraint.m_multiBodyB->setPosUpdated(false); - } - - //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 += residual*residual; - - if(constraint.m_multiBodyA) - constraint.m_multiBodyA->setPosUpdated(false); - if(constraint.m_multiBodyB) - constraint.m_multiBodyB->setPosUpdated(false); - } - - //solve featherstone frictional contact - - 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 += 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_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); - } - return deltaImpulse; -} - - - - -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - - BT_PROFILE("setupMultiBodyContactConstraint"); - btVector3 rel_pos1; - btVector3 rel_pos2; - - 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; - } - } - } - - - ///warm starting (or zero if disabled) - //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); - - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - 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->applyDeltaVeeMultiDof(deltaV,impulse); - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - 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; - - - - } - -} - -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,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, 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"); - 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; - - 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=1; - - for (int j=0;j<manifold->getNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= manifold->getContactProcessingThreshold()) - { - - btScalar relaxation; - - int frictionIndex = m_multiBodyNormalContactConstraints.size(); - - btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); - - // btRigidBody* rb0 = btRigidBody::upcast(colObj0); - // btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - 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, 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 = frictionIndex; - - ///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) - { - addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); - } - if (cp.m_combinedRollingFriction>0) - { - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - - if (cp.m_lateralFrictionDir1.length()>0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - - if (cp.m_lateralFrictionDir2.length()>0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - } - rollingFriction--; - } - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) - {/* - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - 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,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - } - - 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,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM); - - //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - //todo: - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - } - - -#endif //ENABLE_FRICTION - - } - } -} - -void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) -{ - //btPersistentManifold* manifold = 0; - - 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); - } - -} - - - -btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) -{ - return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); -} - -#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) - { - - if(c.m_multiBodyA->isMultiDof()) - { - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } - else - { - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } - } - - if (c.m_multiBodyB) - { - if(c.m_multiBodyB->isMultiDof()) - { - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } - else - { - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } - } -#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); - } - - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - 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_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; - } - //do a callback here? - } - } -#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 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/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h deleted file mode 100644 index 489347d874..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ /dev/null @@ -1,100 +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; - - 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); - - - void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); - - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, - btScalar* jacA,btScalar* jacB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal); - - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - //either rolling or spinning friction - void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); - void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime); -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - ///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/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp deleted file mode 100644 index 9eacc22647..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ /dev/null @@ -1,991 +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::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) - { - 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); -} - - -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<btPersistentManifold*> m_manifolds; - btAlignedObjectArray<btTypedConstraint*> m_constraints; - btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; - - - MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_multiBodySortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - - } - - MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) - { - btAssert(solverInfo); - m_solverInfo = solverInfo; - - m_multiBodySortedConstraints = sortedMultiBodyConstraints; - m_numMultiBodyConstraints = numMultiBodyConstraints; - m_sortedConstraints = sortedConstraints; - m_numConstraints = numConstraints; - - m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); - m_multiBodyConstraints.resize(0); - } - - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) - { - if (islandId<0) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - btMultiBodyConstraint** startMultiBodyConstraint = 0; - - int numCurConstraints = 0; - int numCurMultiBodyConstraints = 0; - - int i; - - //find the first constraint for this island - - for (i=0;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - startConstraint = &m_sortedConstraints[i]; - break; - } - } - //count the number of constraints in this island - for (;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - numCurConstraints++; - } - } - - for (i=0;i<m_numMultiBodyConstraints;i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - - startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; - break; - } - } - //count the number of multi body constraints in this island - for (;i<m_numMultiBodyConstraints;i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - numCurMultiBodyConstraints++; - } - } - - //if (m_solverInfo->m_minimumSolverBatchSize<=1) - //{ - // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - //} else - { - - for (i=0;i<numBodies;i++) - m_bodies.push_back(bodies[i]); - for (i=0;i<numManifolds;i++) - m_manifolds.push_back(manifolds[i]); - for (i=0;i<numCurConstraints;i++) - m_constraints.push_back(startConstraint[i]); - - for (i=0;i<numCurMultiBodyConstraints;i++) - m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); - - if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - void processConstraints() - { - - btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; - btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; - btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; - - //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); - - m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } - -}; - - - -btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) - :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver) -{ - //split impulse is not yet supported for Featherstone hierarchies -// getSolverInfo().m_splitImpulse = false; - getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; - m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); -} - -btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () -{ - delete m_solverMultiBodyIslandCallback; -} - -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) -{ - forwardKinematics(); - - - - BT_PROFILE("solveConstraints"); - - m_sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;i<getNumConstraints();i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i=0;i<m_multiBodyConstraints.size();i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); - - 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()); - - /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); - -#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; - - { - if(!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m); - } - else - { - // - int numDofs = bod->getNumDofs() + 6; - int numPosVars = bod->getNumPosVars() + 7; - btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); - //convenience - btScalar *pMem = &scratch_r2[0]; - btScalar *scratch_q0 = pMem; pMem += numPosVars; - btScalar *scratch_qx = pMem; pMem += numPosVars; - btScalar *scratch_qd0 = pMem; pMem += numDofs; - btScalar *scratch_qd1 = pMem; pMem += numDofs; - btScalar *scratch_qd2 = pMem; pMem += numDofs; - btScalar *scratch_qd3 = pMem; pMem += numDofs; - btScalar *scratch_qdd0 = pMem; pMem += numDofs; - btScalar *scratch_qdd1 = pMem; pMem += numDofs; - btScalar *scratch_qdd2 = pMem; pMem += numDofs; - btScalar *scratch_qdd3 = pMem; pMem += numDofs; - btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); - - ///// - //copy q0 to scratch_q0 and qd0 to scratch_qd0 - scratch_q0[0] = bod->getWorldToBaseRot().x(); - scratch_q0[1] = bod->getWorldToBaseRot().y(); - scratch_q0[2] = bod->getWorldToBaseRot().z(); - scratch_q0[3] = bod->getWorldToBaseRot().w(); - scratch_q0[4] = bod->getBasePos().x(); - scratch_q0[5] = bod->getBasePos().y(); - scratch_q0[6] = bod->getBasePos().z(); - // - for(int link = 0; link < bod->getNumLinks(); ++link) - { - for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; - } - // - for(int dof = 0; dof < numDofs; ++dof) - scratch_qd0[dof] = bod->getVelocityVector()[dof]; - //// - struct - { - btMultiBody *bod; - 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); - 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); - 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); - 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); - 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); - } - - } - } - -#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - bod->clearForcesAndTorques(); -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - }//if (!isSleeping) - } - } - - clearMultiBodyConstraintForces(); - - m_solverMultiBodyIslandCallback->processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); - - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;b<bod->getNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks()+1); - m_scratch_m.resize(bod->getNumLinks()+1); - - - { - if(!bod->isUsingRK4Integration()) - { - bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass); - } - } - } - } - } - - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->processDeltaVeeMultiDof2(); - } - -} - -void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btDiscreteDynamicsWorld::integrateTransforms(timeStep); - - { - BT_PROFILE("btMultiBody stepPositions"); - //integrate and update the Featherstone hierarchies - - for (int b=0;b<m_multiBodies.size();b++) - { - 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(); - - ///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); - - } 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); - - getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); - - - for (int m = 0; m<bod->getNumLinks(); m++) - { - - const btTransform& tr = bod->getLink(m).m_cachedWorldTransform; - - getDebugDrawer()->drawTransform(tr, 0.1); - - //draw the joint axis - if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute) - { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); - } - if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed) - { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); - } - if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic) - { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); - } - - } - } - } - } - - -} - - - -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); - - 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); - } - } - -}
\ No newline at end of file diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h deleted file mode 100644 index c0c132bbba..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ /dev/null @@ -1,114 +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" - -#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 solveConstraints(btContactSolverInfo& solverInfo); - - virtual void serializeMultiBodies(btSerializer* serializer); - -public: - - btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btMultiBodyDynamicsWorld (); - - 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); - - 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); - -}; -#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp deleted file mode 100644 index 1f94117aa9..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ /dev/null @@ -1,211 +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), - 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), - 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) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyFixedConstraint::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->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/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h deleted file mode 100644 index 036025136e..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h +++ /dev/null @@ -1,94 +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/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp deleted file mode 100644 index 5fdb7007d8..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ /dev/null @@ -1,184 +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), - 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) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyGearConstraint::getIslandIdB() const -{ - if (m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->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]; - 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/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h deleted file mode 100644 index 0115de6241..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ /dev/null @@ -1,117 +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/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h deleted file mode 100644 index 5c2fa8ed5b..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h +++ /dev/null @@ -1,27 +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/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp deleted file mode 100644 index 6d173b66a1..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ /dev/null @@ -1,205 +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), - 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) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyJointLimitConstraint::getIslandIdB() const -{ - if(m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->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/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h deleted file mode 100644 index 55b8d122b9..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ /dev/null @@ -1,50 +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) - } - -}; - -#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp deleted file mode 100644 index e0921178e9..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ /dev/null @@ -1,186 +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), - 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), - 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 -{ - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - return -1; -} - -int btMultiBodyJointMotor::getIslandIdB() const -{ - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->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/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h deleted file mode 100644 index 4063bed79a..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ /dev/null @@ -1,81 +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/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h deleted file mode 100644 index 01828e5843..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ /dev/null @@ -1,244 +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. - - 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]; - - //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_collider(0), - m_flags(0), - m_dofCount(0), - m_posVarCount(0), - m_jointType(btMultibodyLink::eInvalid), - m_jointFeedback(0), - m_linkName(0), - m_jointName(0), - m_userPtr(0), - m_jointDamping(0), - m_jointFriction(0), - m_jointLowerLimit(0), - m_jointUpperLimit(0), - m_jointMaxForce(0), - m_jointMaxVelocity(0) - { - - m_inertiaLocal.setValue(1, 1, 1); - setAxisTop(0, 0., 0., 0.); - setAxisBottom(0, 1., 0., 0.); - m_dVector.setValue(0, 0, 0); - m_eVector.setValue(0, 0, 0); - m_cachedRVector.setValue(0, 0, 0); - m_appliedForce.setValue( 0, 0, 0); - m_appliedTorque.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]); - - switch(m_jointType) - { - case eRevolute: - { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); - - break; - } - case ePrismatic: - { - // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0); - - break; - } - case eSpherical: - { - m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); - - break; - } - case ePlanar: - { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector); - - break; - } - case eFixed: - { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); - - break; - } - default: - { - //invalid type - btAssert(0); - } - } - } -}; - - -#endif //BT_MULTIBODY_LINK_H diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h deleted file mode 100644 index 671e15d314..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ /dev/null @@ -1,125 +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" - -class btMultiBodyLinkCollider : public btCollisionObject -{ -//protected: -public: - - btMultiBody* m_multiBody; - int m_link; - - - 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; - } -}; - -#endif //BT_FEATHERSTONE_LINK_COLLIDER_H - diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp deleted file mode 100644 index 125d52ad0b..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ /dev/null @@ -1,221 +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), - 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), - 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) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyPoint2Point::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->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/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h deleted file mode 100644 index bf39acc5b9..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ /dev/null @@ -1,68 +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/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp deleted file mode 100644 index 3b64b8183f..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ /dev/null @@ -1,230 +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), - 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), - 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) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodySliderConstraint::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->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/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h deleted file mode 100644 index 0a6cf3df12..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h +++ /dev/null @@ -1,105 +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/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h deleted file mode 100644 index 6fa1550e9e..0000000000 --- a/thirdparty/bullet/src/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 |