void PointMass::aggregateMassMatrix(Eigen::MatrixXd* _MCol, int _col)
{
// Assign
// We assume that the three generalized coordinates are in a row.
int iStart = getGenCoord(0)->getSkeletonIndex();
mM_F.noalias() = mMass * mM_dV;
_MCol->block<3, 1>(iStart, _col).noalias() = mM_F;
}
void PointMass::aggregateInvMassMatrix(Eigen::MatrixXd* _MInvCol, int _col)
{
// Assign
// We assume that the three generalized coordinates are in a row.
int iStart = getGenCoord(0)->getSkeletonIndex();
_MInvCol->block<3, 1>(iStart, _col)
= mPsi * getGenForces()
- mParentSoftBodyNode->mInvM_U.head<3>().cross(mX)
- mParentSoftBodyNode->mInvM_U.tail<3>();
}
Eigen::VectorXd Joint::getDampingForces() const
{
int numDofs = getNumGenCoords();
Eigen::VectorXd dampingForce(numDofs);
for (int i = 0; i < numDofs; ++i)
dampingForce(i) = -mDampingCoefficient[i] * getGenCoord(i)->get_dq();
return dampingForce;
}
void PointMass::aggregateGravityForceVector(Eigen::VectorXd* _g,
const Eigen::Vector3d& _gravity)
{
mG_F = mMass * (mParentSoftBodyNode->getWorldTransform().linear().transpose()
* _gravity);
// Assign
// We assume that the three generalized coordinates are in a row.
int iStart = getGenCoord(0)->getSkeletonIndex();
_g->segment<3>(iStart) = mG_F;
}
void PointMass::aggregateAugMassMatrix(Eigen::MatrixXd* _MCol, int _col,
double _timeStep)
{
// Assign
// We assume that the three generalized coordinates are in a row.
int iStart = getGenCoord(0)->getSkeletonIndex();
mM_F.noalias() = mMass * mM_dV;
double d = mParentSoftBodyNode->getDampingCoefficient();
double kv = mParentSoftBodyNode->getVertexSpringStiffness();
_MCol->block<3, 1>(iStart, _col).noalias()
= mM_F + (_timeStep * _timeStep * kv + _timeStep * d) * getGenAccs();
}
void PointMass::aggregateCombinedVector(Eigen::VectorXd* _Cg,
const Eigen::Vector3d& _gravity)
{
mCg_F.noalias() = mMass * mCg_dV;
mCg_F -= mMass
* (mParentSoftBodyNode->getWorldTransform().linear().transpose()
* _gravity);
mCg_F += mParentSoftBodyNode->getBodyVelocity().head<3>().cross(mMass * mV);
// Assign
// We assume that the three generalized coordinates are in a row.
int iStart = getGenCoord(0)->getSkeletonIndex();
_Cg->segment<3>(iStart) = mCg_F;
}
void PointMass::aggregateExternalForces(Eigen::VectorXd* _Fext)
{
int iStart = getGenCoord(0)->getSkeletonIndex();
_Fext->segment<3>(iStart) = mFext;
}
