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