C++ ChMatrix类代码示例

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

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChDoubleWishboneReduced::LogConstraintViolations(VehicleSide side) {
    // Revolute joint
    {
        ChMatrix<>* C = m_revolute[side]->GetC();
        GetLog() << "Spindle revolute      ";
        GetLog() << "  " << C->GetElement(0, 0) << "  ";
        GetLog() << "  " << C->GetElement(1, 0) << "  ";
        GetLog() << "  " << C->GetElement(2, 0) << "  ";
        GetLog() << "  " << C->GetElement(3, 0) << "  ";
        GetLog() << "  " << C->GetElement(4, 0) << "\n";
    }

    // Distance constraints
    GetLog() << "UCA front distance    ";
    GetLog() << "  " << m_distUCA_F[side]->GetCurrentDistance() - m_distUCA_F[side]->GetImposedDistance() << "\n";

    GetLog() << "UCA back distance     ";
    GetLog() << "  " << m_distUCA_B[side]->GetCurrentDistance() - m_distUCA_B[side]->GetImposedDistance() << "\n";

    GetLog() << "LCA front distance    ";
    GetLog() << "  " << m_distLCA_F[side]->GetCurrentDistance() - m_distLCA_F[side]->GetImposedDistance() << "\n";

    GetLog() << "LCA back distance     ";
    GetLog() << "  " << m_distLCA_B[side]->GetCurrentDistance() - m_distLCA_B[side]->GetImposedDistance() << "\n";

    GetLog() << "Tierod distance       ";
    GetLog() << "  " << m_distTierod[side]->GetCurrentDistance() - m_distTierod[side]->GetImposedDistance() << "\n";
}

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChPitmanArm::LogConstraintViolations() {
    // Revolute joint
    ////{
    ////    ChMatrix<>* C = m_revolute->GetC();
    ////    GetLog() << "Revolute              ";
    ////    GetLog() << "  " << C->GetElement(0, 0) << "  ";
    ////    GetLog() << "  " << C->GetElement(1, 0) << "  ";
    ////    GetLog() << "  " << C->GetElement(2, 0) << "  ";
    ////    GetLog() << "  " << C->GetElement(3, 0) << "  ";
    ////    GetLog() << "  " << C->GetElement(4, 0) << "\n";
    ////}

    // Universal joint
    {
        ChMatrix<>* C = m_universal->GetC();
        GetLog() << "Universal             ";
        GetLog() << "  " << C->GetElement(0, 0) << "  ";
        GetLog() << "  " << C->GetElement(1, 0) << "  ";
        GetLog() << "  " << C->GetElement(2, 0) << "  ";
        GetLog() << "  " << C->GetElement(3, 0) << "\n";
    }

    // Revolute-spherical joint
    {
        ChMatrix<>* C = m_revsph->GetC();
        GetLog() << "Revolute-spherical    ";
        GetLog() << "  " << C->GetElement(0, 0) << "  ";
        GetLog() << "  " << C->GetElement(1, 0) << "\n";
    }
}

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

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

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChRoadWheel::LogConstraintViolations() {
    ChMatrix<>* C = m_revolute->GetC();
    GetLog() << "  Road-wheel revolute\n";
    GetLog() << "  " << C->GetElement(0, 0) << "  ";
    GetLog() << "  " << C->GetElement(1, 0) << "  ";
    GetLog() << "  " << C->GetElement(2, 0) << "  ";
    GetLog() << "  " << C->GetElement(3, 0) << "  ";
    GetLog() << "  " << C->GetElement(4, 0) << "\n";
}

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChLinearDamperRWAssembly::LogConstraintViolations() {
    ChMatrix<>* C = m_revolute->GetC();
    GetLog() << "  Arm-chassis revolute\n";
    GetLog() << "  " << C->GetElement(0, 0) << "  ";
    GetLog() << "  " << C->GetElement(1, 0) << "  ";
    GetLog() << "  " << C->GetElement(2, 0) << "  ";
    GetLog() << "  " << C->GetElement(3, 0) << "  ";
    GetLog() << "  " << C->GetElement(4, 0) << "\n";

    m_road_wheel->LogConstraintViolations();
}

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

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

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

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

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