int ChLcpSystemDescriptor::FromUnknownsToVector(
ChMatrix<>& mvector,
bool resize_vector
)
{
// Count active variables & constraints and resize vector if necessary
n_q= CountActiveVariables();
n_c= CountActiveConstraints();
if (resize_vector)
{
mvector.Resize(n_q+n_c, 1);
}
// Fill the first part of vector, x.q ,with variables q
#pragma omp parallel for num_threads(this->num_threads)
for (int iv = 0; iv< (int)vvariables.size(); iv++)
{
if (vvariables[iv]->IsActive())
{
mvector.PasteMatrix(&vvariables[iv]->Get_qb(), vvariables[iv]->GetOffset(), 0);
}
}
// Fill the second part of vector, x.l, with constraint multipliers -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())
{
mvector(vconstraints[ic]->GetOffset() + n_q) = -vconstraints[ic]->Get_l_i();
}
}
return n_q+n_c;
}
int ChLcpSystemDescriptor::BuildDiVector(
ChMatrix<>& Dvector
)
{
n_q=CountActiveVariables();
n_c=CountActiveConstraints();
Dvector.Reset(n_q+n_c,1); // fast! Reset() method does not realloc if size doesn't change
// Fills the 'f' vector part
#pragma omp parallel for num_threads(this->num_threads)
for (int iv = 0; iv< (int)vvariables.size(); iv++)
{
if (vvariables[iv]->IsActive())
{
Dvector.PasteMatrix(&vvariables[iv]->Get_fb(), vvariables[iv]->GetOffset(), 0);
}
}
// Fill the '-b' vector (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())
{
Dvector(vconstraints[ic]->GetOffset() + n_q) = - vconstraints[ic]->Get_b_i();
}
}
return n_q+n_c;
}
int ChLcpSystemDescriptor::BuildFbVector(
ChMatrix<>& Fvector ///< matrix which will contain the entire vector of 'f'
)
{
n_q=CountActiveVariables();
Fvector.Reset(n_q,1); // fast! Reset() method does not realloc if size doesn't change
// Fills the 'f' vector
#pragma omp parallel for num_threads(this->num_threads)
for (int iv = 0; iv< (int)vvariables.size(); iv++)
{
if (vvariables[iv]->IsActive())
{
Fvector.PasteMatrix(&vvariables[iv]->Get_fb(), vvariables[iv]->GetOffset(), 0);
}
}
return this->n_q;
}
int ChLcpSystemDescriptor::FromVariablesToVector(
ChMatrix<>& mvector,
bool resize_vector
)
{
// Count active variables and resize vector if necessary
if (resize_vector)
{
n_q= CountActiveVariables();
mvector.Resize(n_q, 1);
}
// Fill the vector
// #pragma omp parallel for num_threads(this->num_threads)
for (int iv = 0; iv< (int)vvariables.size(); iv++)
{
if (vvariables[iv]->IsActive())
{
mvector.PasteMatrix(&vvariables[iv]->Get_qb(), vvariables[iv]->GetOffset(), 0);
}
}
return n_q;
}