示例#1
0
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; 
}
示例#2
0
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";
}
示例#4
0
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
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";
    }
}
示例#5
0
// 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);
}
示例#6
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);
}
示例#7
0
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
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));
}
示例#10
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;
}
示例#11
0
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);
}
示例#14
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;
}
示例#15
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;
}
示例#16
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;
}
示例#17
0
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;
}
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;
	}

}
示例#19
0
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;
}
示例#20
0
int  ChLcpSystemDescriptor::BuildDiagonalVector(
								ChMatrix<>& Diagonal_vect  	///< matrix which will contain the entire vector of terms on M and E diagonal
							)
{
	n_q=CountActiveVariables();
	n_c=CountActiveConstraints();

	Diagonal_vect.Reset(n_q+n_c,1);		// fast! Reset() method does not realloc if size doesn't change

	// Fill the diagonal values given by stiffness blocks, if any
	// (This cannot be easily parallelized because of possible write concurrency).
	for (int is = 0; is< (int)vstiffness.size(); is++)
	{
		vstiffness[is]->DiagonalAdd(Diagonal_vect);
	}

	// Get the 'M' diagonal terms
	#pragma omp parallel for num_threads(this->num_threads)
	for (int iv = 0; iv< (int)vvariables.size(); iv++)
	{
		if (vvariables[iv]->IsActive())
		{
			vvariables[iv]->DiagonalAdd(Diagonal_vect);
		}
	}

	// Get the 'E' diagonal terms (note the sign: E_i = -cfm_i )
	#pragma omp parallel for num_threads(this->num_threads)
	for (int ic = 0; ic< (int)vconstraints.size(); ic++)
	{
		if (vconstraints[ic]->IsActive())
		{
			Diagonal_vect(vconstraints[ic]->GetOffset() + n_q) = - vconstraints[ic]->Get_cfm_i();
		}
	}
	return n_q+n_c;
}
示例#21
0
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;
}
示例#22
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;
}
示例#23
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);
}
示例#24
0
// 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);
}
示例#25
0
void ChLcpSystemDescriptor::SystemProduct(	
								ChMatrix<>&	result,			///< matrix which contains the result of matrix by x 
								ChMatrix<>* x		        ///< optional matrix with the vector to be multiplied (if null, use current l_i and q)
								// std::vector<bool>* enabled=0 ///< optional: vector of enable flags, one per scalar constraint. true=enable, false=disable (skip)
								)
{
	n_q = this->CountActiveVariables();
	n_c = this->CountActiveConstraints();

	ChMatrix<>* x_ql = 0;

	ChMatrix<>* vect;

	if (x)
	{
		#ifdef CH_DEBUG
			assert(x->GetRows()   == n_q+n_c);
			assert(x->GetColumns()== 1);
		#endif
		vect = x;
	}
	else
	{
		x_ql = new ChMatrixDynamic<double>(n_q+n_c,1);
		vect = x_ql;
		this->FromUnknownsToVector(*vect);
	}

	result.Reset(n_q+n_c,1); // fast! Reset() method does not realloc if size doesn't change

	// 1) First row: result.q part =  [M + K]*x.q + [Cq']*x.l

	// 1.1)  do  M*x.q
	#pragma omp parallel for num_threads(this->num_threads)
	for (int iv = 0; iv< (int)vvariables.size(); iv++)
		if (vvariables[iv]->IsActive())
		{
			vvariables[iv]->MultiplyAndAdd(result,*x);
		}

	// 1.2)  add also K*x.q  (NON straight parallelizable - risk of concurrency in writing)
	for (int ik = 0; ik< (int)vstiffness.size(); ik++)
	{
		vstiffness[ik]->MultiplyAndAdd(result,*x);
	}

	// 1.3)  add also [Cq]'*x.l  (NON straight parallelizable - risk of concurrency in writing)
	for (int ic = 0; ic < (int)vconstraints.size(); ic++)
	{	
		if (vconstraints[ic]->IsActive())
		{
			vconstraints[ic]->MultiplyTandAdd(result,  (*x)(vconstraints[ic]->GetOffset()+n_q));
		}
	}

	// 2) Second row: result.l part =  [C_q]*x.q + [E]*x.l
	#pragma omp parallel for num_threads(this->num_threads)
	for (int ic = 0; ic < (int)vconstraints.size(); ic++)
	{	
		if (vconstraints[ic]->IsActive())
		{
			int s_c = vconstraints[ic]->GetOffset() + n_q;
			vconstraints[ic]->MultiplyAndAdd(result(s_c), (*x));     // result.l_i += [C_q_i]*x.q
			result(s_c) -= vconstraints[ic]->Get_cfm_i()* (*x)(s_c); // result.l_i += [E]*x.l_i  NOTE:  cfm = -E
		}
	}		
	


	// if a temp vector has been created because x was not provided, then delete it
	if (x_ql)
		delete x_ql;
}
示例#26
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;
}
示例#27
0
int main(int argc, char* argv[]) {
  // Create output directories.
  if (ChFileutils::MakeDirectory(out_dir.c_str()) < 0) {
    cout << "Error creating directory " << out_dir << endl;
    return 1;
  }
  if (ChFileutils::MakeDirectory(pov_dir.c_str()) < 0) {
    cout << "Error creating directory " << pov_dir << endl;
    return 1;
  }

// -------------
// Create system
// -------------

#ifdef USE_DEM
  cout << "Create DEM system" << endl;
  ChSystemParallelDEM* msystem = new ChSystemParallelDEM();
#else
  cout << "Create DVI system" << endl;
  ChSystemParallelDVI* msystem = new ChSystemParallelDVI();
#endif

  msystem->Set_G_acc(ChVector<>(0, 0, -gravity));

  // Set number of threads.
  int max_threads = omp_get_num_procs();
  if (threads > max_threads)
    threads = max_threads;
  msystem->SetParallelThreadNumber(threads);
  omp_set_num_threads(threads);
  cout << "Using " << threads << " threads" << endl;

  msystem->GetSettings()->max_threads = threads;
  msystem->GetSettings()->perform_thread_tuning = thread_tuning;

  // Edit system settings
  msystem->GetSettings()->solver.use_full_inertia_tensor = false;
  msystem->GetSettings()->solver.tolerance = tolerance;
  msystem->GetSettings()->solver.max_iteration_bilateral = max_iteration_bilateral;
  msystem->GetSettings()->solver.clamp_bilaterals = clamp_bilaterals;
  msystem->GetSettings()->solver.bilateral_clamp_speed = bilateral_clamp_speed;

#ifdef USE_DEM
  msystem->GetSettings()->collision.narrowphase_algorithm = NARROWPHASE_R;
  msystem->GetSettings()->solver.contact_force_model = contact_force_model;
  msystem->GetSettings()->solver.tangential_displ_mode = tangential_displ_mode;
#else
  msystem->GetSettings()->solver.solver_mode = SLIDING;
  msystem->GetSettings()->solver.max_iteration_normal = max_iteration_normal;
  msystem->GetSettings()->solver.max_iteration_sliding = max_iteration_sliding;
  msystem->GetSettings()->solver.max_iteration_spinning = max_iteration_spinning;
  msystem->GetSettings()->solver.alpha = 0;
  msystem->GetSettings()->solver.contact_recovery_speed = contact_recovery_speed;
  msystem->SetMaxPenetrationRecoverySpeed(contact_recovery_speed);
  msystem->ChangeSolverType(APGDREF);

  msystem->GetSettings()->collision.collision_envelope = 0.05 * r_g;
#endif

  msystem->GetSettings()->collision.bins_per_axis = I3(10, 10, 10);

  // --------------
  // Problem set up
  // --------------

  // Depending on problem type:
  // - Select end simulation time
  // - Select output FPS
  // - Create / load objects

  double time_min = 0;
  double time_end;
  int out_fps;
  ChSharedPtr<ChBody> ground;
  ChSharedPtr<ChBody> loadPlate;
  ChSharedPtr<ChLinkLockPrismatic> prismatic;
  ChSharedPtr<ChLinkLinActuator> actuator;

  switch (problem) {
    case SETTLING: {
      time_min = time_settling_min;
      time_end = time_settling_max;
      out_fps = out_fps_settling;

      // Create the mechanism bodies (all fixed).
      CreateMechanismBodies(msystem);

      // Grab handles to mechanism bodies (must increase ref counts)
      ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0));
      loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1));
      msystem->Get_bodylist()->at(0)->AddRef();
      msystem->Get_bodylist()->at(1)->AddRef();

      // Create granular material.
      int num_particles = CreateGranularMaterial(msystem);
      cout << "Granular material:  " << num_particles << " particles" << endl;

      break;
    }

    case PRESSING: {
      time_min = time_pressing_min;
      time_end = time_pressing_max;
      out_fps = out_fps_pressing;

      // Create bodies from checkpoint file.
      cout << "Read checkpoint data from " << settled_ckpnt_file;
      utils::ReadCheckpoint(msystem, settled_ckpnt_file);
      cout << "  done.  Read " << msystem->Get_bodylist()->size() << " bodies." << endl;

      // Grab handles to mechanism bodies (must increase ref counts)
      ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0));
      loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1));
      msystem->Get_bodylist()->at(0)->AddRef();
      msystem->Get_bodylist()->at(1)->AddRef();

      // Move the load plate just above the granular material.
      double highest, lowest;
      FindHeightRange(msystem, lowest, highest);
      ChVector<> pos = loadPlate->GetPos();
      double z_new = highest + 1.01 * r_g;
      loadPlate->SetPos(ChVector<>(pos.x, pos.y, z_new));

      // Add collision geometry to plate
      loadPlate->GetCollisionModel()->ClearModel();
      utils::AddBoxGeometry(loadPlate.get_ptr(), ChVector<>(hdimX_p, hdimY_p, hdimZ_p), ChVector<>(0, 0, hdimZ_p));
      loadPlate->GetCollisionModel()->BuildModel();

      // If using an actuator, connect the load plate and get a handle to the actuator.
      if (use_actuator) {
        ConnectLoadPlate(msystem, ground, loadPlate);
        prismatic = msystem->SearchLink("prismatic").StaticCastTo<ChLinkLockPrismatic>();
        actuator = msystem->SearchLink("actuator").StaticCastTo<ChLinkLinActuator>();
      }

      // Release the load plate.
      loadPlate->SetBodyFixed(!use_actuator);

      break;
    }

    case TESTING: {
      time_end = time_testing;
      out_fps = out_fps_testing;

      // For TESTING only, increse shearing velocity.
      desiredVelocity = 0.5;

      // Create the mechanism bodies (all fixed).
      CreateMechanismBodies(msystem);

      // Create the test ball.
      CreateBall(msystem);

      // Grab handles to mechanism bodies (must increase ref counts)
      ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0));
      loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1));
      msystem->Get_bodylist()->at(0)->AddRef();
      msystem->Get_bodylist()->at(1)->AddRef();

      // Move the load plate just above the test ball.
      ChVector<> pos = loadPlate->GetPos();
      double z_new = 2.1 * radius_ball;
      loadPlate->SetPos(ChVector<>(pos.x, pos.y, z_new));

      // Add collision geometry to plate
      loadPlate->GetCollisionModel()->ClearModel();
      utils::AddBoxGeometry(loadPlate.get_ptr(), ChVector<>(hdimX_p, hdimY_p, hdimZ_p), ChVector<>(0, 0, hdimZ_p));
      loadPlate->GetCollisionModel()->BuildModel();

      // If using an actuator, connect the shear box and get a handle to the actuator.
      if (use_actuator) {
        ConnectLoadPlate(msystem, ground, loadPlate);
        actuator = msystem->SearchLink("actuator").StaticCastTo<ChLinkLinActuator>();
      }

      // Release the shear box when using an actuator.
      loadPlate->SetBodyFixed(!use_actuator);

      break;
    }
  }

  // ----------------------
  // Perform the simulation
  // ----------------------

  // Set number of simulation steps and steps between successive output
  int num_steps = (int)std::ceil(time_end / time_step);
  int out_steps = (int)std::ceil((1.0 / time_step) / out_fps);
  int write_steps = (int)std::ceil((1.0 / time_step) / write_fps);

  // Initialize counters
  double time = 0;
  int sim_frame = 0;
  int out_frame = 0;
  int next_out_frame = 0;
  double exec_time = 0;
  int num_contacts = 0;
  double max_cnstr_viol[2] = {0, 0};

  // Circular buffer with highest particle location
  // (only used for SETTLING or PRESSING)
  int buffer_size = std::ceil(time_min / time_step);
  std::valarray<double> hdata(0.0, buffer_size);

  // Create output files
  ChStreamOutAsciiFile statsStream(stats_file.c_str());
  ChStreamOutAsciiFile sinkageStream(sinkage_file.c_str());
  sinkageStream.SetNumFormat("%16.4e");

