Exemplos de Map::asDiagonal em C++ (Cpp)

Exemplo n.º 1

Arquivo: RigidBody3DState.cpp Projeto: hmazhar/scisim

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

Exemplo n.º 2

Arquivo: GetIndCEScoresCPP.cpp Projeto: cran/fdapace

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