/// Using this function, one may get a vector with all the unknowns x={q,l} i.e. q variables & l_i constr.
/// ordered into a column vector. The column vector must be passed as a ChMatrix<>
/// object, which will be automatically reset and resized to the proper length if necessary
/// (but if you are sure that the vector has already the proper size, you can optimize
/// the performance a bit by setting resize_vector as false).
/// \return the number of scalar unknowns
public virtual int FromUnknownsToVector(
ref ChMatrix mvector, //< matrix which will contain the entire vector x={q,l}
bool resize_vector = true ///< if true the vector size will be checked & resized if necessary
)
{
// 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
for (int iv = 0; iv < (int)vvariables.Count; iv++)
{
if (vvariables[iv].IsActive())
{
mvector.PasteMatrix(vvariables[iv].Get_qb().matrix, vvariables[iv].GetOffset(), 0);
}
}
// Fill the second part of vector, x.l, with constraint multipliers -l (with flipped sign!)
for (int ic = 0; ic < (int)vconstraints.Count; ic++)
{
if (vconstraints[ic].IsActive())
{
mvector[vconstraints[ic].GetOffset() + n_q] = -vconstraints[ic].Get_l_i();
}
}
return(n_q + n_c);
}
/// Get the d vector = {f; -b} with all the 'fb' and 'bi' known terms, as in Z*y-d
/// (it is the concatenation of BuildFbVector and BuildBiVector) The column vector must be passed as a ChMatrix<>
/// object, which will be automatically reset and resized to the proper length if necessary.
public virtual int BuildDiVector(ref ChMatrix Dvector //< matrix which will contain the entire vector of {f;-b}
)
{
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
for (int iv = 0; iv < (int)vvariables.Count; iv++)
{
if (vvariables[iv].IsActive())
{
Dvector.PasteMatrix(vvariables[iv].Get_fb().matrix, vvariables[iv].GetOffset(), 0);
}
}
// Fill the '-b' vector (with flipped sign!)
for (int ic = 0; ic < (int)vconstraints.Count; ic++)
{
if (vconstraints[ic].IsActive())
{
Dvector[vconstraints[ic].GetOffset() + n_q] = -vconstraints[ic].Get_b_i();
}
}
return(n_q + n_c);
}
//
// DATA <. MATH.VECTORS FUNCTIONS
//
/// Get a vector with all the 'fb' known terms ('forces'etc.) associated to all variables,
/// ordered into a column vector. The column vector must be passed as a ChMatrix<>
/// object, which will be automatically reset and resized to the proper length if necessary.
public virtual int BuildFbVector(ref 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
for (int iv = 0; iv < (int)vvariables.Count; iv++)
{
if (vvariables[iv].IsActive())
{
Fvector.PasteMatrix(vvariables[iv].Get_fb().matrix, vvariables[iv].GetOffset(), 0);
}
}
return(this.n_q);
}
/// Using this function, one may get a vector with all the variables 'q'
/// ordered into a column vector. The column vector must be passed as a ChMatrix<>
/// object, which will be automatically reset and resized to the proper length if necessary
/// (but if you are sure that the vector has already the proper size, you can optimize
/// the performance a bit by setting resize_vector as false).
/// \return the number of scalar variables (i.e. the rows of the column vector).
public virtual int FromVariablesToVector(
ChMatrix mvector, //< matrix which will contain the entire vector of 'q'
bool resize_vector = true //< if true the vector size will be checked & resized if necessary
)
{
// Count active variables and resize vector if necessary
if (resize_vector)
{
n_q = CountActiveVariables();
mvector.Resize(n_q, 1);
}
// Fill the vector
for (int iv = 0; iv < vvariables.Count; iv++)
{
if (vvariables[iv].IsActive())
{
mvector.PasteMatrix(vvariables[iv].Get_qb().matrix, vvariables[iv].GetOffset(), 0);
}
}
return(n_q);
}
//
// LOGGING/OUTPUT/ETC.
//
/// The following function may be used to create the Jacobian and the
/// mass matrix of the variational problem in matrix form, by assembling all
/// the jacobians of all the constraints/contacts, all the mass matrices, all vectors,
/// as they are _currently_ stored in the sparse data of all ChConstraint and ChVariables
/// contained in this ChSystemDescriptor.
///
/// This can be useful for debugging, data dumping, and similar purposes (most solvers avoid
/// using these matrices, for performance), for example you will load these matrices in Matlab.
/// Optionally, tangential (u,v) contact jacobians may be skipped, or only bilaterals can be considered
/// The matrices and vectors are automatically resized if needed.
public virtual void ConvertToMatrixForm(
ChSparseMatrix Cq, //< fill this system jacobian matrix, if not null
ChSparseMatrix H, //< fill this system H (mass+stiffness+damp) matrix, if not null
ChSparseMatrix E, //< fill this system 'compliance' matrix , if not null
ChMatrix Fvector, //< fill this vector as the known term 'f', if not null
ChMatrix Bvector, //< fill this vector as the known term 'b', if not null
ChMatrix Frict, //< fill as a vector with friction coefficients (=-1 for
/// tangent comp.; =-2 for bilaterals), if not null
bool only_bilaterals = false, //< skip unilateral constraints
bool skip_contacts_uv = false //< skip the tangential reaction constraints
)
{
List <ChConstraint> mconstraints = this.GetConstraintsList();
List <ChVariables> mvariables = this.GetVariablesList();
// Count bilateral and other constraints.. (if wanted, bilaterals only)
int mn_c = 0;
for (int ic = 0; ic < mconstraints.Count; ic++)
{
/* if (mconstraints[ic].IsActive())
* if (!((mconstraints[ic].GetMode() == CONSTRAINT_FRIC) && only_bilaterals))
* if (!(((ChConstraintTwoTuplesFrictionTall)(mconstraints[ic])) && skip_contacts_uv))
* {
* mn_c++;
* }*/
}
// Count active variables, by scanning through all variable blocks,
// and set offsets
n_q = this.CountActiveVariables();
// Reset and resize (if needed) auxiliary vectors
if (Cq != null)
{
Cq.Reset(mn_c, n_q);
}
if (H != null)
{
H.Reset(n_q, n_q);
}
if (E != null)
{
E.Reset(mn_c, mn_c);
}
// if (Fvector != null)
Fvector.Reset(n_q, 1);
// if (Bvector != null)
Bvector.Reset(mn_c, 1);
// if (Frict != null)
Frict.Reset(mn_c, 1);
// Fills H submasses and 'f' vector,
// by looping on variables
int s_q = 0;
for (int iv = 0; iv < mvariables.Count; iv++)
{
if (mvariables[iv].IsActive())
{
if (H != null)
{
mvariables[iv].Build_M(H, s_q, s_q, this.c_a); // .. fills H (often H=M , the mass)
}
// if (Fvector != null)
Fvector.PasteMatrix(vvariables[iv].Get_fb().matrix, s_q, 0); // .. fills 'f'
s_q += mvariables[iv].Get_ndof();
}
}
// If some stiffness / hessian matrix has been added to H ,
// also add it to the sparse H
int s_k = 0;
if (H != null)
{
for (int ik = 0; ik < this.vstiffness.Count; ik++)
{
this.vstiffness[ik].Build_K(ref H, true);
}
}
// Fills Cq jacobian, E 'compliance' matrix , the 'b' vector and friction coeff.vector,
// by looping on constraints
int s_c = 0;
for (int ic = 0; ic < mconstraints.Count; ic++)
{
if (mconstraints[ic].IsActive())
{
/* if (!((mconstraints[ic].GetMode() == CONSTRAINT_FRIC) && only_bilaterals))
* if (!(((ChConstraintTwoTuplesFrictionTall)mconstraints[ic]) && skip_contacts_uv))
* {
* if (Cq != null)
* mconstraints[ic].Build_Cq(ref Cq, s_c); // .. fills Cq
* if (E != null)
* E.SetElement(s_c, s_c, -mconstraints[ic].Get_cfm_i()); // .. fills E ( = - cfm )
* if (Bvector != null)
* (Bvector)(s_c) = mconstraints[ic].Get_b_i(); // .. fills 'b'
* if (Frict != null) // .. fills vector of friction coefficients
* {
* (Frict)(s_c) = -2; // mark with -2 flag for bilaterals (default)
* ChConstraintTwoTuplesContactNall mcon = new ChConstraintTwoTuplesContactNall();
* if (mcon == (ChConstraintTwoTuplesContactNall)mconstraints[ic])
*
* (Frict)(s_c) =
* mcon.GetFrictionCoefficient(); // friction coeff only in row of normal component
* if (mcon = ChConstraintTwoTuplesFrictionTall) (mconstraints[ic]))
* (Frict)(s_c) = -1; // mark with -1 flag for rows of tangential components*/
s_c++;
}
}
}