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::BuildBiVector(
ChMatrix<>& Bvector ///< matrix which will contain the entire vector of 'b'
)
{
n_c=CountActiveConstraints();
Bvector.Resize(n_c, 1);
// Fill the 'b' vector
#pragma omp parallel for num_threads(this->num_threads)
for (int ic = 0; ic< (int)vconstraints.size(); ic++)
{
if (vconstraints[ic]->IsActive())
{
Bvector(vconstraints[ic]->GetOffset()) = vconstraints[ic]->Get_b_i();
}
}
return n_c;
}
int ChLcpSystemDescriptor::FromConstraintsToVector(
ChMatrix<>& mvector,
bool resize_vector
)
{
// Count active constraints and resize vector if necessary
if (resize_vector)
{
n_c=CountActiveConstraints();
mvector.Resize(n_c, 1);
}
// 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())
{
mvector(vconstraints[ic]->GetOffset()) = vconstraints[ic]->Get_l_i();
}
}
return n_c;
}
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;
}