/// Solve the general system A*x = b.
/// Note that the matrix is modified in-place to contain the LU factors.
public int SolveGeneral(ChMatrix b, ChMatrix x)
{
int err = Setup_LU();
Solve_LU(b, ref x);
return(err);
}
/// Computes the product of the corresponding block in the
/// system matrix by 'vect', and add to 'result'.
/// NOTE: the 'vect' vector 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;
public override void MultiplyAndAdd(ref double result, ChMatrix vect)
{
if (variables_a.IsActive())
{
for (int i = 0; i < Cq_a.matrix.GetRows(); i++)
{
result += vect[variables_a.GetOffset() + i] * Cq_a.matrix.ElementN(i);
}
}
if (variables_b.IsActive())
{
for (int i = 0; i < Cq_b.matrix.GetRows(); i++)
{
result += vect[variables_b.GetOffset() + i] * Cq_b.matrix.ElementN(i);
}
}
if (variables_c.IsActive())
{
for (int i = 0; i < Cq_c.matrix.GetRows(); i++)
{
result += vect[variables_c.GetOffset() + i] * Cq_c.matrix.ElementN(i);
}
}
}
/// Computes the product of the corresponding transposed blocks in the
/// system matrix (ie. the TRANSPOSED jacobian matrix C_q') by 'l', and add to 'result'.
/// NOTE: the 'result' vector 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;
public override void MultiplyTandAdd(ChMatrix result, double l)
{
if (variables_a.IsActive())
{
for (int i = 0; i < Cq_a.matrix.GetRows(); i++)
{
result[variables_a.GetOffset() + i] += Cq_a.matrix.ElementN(i) * l;
}
}
if (variables_b.IsActive())
{
for (int i = 0; i < Cq_b.matrix.GetRows(); i++)
{
result[variables_b.GetOffset() + i] += Cq_b.matrix.ElementN(i) * l;
}
}
if (variables_c.IsActive())
{
for (int i = 0; i < Cq_c.matrix.GetRows(); i++)
{
result[variables_c.GetOffset() + i] += Cq_c.matrix.ElementN(i) * l;
}
}
}
/// Paste a given matrix into \a this sparse matrix at position (\a insrow, \a inscol).
/// The matrix \a matra will be copied into \a this[insrow : insrow + \a matra.GetRows()][[inscol : inscol + matra.GetColumns()]]
/// \param[in] matra The source matrix that will be copied;
/// \param[in] insrow The row index where the first element will be copied;
/// \param[in] inscol The column index where the first element will be copied;
/// \param[in] overwrite Tells if the copied element will overwrite an existing element or be summed to it;
/// \param[in] transp Tells if the \a matra matrix should be copied transposed.
public virtual void PasteMatrix(ChMatrix matra, int insrow, int inscol, bool overwrite = true, bool transp = false)
{
var maxrows = matra.GetRows();
var maxcols = matra.GetColumns();
if (transp)
{
for (var i = 0; i < maxcols; i++)
{
for (var j = 0; j < maxrows; j++)
{
this.SetElement(insrow + i, inscol + j, matra[j, i], overwrite);
}
}
}
else
{
for (var i = 0; i < maxrows; i++)
{
for (var j = 0; j < maxcols; j++)
{
this.SetElement(insrow + i, inscol + j, matra[i, j], overwrite);
}
}
}
}
////////////////////////////////////
///
/// UPDATING PROCEDURES
///////// 4.5- UPDATE Cqw1 and Cqw2
/////////
public void Transform_Cq_to_Cqw(ChMatrix mCq, ChMatrix mCqw, ChBodyFrame mbody)
{
// if (mCq == null)
// return;
// translational part - not changed
mCqw.PasteClippedMatrix(mCq, 0, 0, mCq.GetRows(), 3, 0, 0);
// rotational part [Cq_w] = [Cq_q]*[Gl]'*1/4
int col, row, colres;
double sum;
// ChMatrixNM<IntInterface.Three, IntInterface.Four> mGl = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
ChFrame <double> .SetMatrix_Gl(ref mGl, mbody.GetCoord().rot);
for (colres = 0; colres < 3; colres++)
{
for (row = 0; row < (mCq.GetRows()); row++)
{
sum = 0;
for (col = 0; col < 4; col++)
{
sum += ((mCq.GetElement(row, col + 3)) * (mGl.matrix.GetElement(colres, col)));
}
mCqw.SetElement(row, colres + 3, sum * 0.25);
}
}
}
/// 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);
}
/// Using this function, one may go in the opposite direction of the FromUnknownsToVector()
/// function, i.e. one gives a vector with all the unknowns x={q,l} ordered into a column vector, and
/// the variables q and constr.multipliers l objects are updated according to these values.
/// NOTE!!! differently from FromUnknownsToVector(), which always works, this
/// function will fail if mvector does not match the amount and ordering of
/// the variable and constraint objects!!! (it is up to the user to check this!)
public virtual int FromVectorToUnknowns(ref ChMatrix mvector //< matrix which contains the entire vector x={q,l}
)
{
n_q = CountActiveVariables();
n_c = CountActiveConstraints();
// Debug.Assert((n_q + n_c) == mvector.GetRows());
// Debug.Assert(mvector.GetColumns() == 1);
// fetch from the first part of vector (x.q = q)
for (int iv = 0; iv < (int)vvariables.Count; iv++)
{
if (vvariables[iv].IsActive())
{
vvariables[iv].Get_qb().matrix.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!
for (int ic = 0; ic < (int)vconstraints.Count; ic++)
{
if (vconstraints[ic].IsActive())
{
vconstraints[ic].Set_l_i(-mvector[vconstraints[ic].GetOffset() + n_q]);
}
}
return(n_q + n_c);
}
/// Get the D diagonal of the Z system matrix, as a single column vector (it includes all the diagonal
/// masses of M, and all the diagonal E (-cfm) terms).
/// The Diagonal_vect must already have the size of n. of unknowns, otherwise it will be resized if necessary).
public virtual int BuildDiagonalVector(
ref 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 ChKblock objects , if any
// (This cannot be easily parallelized because of possible write concurrency).
for (int i = 0; i < (int)vstiffness.Count; i++)
{
vstiffness[i].DiagonalAdd(ref Diagonal_vect);
}
// Get the 'M' diagonal terms given by ChVariables objects
for (int iv = 0; iv < (int)vvariables.Count; iv++)
{
if (vvariables[iv].IsActive())
{
vvariables[iv].DiagonalAdd(ref Diagonal_vect, this.c_a);
}
}
// Get the 'E' diagonal terms (note the sign: E_i = -cfm_i )
for (int ic = 0; ic < (int)vconstraints.Count; ic++)
{
if (vconstraints[ic].IsActive())
{
Diagonal_vect[vconstraints[ic].GetOffset() + n_q] = -vconstraints[ic].Get_cfm_i();
}
}
return(n_q + n_c);
}
/// 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.
public override void MultiplyAndAdd(ref ChMatrix result,
ChMatrix vect,
double c_a)
{
// Debug.Assert(result.GetColumns() == 1 && vect.GetColumns() == 1);
result[this.offset] += c_a * m_inertia * vect[this.offset];
}
/// Scales a matrix, multiplying all element by all other elements of
/// matra (it is not the classical matrix multiplication!)
public void MatrScale(ChMatrix matra)
{
// Debug.Assert(matra.GetColumns() == columns && matra.GetRows() == rows);
for (int nel = 0; nel < rows * columns; ++nel)
{
ElementN(nel) *= matra.ElementN(nel);
}
}
/// Copy a matrix "matra" into this matrix. Note that
/// the destination matrix will be resized if necessary.
//template<class doubleB>
public void CopyFromMatrix(ChMatrix matra)
{
Resize(matra.GetRows(), matra.GetColumns());
for (int i = 0; i < rows * columns; ++i)
{
address[i] = matra.GetAddress()[i];
}
}
/// 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.
public override void DiagonalAdd(ref ChMatrix result, double c_a)
{
//Debug.Assert(result.GetColumns() == 1);
for (int i = 0; i < Mmass.matrix.GetRows(); i++)
{
result[this.offset + i] += c_a * Mmass.matrix[i, i];
}
}
public void MatrAdd(ChMatrix matra, ChMatrix matrb)
{
//assert(matra.GetColumns() == matrb.GetColumns() && matra.rows == matrb.GetRows());
//assert(this->columns == matrb.GetColumns() && this->rows == matrb.GetRows());
for (int nel = 0; nel < rows * columns; ++nel)
{
ElementN(nel) = (double)(matra.ElementN(nel) + matrb.ElementN(nel));
}
}
//
// MATH MEMBER FUNCTIONS.
// For speed reasons, sometimes size checking of operands is left to the user!
//
/// Subtract two matrices, and stores the result in "this" matrix: [this]=[A]-[B].
public void MatrSub(ChMatrix matra, ChMatrix matrb)
{
// Debug.Assert(matra.GetColumns() == matrb.GetColumns() && matra.rows == matrb.GetRows());
// Debug.Assert(this.columns == matrb.GetColumns() && this.rows == matrb.GetRows());
for (int nel = 0; nel < rows * columns; ++nel)
{
ElementN(nel) = (matra.ElementN(nel) - matrb.ElementN(nel));
}
}
/// 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.
public override void DiagonalAdd(ref ChMatrix result, double c_a)
{
// Debug.Assert(result.GetColumns() == 1);
result[this.offset + 0] += c_a * mass;
result[this.offset + 1] += c_a * mass;
result[this.offset + 2] += c_a * mass;
result[this.offset + 3] += c_a * inertia.nm.matrix[0, 0];
result[this.offset + 4] += c_a * inertia.nm.matrix[1, 1];
result[this.offset + 5] += c_a * inertia.nm.matrix[2, 2];
}
/// Same as #PasteClippedMatrix(), but with \a overwrite set to \c false.
/// The clipped portion of the source matrix will be summed to \a this matrix.
public virtual void PasteSumClippedMatrix(ChMatrix matra,
int cliprow,
int clipcol,
int nrows,
int ncolumns,
int insrow,
int inscol)
{
PasteClippedMatrix(matra, cliprow, clipcol, nrows, ncolumns, insrow, inscol, false);
}
//
// BOOKKEEPING
//
/// Paste a matrix "matra" into "this", inserting at location insrow-inscol
/// and performing a sum with the preexisting values.
public void PasteSumMatrix(ChMatrix matra, int insrow, int inscol)
{
for (int i = 0; i < matra.GetRows(); ++i)
{
for (int j = 0; j < matra.GetColumns(); ++j)
{
Element(i + insrow, j + inscol) += matra.Element(i, j);
}
}
}
/// Copy the transposed lower triangulat part of "matra" in the upper triangular
/// part of this matrix. (matra must be square)
/// Note that the destination matrix will be resized if necessary.
// _______ //
public void CopyTLwMatrix(ChMatrix matra) // |\ \ | //
{ // | \ ---> \this| //
Resize(matra.GetRows(), matra.GetColumns()); // |A' \ \ | //
for (int i = 0; i < matra.GetRows(); i++)
{ // |______\ \| //
for (int j = 0; j < matra.GetRows(); j++)
{
SetElement(i, j, matra.GetElement(j, i));
}
}
}
/// Copy the transposed upper triangular part of "matra" in the lower triangular
/// part of this matrix. (matra must be square)
/// Note that the destination matrix will be resized if necessary.
// _______ //
public void CopyTUpMatrix(ChMatrix matra) // \ | |\ //
{ // \ A'| ---> | \ //
Resize(matra.GetRows(), matra.GetColumns()); // \ | |this\ //
for (int i = 0; i < matra.GetRows(); i++)
{ // \| |______\ //
for (int j = 0; j < matra.GetRows(); j++)
{
SetElement(j, i, matra.GetElement(i, j));
}
}
}
/// Copy the transpose of matrix "matra" into this matrix. Note that
/// the destination matrix will be resized if necessary.
public void CopyFromMatrixT(ChMatrix matra)
{
Resize(matra.GetColumns(), matra.GetRows());
for (int i = 0; i < matra.GetRows(); ++i)
{
for (int j = 0; j < matra.GetColumns(); ++j)
{
SetElement(j, i, matra.Element(i, j));
}
}
}
/// Computes dot product between two column-matrices (vectors) with
/// same size. Returns a scalar value.
public double MatrDot(ChMatrix ma, ChMatrix mb)
{
//ssert(ma.GetColumns() == mb.GetColumns() && ma.GetRows() == mb.GetRows());
double tot = 0;
for (int i = 0; i < ma.GetRows(); ++i)
{
tot += (double)(ma.ElementN(i) * mb.ElementN(i));
}
return(tot);
}
public void MultiplyTandAdd(ChMatrix result, double l)
{
int off = variables.GetOffset();
if (variables.IsActive())
{
for (int i = 0; i < 6; i++)
{
result[off + i] += Cq.matrix.ElementN(i) * l;
}
}
}
/// Computes the product of the inverse mass matrix by a
/// vector, and increment result: result += [invMb]*vect
public override void Compute_inc_invMb_v(ref ChMatrix result, ChMatrix vect)
{
// Debug.Assert(vect.GetRows() == Get_ndof());
// Debug.Assert(result.GetRows() == Get_ndof());
// optimized unrolled operations
result[0] += inv_mass * vect[0];
result[1] += inv_mass * vect[1];
result[2] += inv_mass * vect[2];
result[3] += inv_inertia.nm.matrix[0, 0] * vect[3] + inv_inertia.nm.matrix[0, 1] * vect[4] + inv_inertia.nm.matrix[0, 2] * vect[5];
result[4] += inv_inertia.nm.matrix[1, 0] * vect[3] + inv_inertia.nm.matrix[1, 1] * vect[4] + inv_inertia.nm.matrix[1, 2] * vect[5];
result[5] += inv_inertia.nm.matrix[2, 0] * vect[3] + inv_inertia.nm.matrix[2, 1] * vect[4] + inv_inertia.nm.matrix[2, 2] * vect[5];
}
public void MultiplyAndAdd(ref double result, ChMatrix vect)
{
int off = variables.GetOffset();
if (variables.IsActive())
{
for (int i = 0; i < 6; i++)
{
result += vect[off + i] * Cq.matrix.ElementN(i);
}
}
}
/// Returns true if vector equals another vector, within a tolerance 'tol'
public bool Equals(ChMatrix other, double tol)
{
if ((other.GetColumns() != this.columns) || (other.GetRows() != this.rows))
{
return(false);
}
for (int nel = 0; nel < rows * columns; ++nel)
{
if (Mathfx.Abs(ElementN(nel) - other.ElementN(nel)) > tol)
{
return(false);
}
}
return(true);
}
/// Performs projection of constraint multipliers onto allowed set (in case
/// of bilateral constraints it does not affect multipliers, but for frictional
/// constraints, for example, it projects multipliers onto the friction cones)
/// Note! the 'l_i' data in the ChConstraints of the system descriptor are changed
/// by this operation (they get the value of 'multipliers' after the projection), so
/// it may happen that you need to backup them via FromConstraintToVector().
public virtual void ConstraintsProject(
ChMatrix multipliers //< matrix which contains the entire vector of 'l_i' multipliers to be projected
)
{
this.FromVectorToConstraints(multipliers);
for (int ic = 0; ic < (int)vconstraints.Count; ic++)
{
if (vconstraints[ic].IsActive())
{
vconstraints[ic].Project();
}
}
this.FromConstraintsToVector(multipliers, false);
}
/// 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.
public override void MultiplyAndAdd(ref ChMatrix result,
ChMatrix vect,
double c_a)
{
//Debug.Assert(result.GetColumns() == 1 && vect.GetColumns() == 1);
for (int i = 0; i < Mmass.matrix.GetRows(); i++)
{
double tot = 0;
for (int j = 0; j < Mmass.matrix.GetColumns(); j++)
{
tot += Mmass.matrix[i, j] * vect[this.offset + i];
}
result[this.offset + i] += c_a * tot;
}
}
//
// 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);
}
/// Paste a clipped portion of the matrix "matra" into "this",
/// inserting the clip (of size nrows, ncolumns) at the location insrow-inscol.
public void PasteClippedMatrix(ChMatrix matra,
int cliprow,
int clipcol,
int nrows,
int ncolumns,
int insrow,
int inscol)
{
for (int i = 0; i < nrows; ++i)
{
for (int j = 0; j < ncolumns; ++j)
{
Element(i + insrow, j + inscol) = matra.Element(i + cliprow, j + clipcol);
}
}
}
