public void Build_CqT(ref ChSparseMatrix storage, int inscol)
{
if (variables.IsActive())
{
storage.PasteTranspMatrix(Cq.matrix, variables.GetOffset(), inscol);
}
}
public void Build_Cq(ref ChSparseMatrix storage, int insrow)
{
if (variables.IsActive())
{
storage.PasteMatrix(Cq.matrix, insrow, variables.GetOffset());
}
}
/// Build the mass matrix (for these variables) scaled by c_a, storing
/// it in 'storage' sparse matrix, at given column/row offset.
/// Note, most iterative solvers don't need to know mass matrix explicitly.
/// Optimized: doesn't fill unneeded elements except mass and 3x3 inertia.
public override void Build_M(ChSparseMatrix storage, int insrow, int inscol, double c_a)
{
storage.SetElement(insrow + 0, inscol + 0, c_a * mass);
storage.SetElement(insrow + 1, inscol + 1, c_a * mass);
storage.SetElement(insrow + 2, inscol + 2, c_a * mass);
// ChMatrix33<double> scaledJ = (ChMatrix33<double>)(inertia.nm.matrix * c_a);
// storage.PasteMatrix(scaledJ, insrow + 3, inscol + 3);
}
public ChSparseMatrix(ChSparseMatrix other)
{
m_num_rows = other.m_num_rows;
m_num_cols = other.m_num_cols;
m_nnz = other.m_nnz;
m_type = other.m_type;
m_lock = other.m_lock;
m_update_sparsity_pattern = other.m_update_sparsity_pattern;
}
/// Build the mass matrix (for these variables) scaled by c_a, storing
/// it in 'storage' sparse matrix, at given column/row offset.
/// Note, most iterative solvers don't need to know mass matrix explicitly.
public override void Build_M(ChSparseMatrix storage, int insrow, int inscol, double c_a)
{
for (int row = 0; row < Mmass.matrix.GetRows(); ++row)
{
for (int col = 0; col < Mmass.matrix.GetColumns(); ++col)
{
storage.SetElement(insrow + row, inscol + col, c_a * Mmass.matrix.GetElement(row, col));
}
}
}
public override void Build_CqT(ref ChSparseMatrix storage, int inscol)
{
if (variables_a.IsActive())
{
storage.PasteTranspMatrix(Cq_a.matrix, variables_a.GetOffset(), inscol);
}
if (variables_b.IsActive())
{
storage.PasteTranspMatrix(Cq_b.matrix, variables_b.GetOffset(), inscol);
}
}
/// Puts the two jacobian parts into the 'insrow' row of a sparse matrix,
/// where both portions of the jacobian are shifted in order to match the
/// offset of the corresponding ChVariable.The same is done
/// on the 'insrow' column, so that the sparse matrix is kept symmetric.
public override void Build_Cq(ref ChSparseMatrix storage, int insrow)
{
if (variables_a.IsActive())
{
storage.PasteMatrix(Cq_a.matrix, insrow, variables_a.GetOffset());
}
if (variables_b.IsActive())
{
storage.PasteMatrix(Cq_b.matrix, insrow, variables_b.GetOffset());
}
}
/// Writes the K matrix associated to these variables into
/// a global 'storage' matrix, at the offsets of variables.
/// Most solvers do not need this: the sparse 'storage' matrix is used for testing, for
/// direct solvers, for dumping full matrix to Matlab for checks, etc.
public override void Build_K(ref ChSparseMatrix storage, bool add)
{
// if (K == null)
// return;
int kio = 0;
for (int iv = 0; iv < this.GetNvars(); iv++)
{
int io = this.GetVariableN(iv).GetOffset();
int ina = this.GetVariableN(iv).Get_ndof();
if (this.GetVariableN(iv).IsActive())
{
int kjo = 0;
for (int jv = 0; jv < this.GetNvars(); jv++)
{
int jo = this.GetVariableN(jv).GetOffset();
int jn = this.GetVariableN(jv).Get_ndof();
if (this.GetVariableN(jv).IsActive())
{
if (add)
{
storage.PasteSumClippedMatrix(K.matrix, kio, kjo, ina, jn, io, jo);
}
else
{
storage.PasteClippedMatrix(K.matrix, kio, kjo, ina, jn, io, jo);
}
}
kjo += jn;
}
}
kio += ina;
}
}
//
// 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++;
}
}
}
/// Writes (and adds) the K matrix associated to these variables into /// a global 'storage' matrix, at the offsets of variables. /// Most solvers do not need this: the sparse 'storage' matrix is used for testing, for /// direct solvers, for dumping full matrix to Matlab for checks, etc. public abstract void Build_K(ref ChSparseMatrix storage, bool add = true);
/// Same as Build_Cq, but puts the _transposed_ jacobian row as a column. public abstract void Build_CqT(ref ChSparseMatrix storage, int inscol);
/// Puts the jacobian portions into the 'insrow' row of a sparse matrix, /// where each portion of jacobian is shifted in order to match the /// offset of the corresponding ChVariable. public abstract void Build_Cq(ref ChSparseMatrix storage, int insrow);
/// Build the mass submatrix (for these variables) multiplied by c_a, storing /// it in 'storage' sparse matrix, at given column/row offset. /// Most iterative solvers don't need to know this matrix explicitly. /// *** This function MUST BE OVERRIDDEN by specialized /// inherited classes public abstract void Build_M(ChSparseMatrix storage, int insrow, int inscol, double c_a);
public override void Build_CqT(ref ChSparseMatrix storage, int inscol)
{
tuple_a.Build_CqT(ref storage, inscol);
tuple_b.Build_CqT(ref storage, inscol);
}
/// Build the mass matrix (for these variables) scaled by c_a, storing
/// it in 'storage' sparse matrix, at given column/row offset.
/// Note, most iterative solvers don't need to know mass matrix explicitly.
/// Optimized: doesn't fill unneeded elements except mass.
public override void Build_M(ChSparseMatrix storage, int insrow, int inscol, double c_a)
{
storage.SetElement(insrow + 0, inscol + 0, c_a * m_inertia);
}
