void RigidBody3DState::updateMandMinv()
{
assert( unsigned( m_M0.nonZeros() ) == 6 * nbodies() );
assert( m_M0.nonZeros() == m_Minv0.nonZeros() );
assert( unsigned( m_M.nonZeros() ) == 12 * nbodies() );
assert( m_M.nonZeros() == m_Minv.nonZeros() );
for( unsigned bdy_idx = 0; bdy_idx < m_nbodies; ++bdy_idx )
{
// Orientation of the ith body
const Eigen::Map<const Matrix33sr> R{ m_q.segment<9>( 3 * m_nbodies + 9 * bdy_idx ).data() };
assert( fabs( ( R * R.transpose() - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() ) <= 1.0e-9 );
assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );
// Inertia tensor of the ith body
{
Eigen::Map<Matrix33sc> I{ &m_M.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
const Eigen::Map<const Vector3s> I0{ &m_M0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
I = R * I0.asDiagonal() * R.transpose();
assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
assert( I.determinant() > 0.0 );
}
// Inverse of the inertia tensor of the ith body
{
Eigen::Map<Matrix33sc> Iinv{ &m_Minv.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
const Eigen::Map<const Vector3s> Iinv0{ &m_Minv0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
Iinv = R * Iinv0.asDiagonal() * R.transpose();
assert( ( Iinv - Iinv.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
assert( Iinv.determinant() > 0.0 );
}
}
assert( MathUtilities::isIdentity( m_M * m_Minv, 1.0e-9 ) );
}
Rcpp::List GetIndCEScoresCPP( const Eigen::Map<Eigen::VectorXd> & yVec, const Eigen::Map<Eigen::VectorXd> & muVec, const Eigen::Map<Eigen::VectorXd> & lamVec, const Eigen::Map<Eigen::MatrixXd> & phiMat, const Eigen::Map<Eigen::MatrixXd> & SigmaYi){
// Setting up initial values
const unsigned int lenlamVec = lamVec.size();
Eigen::MatrixXd xiVar = Eigen::MatrixXd::Constant(lenlamVec,lenlamVec,std::numeric_limits<double>::quiet_NaN());
Eigen::MatrixXd xiEst = Eigen::MatrixXd::Constant(lenlamVec,1,std::numeric_limits<double>::quiet_NaN());
Eigen::MatrixXd fittedY = Eigen::MatrixXd::Constant(lenlamVec,1,std::numeric_limits<double>::quiet_NaN());
Eigen::MatrixXd LamPhi = lamVec.asDiagonal() * phiMat.transpose();
Eigen::LDLT<Eigen::MatrixXd> ldlt_SigmaYi(SigmaYi);
xiEst = LamPhi * ldlt_SigmaYi.solve(yVec - muVec) ; // LamPhiSig * (yVec - muVec);
xiVar = -LamPhi * ldlt_SigmaYi.solve(LamPhi.transpose()); // LamPhiSig.transpose();
xiVar.diagonal() += lamVec;
fittedY = muVec + phiMat * xiEst;
return Rcpp::List::create(Rcpp::Named("xiEst") = xiEst,
Rcpp::Named("xiVar") = xiVar,
Rcpp::Named("fittedY") = fittedY);
}