summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp')
-rw-r--r--thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp2043
1 files changed, 2043 insertions, 0 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp
new file mode 100644
index 0000000000..62865e0c78
--- /dev/null
+++ b/thirdparty/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -0,0 +1,2043 @@
+/*
+ * 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;
+}