////////////////////////////////////
///
/// 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);
}
}
}
// Substitution using existing LU factorization
public void Solve_LU(ChMatrix b, ref ChMatrix x)
{
//Debug.Assert(m_num_rows == b.GetRows());
//Debug.Assert(m_num_cols == x.GetRows());
// BACKWARD substitution - L
double xlast = b.GetElement(m_pindices[0], 0);
x.SetElement(0, 0, xlast);
for (int k = 1; k < m_num_rows; k++)
{
double sum = 0;
ChMelement rowel = GetElarrayMel(k);
for (; (rowel != null && rowel.col < k); rowel = rowel.next)
{
sum += rowel.val * (x.GetElement(rowel.col, 0));
}
double val = (b.GetElement(m_pindices[k], 0) - sum);
x.SetElement(k, 0, val);
}
// BACKWARD substitution - U
xlast = x.GetElement(m_num_rows - 1, 0) / GetElement(m_num_rows - 1, m_num_rows - 1);
x.SetElement(m_num_rows - 1, 0, xlast);
for (int k = m_num_rows - 2; k >= 0; k--)
{
double sum = 0;
ChMelement rowel = GetElarrayMel(k);
for (; rowel != null; rowel = rowel.next)
{
if (rowel.col >= k + 1)
{
sum += rowel.val * (x.GetElement(rowel.col, 0));
}
}
double val = (x.GetElement(k, 0) - sum) / GetElement(k, k);
x.SetElement(k, 0, val);
}
}
/// 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));
}
}
}
/// Paste a clipped portion of the given matrix into \a this sparse matrix at position (\a insrow, \a inscol).
/// So the clipped portion \a matra[cliprow : cliprow + nrows][[clipcol : clipcol + ncolumns]]
/// will be copied into \a this[insrow : insrow + nrows][[inscol : inscol + ncolumns]]
/// \param[in] matra The source matrix that will be copied;
/// \param[in] cliprow The row index of the first element of source matrix that will be copied;
/// \param[in] clipcol The column index of the first element of source matrix that will be copied;
/// \param[in] nrows The number of rows that will be copied;
/// \param[in] ncolumns The number of columns 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.
public virtual void PasteClippedMatrix(ChMatrix matra,
int cliprow,
int clipcol,
int nrows,
int ncolumns,
int insrow,
int inscol,
bool overwrite = true)
{
for (var i = 0; i < nrows; ++i)
{
for (var j = 0; j < ncolumns; ++j)
{
this.SetElement(insrow + i, inscol + j, matra.GetElement(i + cliprow, j + clipcol), overwrite);
}
}
}