#ifdef CHRONO_PARALLEL_HAS_OPENGL
  opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance();
  gl_window.Initialize(1280, 720, "Pressure Sinkage Test", msystem);
  gl_window.SetCamera(ChVector<>(0, -10 * hdimY, hdimZ), ChVector<>(0, 0, hdimZ), ChVector<>(0, 0, 1));
  gl_window.SetRenderMode(opengl::WIREFRAME);
#endif

  // Loop until reaching the end time...
  while (time < time_end) {
    // Current position and velocity of the shear box
    ChVector<> pos_old = loadPlate->GetPos();
    ChVector<> vel_old = loadPlate->GetPos_dt();

    // Calculate minimum and maximum particle heights
    double highest, lowest;
    FindHeightRange(msystem, lowest, highest);

    // If at an output frame, write PovRay file and print info
    if (sim_frame == next_out_frame) {
      cout << "------------ Output frame:   " << out_frame + 1 << endl;
      cout << "             Sim frame:      " << sim_frame << endl;
      cout << "             Time:           " << time << endl;
      cout << "             Load plate pos: " << pos_old.z << endl;
      cout << "             Lowest point:   " << lowest << endl;
      cout << "             Highest point:  " << highest << endl;
      cout << "             Execution time: " << exec_time << endl;

      // Save PovRay post-processing data.
      if (write_povray_data) {
        char filename[100];
        sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), out_frame + 1);
        utils::WriteShapesPovray(msystem, filename, false);
      }

      // Create a checkpoint from the current state.
      if (problem == SETTLING || problem == PRESSING) {
        cout << "             Write checkpoint data " << flush;
        if (problem == SETTLING)
          utils::WriteCheckpoint(msystem, settled_ckpnt_file);
        else
          utils::WriteCheckpoint(msystem, pressed_ckpnt_file);
        cout << msystem->Get_bodylist()->size() << " bodies" << endl;
      }

      // Increment counters
      out_frame++;
      next_out_frame += out_steps;
    }

    // Check for early termination of a settling phase.
    if (problem == SETTLING) {
      // Store maximum particle height in circular buffer
      hdata[sim_frame % buffer_size] = highest;

      // Check variance of data in circular buffer
      if (time > time_min) {
        double mean_height = hdata.sum() / buffer_size;
        std::valarray<double> x = hdata - mean_height;
        double var = std::sqrt((x * x).sum() / buffer_size);

        // Consider the material settled when the variance is below the
        // specified fraction of a particle radius
        if (var < settling_tol * r_g) {
          cout << "Granular material settled...  time = " << time << endl;
          break;
        }
      }
    }

