// Computes the product of the corresponding block in the
// system matrix (ie. the mass matrix) by 'vect', scale by c_a, and add to 'result'.
// NOTE: the 'vect' and 'result' vectors must already have
// the size of the total variables&constraints in the system; the procedure
// will use the ChVariable offsets (that must be already updated) to know the
// indexes in result and vect.
void ChVariablesNode::MultiplyAndAdd(ChMatrix<double>& result, const ChMatrix<double>& vect, const double c_a) const {
assert(result.GetColumns() == 1 && vect.GetColumns() == 1);
// optimized unrolled operations
double scaledmass = c_a * mass;
result(this->offset) += scaledmass * vect(this->offset);
result(this->offset + 1) += scaledmass * vect(this->offset + 1);
result(this->offset + 2) += scaledmass * vect(this->offset + 2);
}
// Add the diagonal of the mass matrix scaled by c_a, to 'result'.
// NOTE: the 'result' vector must already have the size of system unknowns, ie
// the size of the total variables&constraints in the system; the procedure
// will use the ChVariable offset (that must be already updated) as index.
void ChVariablesBodySharedMass::DiagonalAdd(ChMatrix<double>& result, const double c_a) const {
assert(result.GetColumns() == 1);
result(this->offset + 0) += c_a * sharedmass->mass;
result(this->offset + 1) += c_a * sharedmass->mass;
result(this->offset + 2) += c_a * sharedmass->mass;
result(this->offset + 3) += c_a * sharedmass->inertia(0, 0);
result(this->offset + 4) += c_a * sharedmass->inertia(1, 1);
result(this->offset + 5) += c_a * sharedmass->inertia(2, 2);
}
// Computes the product of the corresponding block in the
// system matrix (ie. the mass matrix) by 'vect', scale by c_a, and add to 'result'.
// NOTE: the 'vect' and 'result' vectors must already have
// the size of the total variables&constraints in the system; the procedure
// will use the ChVariable offsets (that must be already updated) to know the
// indexes in result and vect.
void ChVariablesBodySharedMass::MultiplyAndAdd(ChMatrix<double>& result,
const ChMatrix<double>& vect,
const double c_a) const {
assert(result.GetColumns() == 1 && vect.GetColumns() == 1);
// optimized unrolled operations
double q0 = vect(this->offset + 0);
double q1 = vect(this->offset + 1);
double q2 = vect(this->offset + 2);
double q3 = vect(this->offset + 3);
double q4 = vect(this->offset + 4);
double q5 = vect(this->offset + 5);
double scaledmass = c_a * sharedmass->mass;
result(this->offset + 0) += scaledmass * q0;
result(this->offset + 1) += scaledmass * q1;
result(this->offset + 2) += scaledmass * q2;
result(this->offset + 3) +=
c_a * (sharedmass->inertia(0, 0) * q3 + sharedmass->inertia(0, 1) * q4 + sharedmass->inertia(0, 2) * q5);
result(this->offset + 4) +=
c_a * (sharedmass->inertia(1, 0) * q3 + sharedmass->inertia(1, 1) * q4 + sharedmass->inertia(1, 2) * q5);
result(this->offset + 5) +=
c_a * (sharedmass->inertia(2, 0) * q3 + sharedmass->inertia(2, 1) * q4 + sharedmass->inertia(2, 2) * q5);
}
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;
}
void ChLcpKstiffnessGeneric::DiagonalAdd(ChMatrix<double>& result)
{
assert(result.GetColumns()==1);
int kio =0;
for (unsigned int iv = 0; iv< this->GetNvars(); iv++)
{
int io = this->GetVariableN(iv)->GetOffset();
int in = this->GetVariableN(iv)->Get_ndof();
for (int r = 0; r < in; r++)
{
//GetLog() << "Summing" << result(io+r) << " to " << (*this->K)(kio+r,kio+r) << "\n";
result(io+r) += (*this->K)(kio+r,kio+r);
}
kio += in;
}
}
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;
}
// Add the diagonal of the mass matrix scaled by c_a, to 'result'.
// NOTE: the 'result' vector must already have the size of system unknowns, ie
// the size of the total variables&constraints in the system; the procedure
// will use the ChVariable offset (that must be already updated) as index.
void ChVariablesShaft::DiagonalAdd(ChMatrix<double>& result, const double c_a) const {
assert(result.GetColumns() == 1);
result(this->offset) += c_a * m_inertia;
}
// Computes the product of the corresponding block in the
// system matrix (ie. the mass matrix) by 'vect', scale by c_a, and add to 'result'.
// NOTE: the 'vect' and 'result' vectors must already have
// the size of the total variables&constraints in the system; the procedure
// will use the ChVariable offsets (that must be already updated) to know the
// indexes in result and vect.
void ChVariablesShaft::MultiplyAndAdd(ChMatrix<double>& result, const ChMatrix<double>& vect, const double c_a) const {
assert(result.GetColumns() == 1 && vect.GetColumns() == 1);
result(this->offset) += c_a * m_inertia * vect(this->offset);
}
// Add the diagonal of the mass matrix scaled by c_a, to 'result'.
// NOTE: the 'result' vector must already have the size of system unknowns, ie
// the size of the total variables&constraints in the system; the procedure
// will use the ChVariable offset (that must be already updated) as index.
void ChVariablesNode::DiagonalAdd(ChMatrix<double>& result, const double c_a) const {
assert(result.GetColumns() == 1);
result(this->offset) += c_a * mass;
result(this->offset + 1) += c_a * mass;
result(this->offset + 2) += c_a * mass;
}