예제 #1
0
// 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);
}
예제 #4
0
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;
}
예제 #5
0
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;
	}

}
예제 #6
0
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;
}
예제 #7
0
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;
}
예제 #8
0
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;
}
예제 #9
0
// 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;
}
예제 #10
0
// 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);
}
예제 #11
0
// 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;
}