// Advance simulation by one step
#ifdef CHRONO_PARALLEL_HAS_OPENGL
    if (gl_window.Active()) {
      gl_window.DoStepDynamics(time_step);
      gl_window.Render();
    } else
      break;
#else
    msystem->DoStepDynamics(time_step);
#endif

    TimingOutput(msystem);

    // Record stats about the simulation
    if (sim_frame % write_steps == 0) {
      // write stat info
      int numIters = msystem->data_manager->measures.solver.maxd_hist.size();
      double residual = 0;
      if (numIters)
        residual = msystem->data_manager->measures.solver.residual;
      statsStream << time << ", " << exec_time << ", " << num_contacts / write_steps << ", " << numIters << ", "
                  << residual << ", " << max_cnstr_viol[0] << ", " << max_cnstr_viol[1] << ", \n";
      statsStream.GetFstream().flush();

      num_contacts = 0;
      max_cnstr_viol[0] = 0;
      max_cnstr_viol[1] = 0;
    }

    if (problem == PRESSING || problem == TESTING) {
      // Get the current reaction force or impose load plate position
      double cnstr_force = 0;
      if (use_actuator) {
        cnstr_force = actuator->Get_react_force().x;
      } else {
        double zpos_new = pos_old.z + desiredVelocity * time_step;
        loadPlate->SetPos(ChVector<>(pos_old.x, pos_old.y, zpos_new));
        loadPlate->SetPos_dt(ChVector<>(0, 0, desiredVelocity));
      }

      if (sim_frame % write_steps == 0) {
        // std::cout << time << ", " << loadPlate->GetPos().z << ", " << cnstr_force << ", \n";
        sinkageStream << time << ", " << loadPlate->GetPos().z << ", " << cnstr_force << ", \n";
        sinkageStream.GetFstream().flush();
      }
    }

    // Find maximum constraint violation
    if (!prismatic.IsNull()) {
      ChMatrix<>* C = prismatic->GetC();
      for (int i = 0; i < 5; i++)
        max_cnstr_viol[0] = std::max(max_cnstr_viol[0], std::abs(C->GetElement(i, 0)));
    }
    if (!actuator.IsNull()) {
      ChMatrix<>* C = actuator->GetC();
      max_cnstr_viol[1] = std::max(max_cnstr_viol[2], std::abs(C->GetElement(0, 0)));
    }

    // Increment counters
    time += time_step;
    sim_frame++;
    exec_time += msystem->GetTimerStep();
    num_contacts += msystem->GetNcontacts();

    // If requested, output detailed timing information for this step
    if (sim_frame == timing_frame)
      msystem->PrintStepStats();
  }

  // ----------------
  // Final processing
  // ----------------

  // Create a checkpoint from the last state
  if (problem == SETTLING || problem == PRESSING) {
    cout << "             Write checkpoint data " << flush;
    if (problem == SETTLING)
      utils::WriteCheckpoint(msystem, settled_ckpnt_file);
    else
      utils::WriteCheckpoint(msystem, pressed_ckpnt_file);
    cout << msystem->Get_bodylist()->size() << " bodies" << endl;
  }

  // Final stats
  cout << "==================================" << endl;
  cout << "Number of bodies:  " << msystem->Get_bodylist()->size() << endl;
  cout << "Simulation time:   " << exec_time << endl;
  cout << "Number of threads: " << threads << endl;

  return 0;
}
// =============================================================================
//
// Worker function for performing the simulation with specified parameters.
//
bool TestLinActuator(const ChQuaternion<>& rot,              // translation along Z axis
                     double                desiredSpeed,     // imposed translation speed
                     double                simTimeStep,      // simulation time step
                     double                outTimeStep,      // output time step
                     const std::string&    testName,         // name of this test
                     bool                  animate)          // if true, animate with Irrlich
{
  std::cout << "TEST: " << testName << std::endl;

  // Unit vector along translation axis, expressed in global frame
  ChVector<> axis = rot.GetZaxis();

  // Settings
  //---------

  double mass = 1.0;              // mass of plate
  ChVector<> inertiaXX(1, 1, 1);  // mass moments of inertia of plate (centroidal frame)
  double g = 9.80665;

  double timeRecord = 5;          // Stop recording to the file after this much simulated time

  // Create the mechanical system
  // ----------------------------

  ChSystem my_system;
  my_system.Set_G_acc(ChVector<>(0.0, 0.0, -g));

  my_system.SetIntegrationType(ChSystem::INT_ANITESCU);
  my_system.SetIterLCPmaxItersSpeed(100);
  my_system.SetIterLCPmaxItersStab(100); //Tasora stepper uses this, Anitescu does not
  my_system.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
  my_system.SetTol(1e-6);
  my_system.SetTolForce(1e-4);


  // Create the ground body.

  ChSharedBodyPtr  ground(new ChBody);
  my_system.AddBody(ground);
  ground->SetBodyFixed(true);

  // Add geometry to the ground body for visualizing the translational joint
  ChSharedPtr<ChBoxShape> box_g(new ChBoxShape);
  box_g->GetBoxGeometry().SetLengths(ChVector<>(0.1, 0.1, 5));
  box_g->GetBoxGeometry().Pos = 2.5 * axis;
  box_g->GetBoxGeometry().Rot = rot;
  ground->AddAsset(box_g);

  ChSharedPtr<ChColorAsset> col_g(new ChColorAsset);
  col_g->SetColor(ChColor(0.6f, 0.2f, 0.2f));
  ground->AddAsset(col_g);

  // Create the plate body.

  ChSharedBodyPtr  plate(new ChBody);
  my_system.AddBody(plate);
  plate->SetPos(ChVector<>(0, 0, 0));
  plate->SetRot(rot);
  plate->SetPos_dt(desiredSpeed * axis);
  plate->SetMass(mass);
  plate->SetInertiaXX(inertiaXX);

  // Add geometry to the plate for visualization
  ChSharedPtr<ChBoxShape> box_p(new ChBoxShape);
  box_p->GetBoxGeometry().SetLengths(ChVector<>(1, 1, 0.2));
  plate->AddAsset(box_p);

  ChSharedPtr<ChColorAsset> col_p(new ChColorAsset);
  col_p->SetColor(ChColor(0.2f, 0.2f, 0.6f));
  plate->AddAsset(col_p);

  // Create prismatic (translational) joint between plate and ground.
  // We set the ground as the "master" body (second one in the initialization
  // call) so that the link coordinate system is expressed in the ground frame.

  ChSharedPtr<ChLinkLockPrismatic> prismatic(new ChLinkLockPrismatic);
  prismatic->Initialize(plate, ground, ChCoordsys<>(ChVector<>(0, 0, 0), rot));
  my_system.AddLink(prismatic);

  // Create a ramp function to impose constant speed.  This function returns
  //   y(t) = 0 + t * desiredSpeed
  //   y'(t) = desiredSpeed

  ChSharedPtr<ChFunction_Ramp> actuator_fun(new ChFunction_Ramp(0.0, desiredSpeed));

  // Create the linear actuator, connecting the plate to the ground.
  // Here, we set the plate as the master body (second one in the initialization
  // call) so that the link coordinate system is expressed in the plate body
  // frame.

  ChSharedPtr<ChLinkLinActuator> actuator(new ChLinkLinActuator);
  ChVector<> pt1 = ChVector<>(0, 0, 0);
  ChVector<> pt2 = axis;
  actuator->Initialize(ground, plate, false, ChCoordsys<>(pt1, rot), ChCoordsys<>(pt2, rot));
  actuator->Set_lin_offset(1);
  actuator->Set_dist_funct(actuator_fun);
  my_system.AddLink(actuator);

  // Perform the simulation (animation with Irrlicht)
  // ------------------------------------------------

  if (animate)
  {
    // Create the Irrlicht application for visualization
    ChIrrApp application(&my_system, L"ChLinkRevolute demo", core::dimension2d<u32>(800, 600), false, true);
    application.AddTypicalLogo();
    application.AddTypicalSky();
    application.AddTypicalLights();
    application.AddTypicalCamera(core::vector3df(1, 2, -2), core::vector3df(0, 0, 2));

    application.AssetBindAll();
    application.AssetUpdateAll();

    application.SetStepManage(true);
    application.SetTimestep(simTimeStep);

    // Simulation loop
    while (application.GetDevice()->run())
    {
      application.BeginScene();
      application.DrawAll();

      // Draw an XZ grid at the global origin to add in visualization
      ChIrrTools::drawGrid(
        application.GetVideoDriver(), 1, 1, 20, 20,
        ChCoordsys<>(ChVector<>(0, 0, 0), Q_from_AngX(CH_C_PI_2)),
        video::SColor(255, 80, 100, 100), true);

      application.DoStep();  //Take one step in time
      application.EndScene();
    }

    return true;
  }

  // Perform the simulation (record results)
  // ------------------------------------------------

  // Create the CSV_Writer output objects (TAB delimited)
  utils::CSV_writer out_pos = OutStream();
  utils::CSV_writer out_vel = OutStream();
  utils::CSV_writer out_acc = OutStream();

  utils::CSV_writer out_quat = OutStream();
  utils::CSV_writer out_avel = OutStream();
  utils::CSV_writer out_aacc = OutStream();

  utils::CSV_writer out_rfrcP = OutStream();
  utils::CSV_writer out_rtrqP = OutStream();

  utils::CSV_writer out_rfrcA = OutStream();
  utils::CSV_writer out_rtrqA = OutStream();

  utils::CSV_writer out_cnstrP = OutStream();

  utils::CSV_writer out_cnstrA = OutStream();

  // Write headers
  out_pos << "Time" << "X_Pos" << "Y_Pos" << "Z_Pos" << std::endl;
  out_vel << "Time" << "X_Vel" << "Y_Vel" << "Z_Vel" << std::endl;
  out_acc << "Time" << "X_Acc" << "Y_Acc" << "Z_Acc" << std::endl;

  out_quat << "Time" << "e0" << "e1" << "e2" << "e3" << std::endl;
  out_avel << "Time" << "X_AngVel" << "Y_AngVel" << "Z_AngVel" << std::endl;
  out_aacc << "Time" << "X_AngAcc" << "Y_AngAcc" << "Z_AngAcc" << std::endl;

  out_rfrcP << "Time" << "X_Force" << "Y_Force" << "Z_Force" << std::endl;
  out_rtrqP << "Time" << "X_Torque" << "Y_Torque" << "Z_Torque" << std::endl;

  out_rfrcA << "Time" << "X_Force" << "Y_Force" << "Z_Force" << std::endl;
  out_rtrqA << "Time" << "X_Torque" << "Y_Torque" << "Z_Torque" << std::endl;

  out_cnstrP << "Time" << "Cnstr_1" << "Cnstr_2" << "Cnstr_3" << "Constraint_4" << "Cnstr_5" << std::endl;

  out_cnstrA << "Time" << "Cnstr_1" << std::endl;

  // Simulation loop
  double simTime = 0;
  double outTime = 0;

  while (simTime <= timeRecord + simTimeStep / 2)
  {
    // Ensure that the final data point is recorded.
    if (simTime >= outTime - simTimeStep / 2)
    {

      // CM position, velocity, and acceleration (expressed in global frame).
      const ChVector<>& position = plate->GetPos();
      const ChVector<>& velocity = plate->GetPos_dt();
      out_pos << simTime << position << std::endl;
      out_vel << simTime << velocity << std::endl;
      out_acc << simTime << plate->GetPos_dtdt() << std::endl;

      // Orientation, angular velocity, and angular acceleration (expressed in
      // global frame).
      out_quat << simTime << plate->GetRot() << std::endl;
      out_avel << simTime << plate->GetWvel_par() << std::endl;
      out_aacc << simTime << plate->GetWacc_par() << std::endl;

      // Reaction Force and Torque in prismatic joint.
      // These are expressed in the link coordinate system. We convert them to
      // the coordinate system of Body2 (in our case this is the ground).
      ChCoordsys<> linkCoordsysP = prismatic->GetLinkRelativeCoords();
      ChVector<> reactForceP = prismatic->Get_react_force();
      ChVector<> reactForceGlobalP = linkCoordsysP.TransformDirectionLocalToParent(reactForceP);
      out_rfrcP << simTime << reactForceGlobalP << std::endl;

      ChVector<> reactTorqueP = prismatic->Get_react_torque();
      ChVector<> reactTorqueGlobalP = linkCoordsysP.TransformDirectionLocalToParent(reactTorqueP);
      out_rtrqP << simTime << reactTorqueGlobalP << std::endl;


      // Reaction force and Torque in linear actuator.
      // These are expressed  in the link coordinate system. We convert them to
      // the coordinate system of Body2 (in our case this is the plate). As such,
      // the reaction force represents the force that needs to be applied to the
      // plate in order to maintain the prescribed constant velocity.  These are
      // then converted to the global frame for comparison to ADAMS
      ChCoordsys<> linkCoordsysA = actuator->GetLinkRelativeCoords();
      ChVector<> reactForceA = actuator->Get_react_force();
      reactForceA = linkCoordsysA.TransformDirectionLocalToParent(reactForceA);
      ChVector<> reactForceGlobalA = plate->TransformDirectionLocalToParent(reactForceA);
      out_rfrcA << simTime << reactForceGlobalA << std::endl;

      ChVector<> reactTorqueA = actuator->Get_react_torque();
      reactTorqueA = linkCoordsysA.TransformDirectionLocalToParent(reactTorqueA);
      ChVector<> reactTorqueGlobalA = plate->TransformDirectionLocalToParent(reactTorqueA);
      out_rtrqA << simTime << reactTorqueGlobalA << std::endl;


      // Constraint violations in prismatic joint
      ChMatrix<>* CP = prismatic->GetC();
      out_cnstrP << simTime
                 << CP->GetElement(0, 0)
                 << CP->GetElement(1, 0)
                 << CP->GetElement(2, 0)
                 << CP->GetElement(3, 0)
                 << CP->GetElement(4, 0) << std::endl;

      // Constraint violations in linear actuator
      ChMatrix<>* CA = actuator->GetC();
      out_cnstrA << simTime << CA->GetElement(0, 0) << std::endl;

      // Increment output time
      outTime += outTimeStep;
    }

    // Advance simulation by one step
    my_system.DoStepDynamics(simTimeStep);

    // Increment simulation time
    simTime += simTimeStep;
  }

  // Write output files
  out_pos.write_to_file(out_dir + testName + "_CHRONO_Pos.txt", testName + "\n\n");
  out_vel.write_to_file(out_dir + testName + "_CHRONO_Vel.txt", testName + "\n\n");
  out_acc.write_to_file(out_dir + testName + "_CHRONO_Acc.txt", testName + "\n\n");

  out_quat.write_to_file(out_dir + testName + "_CHRONO_Quat.txt", testName + "\n\n");
  out_avel.write_to_file(out_dir + testName + "_CHRONO_Avel.txt", testName + "\n\n");
  out_aacc.write_to_file(out_dir + testName + "_CHRONO_Aacc.txt", testName + "\n\n");

  out_rfrcP.write_to_file(out_dir + testName + "_CHRONO_RforceP.txt", testName + "\n\n");
  out_rtrqP.write_to_file(out_dir + testName + "_CHRONO_RtorqueP.txt", testName + "\n\n");

  out_rfrcA.write_to_file(out_dir + testName + "_CHRONO_RforceA.txt", testName + "\n\n");
  out_rtrqA.write_to_file(out_dir + testName + "_CHRONO_RtorqueA.txt", testName + "\n\n");

  out_cnstrP.write_to_file(out_dir + testName + "_CHRONO_ConstraintsP.txt", testName + "\n\n");

  out_cnstrA.write_to_file(out_dir + testName + "_CHRONO_ConstraintsA.txt", testName + "\n\n");

  return true;
}
示例#29
0
void ChLcpSystemDescriptor::ShurComplementProduct(	
								ChMatrix<>&	result,	
								ChMatrix<>* lvector,
								std::vector<bool>* enabled  
								)
{
	#ifdef CH_DEBUG
		assert(this->vstiffness.size() == 0); // currently, the case with ChLcpKblock items is not supported (only diagonal M is supported, no K)
		int n_c=CountActiveConstraints();
		assert(lvector->GetRows()   == n_c);
		assert(lvector->GetColumns()== 1);
		if (enabled) assert(enabled->size() == n_c);
	#endif

	result.Reset(n_c,1);  // fast! Reset() method does not realloc if size doesn't change


	// Performs the sparse product    result = [N]*l = [ [Cq][M^(-1)][Cq'] - [E] ] *l
	// in different phases:

	// 1 - set the qb vector (aka speeds, in each ChLcpVariable sparse data) as zero

	#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().FillElem(0);
	}

	// 2 - performs    qb=[M^(-1)][Cq']*l  by
	//     iterating over all constraints (when implemented in parallel this
	//     could be non-trivial because race conditions might occur -> reduction buffer etc.)
	//     Also, begin to add the cfm term ( -[E]*l ) to the result.

	//#pragma omp parallel for num_threads(this->num_threads)  ***NOT POSSIBLE!!! concurrent write to same q may happen
	for (int ic = 0; ic < (int)vconstraints.size(); ic++)
	{	
		if (vconstraints[ic]->IsActive())
		{
			int s_c = vconstraints[ic]->GetOffset();

			bool process=true;
			if (enabled)
				if ((*enabled)[s_c]==false)
					process = false;

			if (process) 
			{
				double li;
				if (lvector)
					li = (*lvector)(s_c,0);
				else
					li = vconstraints[ic]->Get_l_i();

				// Compute qb += [M^(-1)][Cq']*l_i
				//  NOTE! parallel update to same q data, risk of collision if parallel!!
				vconstraints[ic]->Increment_q(li);	// <----!!!  fpu intensive

				// Add constraint force mixing term  result = cfm * l_i = -[E]*l_i
				result(s_c,0) =  vconstraints[ic]->Get_cfm_i() * li;

			}

		}
	}

	// 3 - performs    result=[Cq']*qb    by
	//     iterating over all constraints 

	#pragma omp parallel for num_threads(this->num_threads)
	for (int ic = 0; ic < (int)vconstraints.size(); ic++)
	{	
		if (vconstraints[ic]->IsActive())
		{
			bool process=true;
			if (enabled)
				if ((*enabled)[vconstraints[ic]->GetOffset()]==false)
					process = false;
			
			if (process) 
				result(vconstraints[ic]->GetOffset(),0)+= vconstraints[ic]->Compute_Cq_q();	// <----!!!  fpu intensive
			else
				result(vconstraints[ic]->GetOffset(),0)= 0; // not enabled constraints, just set to 0 result 
		}
	}


}