2330 lines
76 KiB
C++
2330 lines
76 KiB
C++
/*
|
|
* PURPOSE:
|
|
* Class representing an articulated rigid body. Stores the body's
|
|
* current state, allows forces and torques to be set, handles
|
|
* timestepping and implements Featherstone's algorithm.
|
|
*
|
|
* COPYRIGHT:
|
|
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
|
|
* Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
|
|
* Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
|
|
*/
|
|
|
|
#include "btMultiBody.h"
|
|
#include "btMultiBodyLink.h"
|
|
#include "btMultiBodyLinkCollider.h"
|
|
#include "btMultiBodyJointFeedback.h"
|
|
#include "LinearMath/btTransformUtil.h"
|
|
#include "LinearMath/btSerializer.h"
|
|
//#include "Bullet3Common/b3Logging.h"
|
|
// #define INCLUDE_GYRO_TERM
|
|
|
|
|
|
namespace
|
|
{
|
|
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
|
|
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
|
|
} // namespace
|
|
|
|
void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
|
|
const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
|
|
const btVector3 &top_in, // top part of input vector
|
|
const btVector3 &bottom_in, // bottom part of input vector
|
|
btVector3 &top_out, // top part of output vector
|
|
btVector3 &bottom_out) // bottom part of output vector
|
|
{
|
|
top_out = rotation_matrix * top_in;
|
|
bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
|
|
}
|
|
|
|
namespace
|
|
{
|
|
|
|
|
|
#if 0
|
|
void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
|
|
const btVector3 &displacement,
|
|
const btVector3 &top_in,
|
|
const btVector3 &bottom_in,
|
|
btVector3 &top_out,
|
|
btVector3 &bottom_out)
|
|
{
|
|
top_out = rotation_matrix.transpose() * top_in;
|
|
bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
|
|
}
|
|
|
|
btScalar SpatialDotProduct(const btVector3 &a_top,
|
|
const btVector3 &a_bottom,
|
|
const btVector3 &b_top,
|
|
const btVector3 &b_bottom)
|
|
{
|
|
return a_bottom.dot(b_top) + a_top.dot(b_bottom);
|
|
}
|
|
|
|
void SpatialCrossProduct(const btVector3 &a_top,
|
|
const btVector3 &a_bottom,
|
|
const btVector3 &b_top,
|
|
const btVector3 &b_bottom,
|
|
btVector3 &top_out,
|
|
btVector3 &bottom_out)
|
|
{
|
|
top_out = a_top.cross(b_top);
|
|
bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
|
|
}
|
|
#endif
|
|
|
|
} // namespace
|
|
|
|
//
|
|
// Implementation of class btMultiBody
|
|
//
|
|
|
|
btMultiBody::btMultiBody(int n_links,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
bool fixedBase,
|
|
bool canSleep,
|
|
bool /*deprecatedUseMultiDof*/)
|
|
: m_baseCollider(0),
|
|
m_baseName(0),
|
|
m_basePos(0, 0, 0),
|
|
m_baseQuat(0, 0, 0, 1),
|
|
m_basePos_interpolate(0, 0, 0),
|
|
m_baseQuat_interpolate(0, 0, 0, 1),
|
|
m_baseMass(mass),
|
|
m_baseInertia(inertia),
|
|
|
|
m_fixedBase(fixedBase),
|
|
m_awake(true),
|
|
m_canSleep(canSleep),
|
|
m_canWakeup(true),
|
|
m_sleepTimer(0),
|
|
m_userObjectPointer(0),
|
|
m_userIndex2(-1),
|
|
m_userIndex(-1),
|
|
m_companionId(-1),
|
|
m_linearDamping(0.04f),
|
|
m_angularDamping(0.04f),
|
|
m_useGyroTerm(true),
|
|
m_maxAppliedImpulse(1000.f),
|
|
m_maxCoordinateVelocity(100.f),
|
|
m_hasSelfCollision(true),
|
|
__posUpdated(false),
|
|
m_dofCount(0),
|
|
m_posVarCnt(0),
|
|
m_useRK4(false),
|
|
m_useGlobalVelocities(false),
|
|
m_internalNeedsJointFeedback(false)
|
|
{
|
|
m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
m_cachedInertiaValid = false;
|
|
|
|
m_links.resize(n_links);
|
|
m_matrixBuf.resize(n_links + 1);
|
|
|
|
m_baseForce.setValue(0, 0, 0);
|
|
m_baseTorque.setValue(0, 0, 0);
|
|
|
|
clearConstraintForces();
|
|
clearForcesAndTorques();
|
|
}
|
|
|
|
btMultiBody::~btMultiBody()
|
|
{
|
|
}
|
|
|
|
void btMultiBody::setupFixed(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &parentComToThisPivotOffset,
|
|
const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
|
|
{
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].setAxisTop(0, 0., 0., 0.);
|
|
m_links[i].setAxisBottom(0, btVector3(0, 0, 0));
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].m_dVector = thisPivotToThisComOffset;
|
|
m_links[i].m_eVector = parentComToThisPivotOffset;
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::eFixed;
|
|
m_links[i].m_dofCount = 0;
|
|
m_links[i].m_posVarCount = 0;
|
|
|
|
m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
|
|
m_links[i].updateCacheMultiDof();
|
|
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
void btMultiBody::setupPrismatic(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &jointAxis,
|
|
const btVector3 &parentComToThisPivotOffset,
|
|
const btVector3 &thisPivotToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
m_dofCount += 1;
|
|
m_posVarCnt += 1;
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].setAxisTop(0, 0., 0., 0.);
|
|
m_links[i].setAxisBottom(0, jointAxis);
|
|
m_links[i].m_eVector = parentComToThisPivotOffset;
|
|
m_links[i].m_dVector = thisPivotToThisComOffset;
|
|
m_links[i].m_cachedRotParentToThis = rotParentToThis;
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::ePrismatic;
|
|
m_links[i].m_dofCount = 1;
|
|
m_links[i].m_posVarCount = 1;
|
|
m_links[i].m_jointPos[0] = 0.f;
|
|
m_links[i].m_jointTorque[0] = 0.f;
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
|
|
m_links[i].updateCacheMultiDof();
|
|
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
void btMultiBody::setupRevolute(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &jointAxis,
|
|
const btVector3 &parentComToThisPivotOffset,
|
|
const btVector3 &thisPivotToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
m_dofCount += 1;
|
|
m_posVarCnt += 1;
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].setAxisTop(0, jointAxis);
|
|
m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
|
|
m_links[i].m_dVector = thisPivotToThisComOffset;
|
|
m_links[i].m_eVector = parentComToThisPivotOffset;
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::eRevolute;
|
|
m_links[i].m_dofCount = 1;
|
|
m_links[i].m_posVarCount = 1;
|
|
m_links[i].m_jointPos[0] = 0.f;
|
|
m_links[i].m_jointTorque[0] = 0.f;
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
m_links[i].updateCacheMultiDof();
|
|
//
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
void btMultiBody::setupSpherical(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &parentComToThisPivotOffset,
|
|
const btVector3 &thisPivotToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
m_dofCount += 3;
|
|
m_posVarCnt += 4;
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].m_dVector = thisPivotToThisComOffset;
|
|
m_links[i].m_eVector = parentComToThisPivotOffset;
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::eSpherical;
|
|
m_links[i].m_dofCount = 3;
|
|
m_links[i].m_posVarCount = 4;
|
|
m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
|
|
m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
|
|
m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
|
|
m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
|
|
m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
|
|
m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
|
|
m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
|
|
m_links[i].m_jointPos[3] = 1.f;
|
|
m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
m_links[i].updateCacheMultiDof();
|
|
//
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
void btMultiBody::setupPlanar(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &rotationAxis,
|
|
const btVector3 &parentComToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
m_dofCount += 3;
|
|
m_posVarCnt += 3;
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].m_dVector.setZero();
|
|
m_links[i].m_eVector = parentComToThisComOffset;
|
|
|
|
//
|
|
btVector3 vecNonParallelToRotAxis(1, 0, 0);
|
|
if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
|
|
vecNonParallelToRotAxis.setValue(0, 1, 0);
|
|
//
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::ePlanar;
|
|
m_links[i].m_dofCount = 3;
|
|
m_links[i].m_posVarCount = 3;
|
|
btVector3 n = rotationAxis.normalized();
|
|
m_links[i].setAxisTop(0, n[0], n[1], n[2]);
|
|
m_links[i].setAxisTop(1, 0, 0, 0);
|
|
m_links[i].setAxisTop(2, 0, 0, 0);
|
|
m_links[i].setAxisBottom(0, 0, 0, 0);
|
|
btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
|
|
m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
|
|
cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
|
|
m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
|
|
m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
|
|
m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
m_links[i].updateCacheMultiDof();
|
|
//
|
|
updateLinksDofOffsets();
|
|
|
|
m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
|
|
m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
|
|
}
|
|
|
|
void btMultiBody::finalizeMultiDof()
|
|
{
|
|
m_deltaV.resize(0);
|
|
m_deltaV.resize(6 + m_dofCount);
|
|
m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
|
|
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
|
|
m_matrixBuf.resize(m_links.size() + 1);
|
|
for (int i = 0; i < m_vectorBuf.size(); i++)
|
|
{
|
|
m_vectorBuf[i].setValue(0, 0, 0);
|
|
}
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
int btMultiBody::getParent(int link_num) const
|
|
{
|
|
return m_links[link_num].m_parent;
|
|
}
|
|
|
|
btScalar btMultiBody::getLinkMass(int i) const
|
|
{
|
|
return m_links[i].m_mass;
|
|
}
|
|
|
|
const btVector3 &btMultiBody::getLinkInertia(int i) const
|
|
{
|
|
return m_links[i].m_inertiaLocal;
|
|
}
|
|
|
|
btScalar btMultiBody::getJointPos(int i) const
|
|
{
|
|
return m_links[i].m_jointPos[0];
|
|
}
|
|
|
|
btScalar btMultiBody::getJointVel(int i) const
|
|
{
|
|
return m_realBuf[6 + m_links[i].m_dofOffset];
|
|
}
|
|
|
|
btScalar *btMultiBody::getJointPosMultiDof(int i)
|
|
{
|
|
return &m_links[i].m_jointPos[0];
|
|
}
|
|
|
|
btScalar *btMultiBody::getJointVelMultiDof(int i)
|
|
{
|
|
return &m_realBuf[6 + m_links[i].m_dofOffset];
|
|
}
|
|
|
|
const btScalar *btMultiBody::getJointPosMultiDof(int i) const
|
|
{
|
|
return &m_links[i].m_jointPos[0];
|
|
}
|
|
|
|
const btScalar *btMultiBody::getJointVelMultiDof(int i) const
|
|
{
|
|
return &m_realBuf[6 + m_links[i].m_dofOffset];
|
|
}
|
|
|
|
void btMultiBody::setJointPos(int i, btScalar q)
|
|
{
|
|
m_links[i].m_jointPos[0] = q;
|
|
m_links[i].updateCacheMultiDof();
|
|
}
|
|
|
|
|
|
void btMultiBody::setJointPosMultiDof(int i, const double *q)
|
|
{
|
|
for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
|
|
m_links[i].m_jointPos[pos] = (btScalar)q[pos];
|
|
|
|
m_links[i].updateCacheMultiDof();
|
|
}
|
|
|
|
void btMultiBody::setJointPosMultiDof(int i, const float *q)
|
|
{
|
|
for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
|
|
m_links[i].m_jointPos[pos] = (btScalar)q[pos];
|
|
|
|
m_links[i].updateCacheMultiDof();
|
|
}
|
|
|
|
|
|
|
|
void btMultiBody::setJointVel(int i, btScalar qdot)
|
|
{
|
|
m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
|
|
}
|
|
|
|
void btMultiBody::setJointVelMultiDof(int i, const double *qdot)
|
|
{
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
|
|
}
|
|
|
|
void btMultiBody::setJointVelMultiDof(int i, const float* qdot)
|
|
{
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
|
|
}
|
|
|
|
const btVector3 &btMultiBody::getRVector(int i) const
|
|
{
|
|
return m_links[i].m_cachedRVector;
|
|
}
|
|
|
|
const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
|
|
{
|
|
return m_links[i].m_cachedRotParentToThis;
|
|
}
|
|
|
|
const btVector3 &btMultiBody::getInterpolateRVector(int i) const
|
|
{
|
|
return m_links[i].m_cachedRVector_interpolate;
|
|
}
|
|
|
|
const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const
|
|
{
|
|
return m_links[i].m_cachedRotParentToThis_interpolate;
|
|
}
|
|
|
|
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
|
|
{
|
|
btAssert(i >= -1);
|
|
btAssert(i < m_links.size());
|
|
if ((i < -1) || (i >= m_links.size()))
|
|
{
|
|
return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
|
|
}
|
|
|
|
btVector3 result = local_pos;
|
|
while (i != -1)
|
|
{
|
|
// 'result' is in frame i. transform it to frame parent(i)
|
|
result += getRVector(i);
|
|
result = quatRotate(getParentToLocalRot(i).inverse(), result);
|
|
i = getParent(i);
|
|
}
|
|
|
|
// 'result' is now in the base frame. transform it to world frame
|
|
result = quatRotate(getWorldToBaseRot().inverse(), result);
|
|
result += getBasePos();
|
|
|
|
return result;
|
|
}
|
|
|
|
btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
|
|
{
|
|
btAssert(i >= -1);
|
|
btAssert(i < m_links.size());
|
|
if ((i < -1) || (i >= m_links.size()))
|
|
{
|
|
return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
|
|
}
|
|
|
|
if (i == -1)
|
|
{
|
|
// world to base
|
|
return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos()));
|
|
}
|
|
else
|
|
{
|
|
// find position in parent frame, then transform to current frame
|
|
return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
|
|
}
|
|
}
|
|
|
|
btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
|
|
{
|
|
btAssert(i >= -1);
|
|
btAssert(i < m_links.size());
|
|
if ((i < -1) || (i >= m_links.size()))
|
|
{
|
|
return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
|
|
}
|
|
|
|
btVector3 result = local_dir;
|
|
while (i != -1)
|
|
{
|
|
result = quatRotate(getParentToLocalRot(i).inverse(), result);
|
|
i = getParent(i);
|
|
}
|
|
result = quatRotate(getWorldToBaseRot().inverse(), result);
|
|
return result;
|
|
}
|
|
|
|
btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
|
|
{
|
|
btAssert(i >= -1);
|
|
btAssert(i < m_links.size());
|
|
if ((i < -1) || (i >= m_links.size()))
|
|
{
|
|
return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
|
|
}
|
|
|
|
if (i == -1)
|
|
{
|
|
return quatRotate(getWorldToBaseRot(), world_dir);
|
|
}
|
|
else
|
|
{
|
|
return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir));
|
|
}
|
|
}
|
|
|
|
btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
|
|
{
|
|
btMatrix3x3 result = local_frame;
|
|
btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
|
|
btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
|
|
btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
|
|
result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
|
|
return result;
|
|
}
|
|
|
|
void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
|
|
{
|
|
int num_links = getNumLinks();
|
|
// Calculates the velocities of each link (and the base) in its local frame
|
|
const btQuaternion& base_rot = getWorldToBaseRot();
|
|
omega[0] = quatRotate(base_rot, getBaseOmega());
|
|
vel[0] = quatRotate(base_rot, getBaseVel());
|
|
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
const btMultibodyLink& link = getLink(i);
|
|
const int parent = link.m_parent;
|
|
|
|
// transform parent vel into this frame, store in omega[i+1], vel[i+1]
|
|
spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,
|
|
omega[parent + 1], vel[parent + 1],
|
|
omega[i + 1], vel[i + 1]);
|
|
|
|
// now add qidot * shat_i
|
|
const btScalar* jointVel = getJointVelMultiDof(i);
|
|
for (int dof = 0; dof < link.m_dofCount; ++dof)
|
|
{
|
|
omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
|
|
vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
|
|
}
|
|
}
|
|
}
|
|
|
|
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,
|
|
bool jointFeedbackInWorldSpace,
|
|
bool jointFeedbackInJointFrame)
|
|
{
|
|
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
|
|
// and the base linear & angular accelerations.
|
|
|
|
// We apply damping forces in this routine as well as any external forces specified by the
|
|
// caller (via addBaseForce etc).
|
|
|
|
// output should point to an array of 6 + num_links reals.
|
|
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
|
|
// num_links joint acceleration values.
|
|
|
|
// We added support for multi degree of freedom (multi dof) joints.
|
|
// In addition we also can compute the joint reaction forces. This is performed in a second pass,
|
|
// so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
|
|
|
|
m_internalNeedsJointFeedback = false;
|
|
|
|
int num_links = getNumLinks();
|
|
|
|
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
|
|
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
|
|
|
|
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
|
|
const btScalar DAMPING_K2_ANGULAR = m_angularDamping;
|
|
|
|
const btVector3 base_vel = getBaseVel();
|
|
const btVector3 base_omega = getBaseOmega();
|
|
|
|
// Temporary matrices/vectors -- use scratch space from caller
|
|
// so that we don't have to keep reallocating every frame
|
|
|
|
scratch_r.resize(2 * m_dofCount + 7); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
|
|
scratch_v.resize(8 * num_links + 6);
|
|
scratch_m.resize(4 * num_links + 4);
|
|
|
|
//btScalar * r_ptr = &scratch_r[0];
|
|
btScalar *output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
|
|
btVector3 *v_ptr = &scratch_v[0];
|
|
|
|
// vhat_i (top = angular, bottom = linear part)
|
|
btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
|
|
v_ptr += num_links * 2 + 2;
|
|
//
|
|
// zhat_i^A
|
|
btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
|
|
v_ptr += num_links * 2 + 2;
|
|
//
|
|
// chat_i (note NOT defined for the base)
|
|
btSpatialMotionVector *spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
|
|
v_ptr += num_links * 2;
|
|
//
|
|
// Ihat_i^A.
|
|
btSymmetricSpatialDyad *spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
|
|
|
|
// Cached 3x3 rotation matrices from parent frame to this frame.
|
|
btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
|
|
btMatrix3x3 *rot_from_world = &scratch_m[0];
|
|
|
|
// hhat_i, ahat_i
|
|
// hhat is NOT stored for the base (but ahat is)
|
|
btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
|
|
btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
|
|
v_ptr += num_links * 2 + 2;
|
|
//
|
|
// Y_i, invD_i
|
|
btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
|
|
btScalar *Y = &scratch_r[0];
|
|
//
|
|
//aux variables
|
|
btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
|
|
btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
|
|
btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
|
|
btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
|
|
btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
|
|
btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
|
|
btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
|
|
btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
|
|
btSpatialTransformationMatrix fromWorld;
|
|
fromWorld.m_trnVec.setZero();
|
|
/////////////////
|
|
|
|
// ptr to the joint accel part of the output
|
|
btScalar *joint_accel = output + 6;
|
|
|
|
// Start of the algorithm proper.
|
|
|
|
// First 'upward' loop.
|
|
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
|
|
|
rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
|
|
|
|
//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
|
|
spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
|
|
|
|
if (m_fixedBase)
|
|
{
|
|
zeroAccSpatFrc[0].setZero();
|
|
}
|
|
else
|
|
{
|
|
const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;
|
|
const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;
|
|
//external forces
|
|
zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
|
|
|
|
//adding damping terms (only)
|
|
const btScalar linDampMult = 1., angDampMult = 1.;
|
|
zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
|
|
linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
|
|
|
|
//
|
|
//p += vhat x Ihat vhat - done in a simpler way
|
|
if (m_useGyroTerm)
|
|
zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
|
|
//
|
|
zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
|
|
}
|
|
|
|
//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
|
|
spatInertia[0].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
|
|
//
|
|
btMatrix3x3(m_baseMass, 0, 0,
|
|
0, m_baseMass, 0,
|
|
0, 0, m_baseMass),
|
|
//
|
|
btMatrix3x3(m_baseInertia[0], 0, 0,
|
|
0, m_baseInertia[1], 0,
|
|
0, 0, m_baseInertia[2]));
|
|
|
|
rot_from_world[0] = rot_from_parent[0];
|
|
|
|
//
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
const int parent = m_links[i].m_parent;
|
|
rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
|
|
rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
|
|
|
|
fromParent.m_rotMat = rot_from_parent[i + 1];
|
|
fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
|
fromWorld.m_rotMat = rot_from_world[i + 1];
|
|
fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
|
|
|
|
// now set vhat_i to its true value by doing
|
|
// vhat_i += qidot * shat_i
|
|
if (!m_useGlobalVelocities)
|
|
{
|
|
spatJointVel.setZero();
|
|
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
|
|
|
|
// remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
|
|
spatVel[i + 1] += spatJointVel;
|
|
|
|
//
|
|
// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
|
|
//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
|
|
}
|
|
else
|
|
{
|
|
fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i + 1]);
|
|
fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
|
|
}
|
|
|
|
// we can now calculate chat_i
|
|
spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]);
|
|
|
|
// calculate zhat_i^A
|
|
//
|
|
//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)
|
|
{
|
|
const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
|
|
D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
|
|
}
|
|
}
|
|
|
|
btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
|
|
switch (m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::ePrismatic:
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
if (D[0] >= SIMD_EPSILON)
|
|
{
|
|
invDi[0] = 1.0f / D[0];
|
|
}
|
|
else
|
|
{
|
|
invDi[0] = 0;
|
|
}
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
|
|
const btMatrix3x3 invD3x3(D3x3.inverse());
|
|
|
|
//unroll the loop?
|
|
for (int row = 0; row < 3; ++row)
|
|
{
|
|
for (int col = 0; col < 3; ++col)
|
|
{
|
|
invDi[row * 3 + col] = invD3x3[row][col];
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
|
|
//determine h*D^{-1}
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
spatForceVecTemps[dof].setZero();
|
|
|
|
for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
{
|
|
const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
|
|
//
|
|
spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
|
|
}
|
|
}
|
|
|
|
dyadTemp = spatInertia[i + 1];
|
|
|
|
//determine (h*D^{-1}) * h^{T}
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
|
|
}
|
|
|
|
fromParent.transformInverse(dyadTemp, spatInertia[parent + 1], btSpatialTransformationMatrix::Add);
|
|
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
invD_times_Y[dof] = 0.f;
|
|
|
|
for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
{
|
|
invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
|
|
}
|
|
}
|
|
|
|
spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
|
|
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
spatForceVecTemps[0] += hDof * invD_times_Y[dof];
|
|
}
|
|
|
|
fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
|
|
|
|
zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
|
|
}
|
|
|
|
// Second 'upward' loop
|
|
// (part of TreeForwardDynamics in Mirtich)
|
|
|
|
if (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)
|
|
{
|
|
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
|
|
}
|
|
|
|
btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
|
|
//D^{-1} * (Y - h^{T}*apar)
|
|
mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
|
|
|
|
spatAcc[i + 1] += spatCoriolisAcc[i];
|
|
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
|
|
|
|
if (m_links[i].m_jointFeedback)
|
|
{
|
|
m_internalNeedsJointFeedback = true;
|
|
|
|
btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
|
|
btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
|
|
|
|
if (jointFeedbackInJointFrame)
|
|
{
|
|
//shift the reaction forces to the joint frame
|
|
//linear (force) component is the same
|
|
//shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector
|
|
angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
|
|
}
|
|
|
|
if (jointFeedbackInWorldSpace)
|
|
{
|
|
if (isConstraintPass)
|
|
{
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
|
|
}
|
|
else
|
|
{
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (isConstraintPass)
|
|
{
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
|
|
}
|
|
else
|
|
{
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// transform base accelerations back to the world frame.
|
|
const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
|
|
output[0] = omegadot_out[0];
|
|
output[1] = omegadot_out[1];
|
|
output[2] = omegadot_out[2];
|
|
|
|
const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
|
|
output[3] = vdot_out[0];
|
|
output[4] = vdot_out[1];
|
|
output[5] = vdot_out[2];
|
|
|
|
/////////////////
|
|
//printf("q = [");
|
|
//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
|
|
//for(int link = 0; link < getNumLinks(); ++link)
|
|
// for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
|
|
// printf("%.6f ", m_links[link].m_jointPos[dof]);
|
|
//printf("]\n");
|
|
////
|
|
//printf("qd = [");
|
|
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
|
// printf("%.6f ", m_realBuf[dof]);
|
|
//printf("]\n");
|
|
//printf("qdd = [");
|
|
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
|
// printf("%.6f ", output[dof]);
|
|
//printf("]\n");
|
|
/////////////////
|
|
|
|
// Final step: add the accelerations (times dt) to the velocities.
|
|
|
|
if (!isConstraintPass)
|
|
{
|
|
if (dt > 0.)
|
|
applyDeltaVeeMultiDof(output, dt);
|
|
}
|
|
/////
|
|
//btScalar angularThres = 1;
|
|
//btScalar maxAngVel = 0.;
|
|
//bool scaleDown = 1.;
|
|
//for(int link = 0; link < m_links.size(); ++link)
|
|
//{
|
|
// if(spatVel[link+1].getAngular().length() > maxAngVel)
|
|
// {
|
|
// maxAngVel = spatVel[link+1].getAngular().length();
|
|
// scaleDown = angularThres / spatVel[link+1].getAngular().length();
|
|
// break;
|
|
// }
|
|
//}
|
|
|
|
//if(scaleDown != 1.)
|
|
//{
|
|
// for(int link = 0; link < m_links.size(); ++link)
|
|
// {
|
|
// if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
|
|
// {
|
|
// for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
|
|
// getJointVelMultiDof(link)[dof] *= scaleDown;
|
|
// }
|
|
// }
|
|
//}
|
|
/////
|
|
|
|
/////////////////////
|
|
if (m_useGlobalVelocities)
|
|
{
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
const int parent = m_links[i].m_parent;
|
|
//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
|
|
//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
|
|
|
|
fromParent.m_rotMat = rot_from_parent[i + 1];
|
|
fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
|
fromWorld.m_rotMat = rot_from_world[i + 1];
|
|
|
|
// vhat_i = i_xhat_p(i) * vhat_p(i)
|
|
fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
|
|
//nice alternative below (using operator *) but it generates temps
|
|
/////////////////////////////////////////////////////////////
|
|
|
|
// now set vhat_i to its true value by doing
|
|
// vhat_i += qidot * shat_i
|
|
spatJointVel.setZero();
|
|
|
|
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
|
|
|
|
// remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
|
|
spatVel[i + 1] += spatJointVel;
|
|
|
|
fromWorld.transformInverseRotationOnly(spatVel[i + 1], m_links[i].m_absFrameTotVelocity);
|
|
fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
|
|
}
|
|
}
|
|
}
|
|
|
|
void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
|
|
{
|
|
int num_links = getNumLinks();
|
|
///solve I * x = rhs, so the result = invI * rhs
|
|
if (num_links == 0)
|
|
{
|
|
// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
|
|
|
|
if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
|
|
{
|
|
result[0] = rhs_bot[0] / m_baseInertia[0];
|
|
result[1] = rhs_bot[1] / m_baseInertia[1];
|
|
result[2] = rhs_bot[2] / m_baseInertia[2];
|
|
}
|
|
else
|
|
{
|
|
result[0] = 0;
|
|
result[1] = 0;
|
|
result[2] = 0;
|
|
}
|
|
if (m_baseMass >= SIMD_EPSILON)
|
|
{
|
|
result[3] = rhs_top[0] / m_baseMass;
|
|
result[4] = rhs_top[1] / m_baseMass;
|
|
result[5] = rhs_top[2] / m_baseMass;
|
|
}
|
|
else
|
|
{
|
|
result[3] = 0;
|
|
result[4] = 0;
|
|
result[5] = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (!m_cachedInertiaValid)
|
|
{
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
result[i] = 0.f;
|
|
}
|
|
return;
|
|
}
|
|
/// Special routine for calculating the inverse of a spatial inertia matrix
|
|
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
|
|
btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
|
|
btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
|
|
btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
|
|
tmp = invIupper_right * m_cachedInertiaLowerRight;
|
|
btMatrix3x3 invI_upper_left = (tmp * Binv);
|
|
btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
|
|
tmp = m_cachedInertiaTopLeft * invI_upper_left;
|
|
tmp[0][0] -= 1.0;
|
|
tmp[1][1] -= 1.0;
|
|
tmp[2][2] -= 1.0;
|
|
btMatrix3x3 invI_lower_left = (Binv * tmp);
|
|
|
|
//multiply result = invI * rhs
|
|
{
|
|
btVector3 vtop = invI_upper_left * rhs_top;
|
|
btVector3 tmp;
|
|
tmp = invIupper_right * rhs_bot;
|
|
vtop += tmp;
|
|
btVector3 vbot = invI_lower_left * rhs_top;
|
|
tmp = invI_lower_right * rhs_bot;
|
|
vbot += tmp;
|
|
result[0] = vtop[0];
|
|
result[1] = vtop[1];
|
|
result[2] = vtop[2];
|
|
result[3] = vbot[0];
|
|
result[4] = vbot[1];
|
|
result[5] = vbot[2];
|
|
}
|
|
}
|
|
}
|
|
void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
|
|
{
|
|
int num_links = getNumLinks();
|
|
///solve I * x = rhs, so the result = invI * rhs
|
|
if (num_links == 0)
|
|
{
|
|
// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
|
|
if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
|
|
{
|
|
result.setAngular(rhs.getAngular() / m_baseInertia);
|
|
}
|
|
else
|
|
{
|
|
result.setAngular(btVector3(0, 0, 0));
|
|
}
|
|
if (m_baseMass >= SIMD_EPSILON)
|
|
{
|
|
result.setLinear(rhs.getLinear() / m_baseMass);
|
|
}
|
|
else
|
|
{
|
|
result.setLinear(btVector3(0, 0, 0));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/// Special routine for calculating the inverse of a spatial inertia matrix
|
|
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
|
|
if (!m_cachedInertiaValid)
|
|
{
|
|
result.setLinear(btVector3(0, 0, 0));
|
|
result.setAngular(btVector3(0, 0, 0));
|
|
result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0));
|
|
return;
|
|
}
|
|
btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
|
|
btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
|
|
btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
|
|
tmp = invIupper_right * m_cachedInertiaLowerRight;
|
|
btMatrix3x3 invI_upper_left = (tmp * Binv);
|
|
btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
|
|
tmp = m_cachedInertiaTopLeft * invI_upper_left;
|
|
tmp[0][0] -= 1.0;
|
|
tmp[1][1] -= 1.0;
|
|
tmp[2][2] -= 1.0;
|
|
btMatrix3x3 invI_lower_left = (Binv * tmp);
|
|
|
|
//multiply result = invI * rhs
|
|
{
|
|
btVector3 vtop = invI_upper_left * rhs.getLinear();
|
|
btVector3 tmp;
|
|
tmp = invIupper_right * rhs.getAngular();
|
|
vtop += tmp;
|
|
btVector3 vbot = invI_lower_left * rhs.getLinear();
|
|
tmp = invI_lower_right * rhs.getAngular();
|
|
vbot += tmp;
|
|
result.setVector(vtop, vbot);
|
|
}
|
|
}
|
|
}
|
|
|
|
void btMultiBody::mulMatrix(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::predictPositionsMultiDof(btScalar dt)
|
|
{
|
|
int num_links = getNumLinks();
|
|
// step position by adding dt * velocity
|
|
//btVector3 v = getBaseVel();
|
|
//m_basePos += dt * v;
|
|
//
|
|
btScalar *pBasePos;
|
|
btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
|
|
|
|
// reset to current position
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
m_basePos_interpolate[i] = m_basePos[i];
|
|
}
|
|
pBasePos = m_basePos_interpolate;
|
|
|
|
pBasePos[0] += dt * pBaseVel[0];
|
|
pBasePos[1] += dt * pBaseVel[1];
|
|
pBasePos[2] += dt * pBaseVel[2];
|
|
|
|
///////////////////////////////
|
|
//local functor for quaternion integration (to avoid error prone redundancy)
|
|
struct
|
|
{
|
|
//"exponential map" based on btTransformUtil::integrateTransform(..)
|
|
void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
|
|
{
|
|
//baseBody => quat is alias and omega is global coor
|
|
//!baseBody => quat is alibi and omega is local coor
|
|
|
|
btVector3 axis;
|
|
btVector3 angvel;
|
|
|
|
if (!baseBody)
|
|
angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
|
|
else
|
|
angvel = omega;
|
|
|
|
btScalar fAngle = angvel.length();
|
|
//limit the angular motion
|
|
if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
|
|
{
|
|
fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
|
|
}
|
|
|
|
if (fAngle < btScalar(0.001))
|
|
{
|
|
// use Taylor's expansions of sync function
|
|
axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
|
|
}
|
|
else
|
|
{
|
|
// sync(fAngle) = sin(c*fAngle)/t
|
|
axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
|
|
}
|
|
|
|
if (!baseBody)
|
|
quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
|
|
else
|
|
quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
|
|
//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
|
|
|
|
quat.normalize();
|
|
}
|
|
} pQuatUpdateFun;
|
|
///////////////////////////////
|
|
|
|
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
|
|
//
|
|
btScalar *pBaseQuat;
|
|
|
|
// reset to current orientation
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
m_baseQuat_interpolate[i] = m_baseQuat[i];
|
|
}
|
|
pBaseQuat = m_baseQuat_interpolate;
|
|
|
|
btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
|
|
//
|
|
btQuaternion baseQuat;
|
|
baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
|
|
btVector3 baseOmega;
|
|
baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
|
|
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
|
|
pBaseQuat[0] = baseQuat.x();
|
|
pBaseQuat[1] = baseQuat.y();
|
|
pBaseQuat[2] = baseQuat.z();
|
|
pBaseQuat[3] = baseQuat.w();
|
|
|
|
// Finally we can update m_jointPos for each of the m_links
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
btScalar *pJointPos;
|
|
pJointPos = &m_links[i].m_jointPos_interpolate[0];
|
|
|
|
btScalar *pJointVel = getJointVelMultiDof(i);
|
|
|
|
switch (m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::ePrismatic:
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
//reset to current pos
|
|
pJointPos[0] = m_links[i].m_jointPos[0];
|
|
btScalar jointVel = pJointVel[0];
|
|
pJointPos[0] += dt * jointVel;
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
{
|
|
//reset to current pos
|
|
|
|
for (int j = 0; j < 4; ++j)
|
|
{
|
|
pJointPos[j] = m_links[i].m_jointPos[j];
|
|
}
|
|
|
|
btVector3 jointVel;
|
|
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
|
btQuaternion jointOri;
|
|
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
|
|
pQuatUpdateFun(jointVel, jointOri, false, dt);
|
|
pJointPos[0] = jointOri.x();
|
|
pJointPos[1] = jointOri.y();
|
|
pJointPos[2] = jointOri.z();
|
|
pJointPos[3] = jointOri.w();
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
for (int j = 0; j < 3; ++j)
|
|
{
|
|
pJointPos[j] = m_links[i].m_jointPos[j];
|
|
}
|
|
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
|
|
|
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
|
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
|
|
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
|
|
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
|
|
m_links[i].updateInterpolationCacheMultiDof();
|
|
}
|
|
}
|
|
|
|
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
|
|
{
|
|
int num_links = getNumLinks();
|
|
// step position by adding dt * velocity
|
|
//btVector3 v = getBaseVel();
|
|
//m_basePos += dt * v;
|
|
//
|
|
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;
|
|
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
|
|
|
|
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
|
|
|
|
switch (m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::ePrismatic:
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
//reset to current pos
|
|
btScalar jointVel = pJointVel[0];
|
|
pJointPos[0] += dt * jointVel;
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
{
|
|
//reset to current pos
|
|
btVector3 jointVel;
|
|
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
|
btQuaternion jointOri;
|
|
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
|
|
pQuatUpdateFun(jointVel, jointOri, false, dt);
|
|
pJointPos[0] = jointOri.x();
|
|
pJointPos[1] = jointOri.y();
|
|
pJointPos[2] = jointOri.z();
|
|
pJointPos[3] = jointOri.w();
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
|
|
|
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
|
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
|
|
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
|
|
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
|
|
m_links[i].updateCacheMultiDof(pq);
|
|
|
|
if (pq)
|
|
pq += m_links[i].m_posVarCount;
|
|
if (pqd)
|
|
pqd += m_links[i].m_dofCount;
|
|
}
|
|
}
|
|
|
|
void btMultiBody::fillConstraintJacobianMultiDof(int link,
|
|
const btVector3 &contact_point,
|
|
const btVector3 &normal_ang,
|
|
const btVector3 &normal_lin,
|
|
btScalar *jac,
|
|
btAlignedObjectArray<btScalar> &scratch_r1,
|
|
btAlignedObjectArray<btVector3> &scratch_v,
|
|
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
|
|
{
|
|
// temporary space
|
|
int num_links = getNumLinks();
|
|
int m_dofCount = getNumDofs();
|
|
scratch_v.resize(3 * num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
|
|
scratch_m.resize(num_links + 1);
|
|
|
|
btVector3 *v_ptr = &scratch_v[0];
|
|
btVector3 *p_minus_com_local = v_ptr;
|
|
v_ptr += num_links + 1;
|
|
btVector3 *n_local_lin = v_ptr;
|
|
v_ptr += num_links + 1;
|
|
btVector3 *n_local_ang = v_ptr;
|
|
v_ptr += num_links + 1;
|
|
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
|
|
|
|
//scratch_r.resize(m_dofCount);
|
|
//btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;
|
|
|
|
scratch_r1.resize(m_dofCount+num_links);
|
|
btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
|
|
btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
|
|
int numLinksChildToRoot=0;
|
|
int l = link;
|
|
while (l != -1)
|
|
{
|
|
links[numLinksChildToRoot++]=l;
|
|
l = m_links[l].m_parent;
|
|
}
|
|
|
|
btMatrix3x3 *rot_from_world = &scratch_m[0];
|
|
|
|
const btVector3 p_minus_com_world = contact_point - m_basePos;
|
|
const btVector3 &normal_lin_world = normal_lin; //convenience
|
|
const btVector3 &normal_ang_world = normal_ang;
|
|
|
|
rot_from_world[0] = btMatrix3x3(m_baseQuat);
|
|
|
|
// omega coeffients first.
|
|
btVector3 omega_coeffs_world;
|
|
omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
|
|
jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
|
|
jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
|
|
jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
|
|
// then v coefficients
|
|
jac[3] = normal_lin_world[0];
|
|
jac[4] = normal_lin_world[1];
|
|
jac[5] = normal_lin_world[2];
|
|
|
|
//create link-local versions of p_minus_com and normal
|
|
p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
|
|
n_local_lin[0] = rot_from_world[0] * normal_lin_world;
|
|
n_local_ang[0] = rot_from_world[0] * normal_ang_world;
|
|
|
|
// Set remaining jac values to zero for now.
|
|
for (int i = 6; i < 6 + m_dofCount; ++i)
|
|
{
|
|
jac[i] = 0;
|
|
}
|
|
|
|
// Qdot coefficients, if necessary.
|
|
if (num_links > 0 && link > -1)
|
|
{
|
|
// TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
|
|
// which is resulting in repeated work being done...)
|
|
|
|
// calculate required normals & positions in the local frames.
|
|
for (int a = 0; a < numLinksChildToRoot; a++)
|
|
{
|
|
int i = links[numLinksChildToRoot-1-a];
|
|
// transform to local frame
|
|
const int parent = m_links[i].m_parent;
|
|
const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
|
|
rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
|
|
|
|
n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
|
|
n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
|
|
p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector;
|
|
|
|
// calculate the jacobian entry
|
|
switch (m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
|
|
results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePrismatic:
|
|
{
|
|
results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0));
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
{
|
|
results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
|
|
results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1));
|
|
results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2));
|
|
|
|
results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
|
|
results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));
|
|
results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));
|
|
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1])); // + m_links[i].getAxisBottom(0));
|
|
results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1));
|
|
results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2));
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
}
|
|
|
|
// Now copy through to output.
|
|
//printf("jac[%d] = ", link);
|
|
while (link != -1)
|
|
{
|
|
for (int dof = 0; dof < m_links[link].m_dofCount; ++dof)
|
|
{
|
|
jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
|
|
//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
|
|
}
|
|
|
|
link = m_links[link].m_parent;
|
|
}
|
|
//printf("]\n");
|
|
}
|
|
}
|
|
|
|
void btMultiBody::wakeUp()
|
|
{
|
|
m_sleepTimer = 0;
|
|
m_awake = true;
|
|
}
|
|
|
|
void btMultiBody::goToSleep()
|
|
{
|
|
m_awake = false;
|
|
}
|
|
|
|
void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
|
|
{
|
|
extern bool gDisableDeactivation;
|
|
if (!m_canSleep || gDisableDeactivation)
|
|
{
|
|
m_awake = true;
|
|
m_sleepTimer = 0;
|
|
return;
|
|
}
|
|
|
|
|
|
|
|
// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
|
|
btScalar motion = 0;
|
|
{
|
|
for (int i = 0; i < 6 + m_dofCount; ++i)
|
|
motion += m_realBuf[i] * m_realBuf[i];
|
|
}
|
|
|
|
if (motion < SLEEP_EPSILON)
|
|
{
|
|
m_sleepTimer += timestep;
|
|
if (m_sleepTimer > SLEEP_TIMEOUT)
|
|
{
|
|
goToSleep();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m_sleepTimer = 0;
|
|
if (m_canWakeup)
|
|
{
|
|
if (!m_awake)
|
|
wakeUp();
|
|
}
|
|
}
|
|
}
|
|
|
|
void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
|
|
{
|
|
int num_links = getNumLinks();
|
|
|
|
// Cached 3x3 rotation matrices from parent frame to this frame.
|
|
btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0];
|
|
|
|
rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
|
|
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
|
|
}
|
|
|
|
int nLinks = getNumLinks();
|
|
///base + num m_links
|
|
world_to_local.resize(nLinks + 1);
|
|
local_origin.resize(nLinks + 1);
|
|
|
|
world_to_local[0] = getWorldToBaseRot();
|
|
local_origin[0] = getBasePos();
|
|
|
|
for (int k = 0; k < getNumLinks(); k++)
|
|
{
|
|
const int parent = getParent(k);
|
|
world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
|
|
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
|
|
}
|
|
|
|
for (int link = 0; link < getNumLinks(); link++)
|
|
{
|
|
int index = link + 1;
|
|
|
|
btVector3 posr = local_origin[index];
|
|
btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
|
|
btTransform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(posr);
|
|
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
|
|
getLink(link).m_cachedWorldTransform = tr;
|
|
}
|
|
}
|
|
|
|
void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
|
|
{
|
|
world_to_local.resize(getNumLinks() + 1);
|
|
local_origin.resize(getNumLinks() + 1);
|
|
|
|
world_to_local[0] = getWorldToBaseRot();
|
|
local_origin[0] = getBasePos();
|
|
|
|
if (getBaseCollider())
|
|
{
|
|
btVector3 posr = local_origin[0];
|
|
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
|
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
|
|
btTransform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(posr);
|
|
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
|
|
|
|
getBaseCollider()->setWorldTransform(tr);
|
|
getBaseCollider()->setInterpolationWorldTransform(tr);
|
|
}
|
|
|
|
for (int k = 0; k < getNumLinks(); k++)
|
|
{
|
|
const int parent = getParent(k);
|
|
world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
|
|
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
|
|
}
|
|
|
|
for (int m = 0; m < getNumLinks(); m++)
|
|
{
|
|
btMultiBodyLinkCollider *col = getLink(m).m_collider;
|
|
if (col)
|
|
{
|
|
int link = col->m_link;
|
|
btAssert(link == m);
|
|
|
|
int index = link + 1;
|
|
|
|
btVector3 posr = local_origin[index];
|
|
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
|
btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
|
|
btTransform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(posr);
|
|
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
|
|
|
|
col->setWorldTransform(tr);
|
|
col->setInterpolationWorldTransform(tr);
|
|
}
|
|
}
|
|
}
|
|
|
|
void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
|
|
{
|
|
world_to_local.resize(getNumLinks() + 1);
|
|
local_origin.resize(getNumLinks() + 1);
|
|
|
|
world_to_local[0] = getInterpolateWorldToBaseRot();
|
|
local_origin[0] = getInterpolateBasePos();
|
|
|
|
if (getBaseCollider())
|
|
{
|
|
btVector3 posr = local_origin[0];
|
|
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
|
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
|
|
btTransform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(posr);
|
|
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
|
|
|
|
getBaseCollider()->setInterpolationWorldTransform(tr);
|
|
}
|
|
|
|
for (int k = 0; k < getNumLinks(); k++)
|
|
{
|
|
const int parent = getParent(k);
|
|
world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
|
|
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
|
|
}
|
|
|
|
for (int m = 0; m < getNumLinks(); m++)
|
|
{
|
|
btMultiBodyLinkCollider *col = getLink(m).m_collider;
|
|
if (col)
|
|
{
|
|
int link = col->m_link;
|
|
btAssert(link == m);
|
|
|
|
int index = link + 1;
|
|
|
|
btVector3 posr = local_origin[index];
|
|
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
|
btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
|
|
btTransform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(posr);
|
|
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
|
|
|
|
col->setInterpolationWorldTransform(tr);
|
|
}
|
|
}
|
|
}
|
|
|
|
int btMultiBody::calculateSerializeBufferSize() const
|
|
{
|
|
int sz = sizeof(btMultiBodyData);
|
|
return sz;
|
|
}
|
|
|
|
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
|
const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const
|
|
{
|
|
btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer;
|
|
getBasePos().serialize(mbd->m_baseWorldPosition);
|
|
getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
|
|
getBaseVel().serialize(mbd->m_baseLinearVelocity);
|
|
getBaseOmega().serialize(mbd->m_baseAngularVelocity);
|
|
|
|
mbd->m_baseMass = this->getBaseMass();
|
|
getBaseInertia().serialize(mbd->m_baseInertia);
|
|
{
|
|
char *name = (char *)serializer->findNameForPointer(m_baseName);
|
|
mbd->m_baseName = (char *)serializer->getUniquePointer(name);
|
|
if (mbd->m_baseName)
|
|
{
|
|
serializer->serializeName(name);
|
|
}
|
|
}
|
|
mbd->m_numLinks = this->getNumLinks();
|
|
if (mbd->m_numLinks)
|
|
{
|
|
int sz = sizeof(btMultiBodyLinkData);
|
|
int numElem = mbd->m_numLinks;
|
|
btChunk *chunk = serializer->allocate(sz, numElem);
|
|
btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
|
|
for (int i = 0; i < numElem; i++, memPtr++)
|
|
{
|
|
memPtr->m_jointType = getLink(i).m_jointType;
|
|
memPtr->m_dofCount = getLink(i).m_dofCount;
|
|
memPtr->m_posVarCount = getLink(i).m_posVarCount;
|
|
|
|
getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
|
|
|
|
getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
|
|
getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
|
|
getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
|
|
getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
|
|
|
|
memPtr->m_linkMass = getLink(i).m_mass;
|
|
memPtr->m_parentIndex = getLink(i).m_parent;
|
|
memPtr->m_jointDamping = getLink(i).m_jointDamping;
|
|
memPtr->m_jointFriction = getLink(i).m_jointFriction;
|
|
memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
|
|
memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
|
|
memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
|
|
memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
|
|
|
|
getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
|
|
getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
|
|
getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
|
|
btAssert(memPtr->m_dofCount <= 3);
|
|
for (int dof = 0; dof < getLink(i).m_dofCount; dof++)
|
|
{
|
|
getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
|
|
getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
|
|
|
|
memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
|
|
memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
|
|
}
|
|
int numPosVar = getLink(i).m_posVarCount;
|
|
for (int posvar = 0; posvar < numPosVar; posvar++)
|
|
{
|
|
memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
|
|
}
|
|
|
|
{
|
|
char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName);
|
|
memPtr->m_linkName = (char *)serializer->getUniquePointer(name);
|
|
if (memPtr->m_linkName)
|
|
{
|
|
serializer->serializeName(name);
|
|
}
|
|
}
|
|
{
|
|
char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName);
|
|
memPtr->m_jointName = (char *)serializer->getUniquePointer(name);
|
|
if (memPtr->m_jointName)
|
|
{
|
|
serializer->serializeName(name);
|
|
}
|
|
}
|
|
memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider);
|
|
}
|
|
serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]);
|
|
}
|
|
mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0;
|
|
|
|
// Fill padding with zeros to appease msan.
|
|
#ifdef BT_USE_DOUBLE_PRECISION
|
|
memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
|
|
#endif
|
|
|
|
return btMultiBodyDataName;
|
|
}
|