// Computes the product of the mass matrix by a
// vector, and set in result: result = [Mb]*vect
void ChVariablesNode::Compute_inc_Mb_v(ChMatrix<double>& result, const ChMatrix<double>& vect) const {
assert(result.GetRows() == vect.GetRows());
assert(vect.GetRows() == Get_ndof());
// optimized unrolled operations
result(0) += mass * vect(0);
result(1) += mass * vect(1);
result(2) += mass * vect(2);
}
ChMapMatrix::ChMapMatrix(const ChMatrix<>& mat) {
m_num_rows = mat.GetRows();
m_num_cols = mat.GetColumns();
m_rows.resize(mat.GetRows());
for (int ir = 0; ir < m_num_rows; ir++) {
for (int ic = 0; ic < m_num_cols; ic++) {
double val = mat.GetElement(ir, ic);
if (val != 0) {
ChMapMatrix::SetElement(ir, ic, val);
}
}
}
m_CSR_current = false;
}
// Computes the product of the mass matrix by a
// vector, and set in result: result = [Mb]*vect
void ChVariablesBodySharedMass::Compute_inc_Mb_v(ChMatrix<double>& result, const ChMatrix<double>& vect) const {
assert(result.GetRows() == Get_ndof());
assert(vect.GetRows() == Get_ndof());
// optimized unrolled operations
result(0) += sharedmass->mass * vect(0);
result(1) += sharedmass->mass * vect(1);
result(2) += sharedmass->mass * vect(2);
result(3) += (sharedmass->inertia(0, 0) * vect(3) + sharedmass->inertia(0, 1) * vect(4) +
sharedmass->inertia(0, 2) * vect(5));
result(4) += (sharedmass->inertia(1, 0) * vect(3) + sharedmass->inertia(1, 1) * vect(4) +
sharedmass->inertia(1, 2) * vect(5));
result(5) += (sharedmass->inertia(2, 0) * vect(3) + sharedmass->inertia(2, 1) * vect(4) +
sharedmass->inertia(2, 2) * vect(5));
}
int ChLcpSystemDescriptor::FromVectorToVariables(
ChMatrix<>& mvector
)
{
#ifdef CH_DEBUG
n_q= CountActiveVariables();
assert(n_q == mvector.GetRows());
assert(mvector.GetColumns()==1);
#endif
// fetch from the vector
#pragma omp parallel for num_threads(this->num_threads)
for (int iv = 0; iv< (int)vvariables.size(); iv++)
{
if (vvariables[iv]->IsActive())
{
vvariables[iv]->Get_qb().PasteClippedMatrix(&mvector, vvariables[iv]->GetOffset(), 0, vvariables[iv]->Get_ndof(),1, 0,0);
}
}
return n_q;
}
int ChLcpSystemDescriptor::FromVectorToUnknowns(
ChMatrix<>& mvector
)
{
n_q= CountActiveVariables();
n_c= CountActiveConstraints();
#ifdef CH_DEBUG
assert((n_q+n_c) == mvector.GetRows());
assert(mvector.GetColumns()==1);
#endif
// fetch from the first part of vector (x.q = q)
#pragma omp parallel for num_threads(this->num_threads)
for (int iv = 0; iv< (int)vvariables.size(); iv++)
{
//int rank = CHOMPfunctions::GetThreadNum();
//int count = CHOMPfunctions::GetNumThreads();
//GetLog() << " FromVectorToUnknowns: thread " << rank << " on " << count << "\n";
//GetLog().Flush();
if (vvariables[iv]->IsActive())
{
vvariables[iv]->Get_qb().PasteClippedMatrix(&mvector, vvariables[iv]->GetOffset(), 0, vvariables[iv]->Get_ndof(),1, 0,0);
}
}
// fetch from the second part of vector (x.l = -l), with flipped sign!
#pragma omp parallel for num_threads(this->num_threads)
for (int ic = 0; ic< (int)vconstraints.size(); ic++)
{
if (vconstraints[ic]->IsActive())
{
vconstraints[ic]->Set_l_i( - mvector( vconstraints[ic]->GetOffset() + n_q ));
}
}
return n_q+n_c;
}
int ChLcpSystemDescriptor::FromVectorToConstraints(
ChMatrix<>& mvector
)
{
n_c=CountActiveConstraints();
#ifdef CH_DEBUG
assert(n_c == mvector.GetRows());
assert(mvector.GetColumns()==1);
#endif
// Fill the vector
#pragma omp parallel for num_threads(this->num_threads)
for (int ic = 0; ic< (int)vconstraints.size(); ic++)
{
if (vconstraints[ic]->IsActive())
{
vconstraints[ic]->Set_l_i( mvector(vconstraints[ic]->GetOffset()) );
}
}
return n_c;
}
// Computes the product of the mass matrix by a
// vector, and set in result: result = [Mb]*vect
void ChVariablesShaft::Compute_inc_Mb_v(ChMatrix<double>& result, const ChMatrix<double>& vect) const {
assert(result.GetRows() == vect.GetRows());
assert(vect.GetRows() == Get_ndof());
result(0) += m_inertia * vect(0);
}