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