void BodyNode::aggregateInvAugMassMatrix(Eigen::MatrixXd* _InvMCol, int _col,
double /*_timeStep*/) {
Eigen::VectorXd MInvCol;
int dof = mParentJoint->getNumGenCoords();
if (dof > 0) {
if (mParentBodyNode) {
MInvCol.noalias() = mImplicitPsi * mInvM_a;
MInvCol.noalias() -= mImplicitAI_S_ImplicitPsi.transpose()
* math::AdInvT(mParentJoint->getLocalTransform(),
mParentBodyNode->mInvM_U);
} else {
MInvCol.noalias() = mImplicitPsi * mInvM_a;
}
assert(!math::isNan(MInvCol));
// Assign
int iStart = mParentJoint->getGenCoord(0)->getSkeletonIndex();
_InvMCol->block(iStart, _col, dof, 1) = MInvCol;
}
if (mChildBodyNodes.size() > 0) {
if (dof > 0)
mInvM_U.noalias() = mParentJoint->getLocalJacobian() * MInvCol;
else
mInvM_U.setZero();
if (mParentBodyNode) {
mInvM_U += math::AdInvT(mParentJoint->getLocalTransform(),
mParentBodyNode->mInvM_U);
}
assert(!math::isNan(mInvM_U));
}
}
bool operator() (
Eigen::VectorXd& newx,
const Eigen::VectorXd& x,
const Eigen::VectorXd& g,
double step_size) {
newx.noalias() = x - step_size * g;
for (ssize_t n = 0; n < newx.size(); n += 3)
newx.segment (n,3).normalize();
return newx != x;
}
void BodyNode::update_ddq() {
if (mParentJoint->getNumGenCoords() == 0)
return;
Eigen::VectorXd ddq;
if (mParentBodyNode) {
ddq.noalias() =
mImplicitPsi * (mAlpha - mImplicitAI_S.transpose() *
math::AdInvT(mParentJoint->getLocalTransform(),
mParentBodyNode->getBodyAcceleration()));
} else {
ddq.noalias() = mImplicitPsi * mAlpha;
}
mParentJoint->GenCoordSystem::setGenAccs(ddq);
assert(!math::isNan(ddq));
updateAcceleration();
}
void operator() (Image<value_type>& fod_img, Image<value_type>& dec_img) {
if (mask_img.valid()) {
assign_pos_of(fod_img, 0, 3).to(mask_img);
if (!mask_img.value()) {
dec_img.row(3).fill(UNIT);
return;
}
}
amps.noalias() = dectrans.sht * fod_img.row(3).cast<double>();
dec.setZero();
ampsum = 0.0;
for (ssize_t i = 0; i < amps.rows(); i++) {
if (!std::isnan(dectrans.thresh) && amps(i) < dectrans.thresh)
continue;
dec += dectrans.decs.row(i) * amps(i);
ampsum += amps(i);
}
dec = dec.cwiseMax(0.0);
ampsum = std::max(ampsum, 0.0);
decnorm = dec.norm();
if (decnorm == 0.0)
dec_img.row(3).fill(UNIT);
else
dec_img.row(3) = (dec / decnorm).cast<value_type>();
if (int_img.valid()) {
assign_pos_of(fod_img, 0, 3).to(int_img);
int_img.value() = (ampsum / amps.rows()) * 4.0 * Math::pi;
}
}
void interpolateToNodes(Eigen::VectorXd const& src, Eigen::VectorXd &dst) {
if(!is_constructed) {
construct();
}
dst.noalias() = interpolation_matrix*src;
}
