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; }