////////////////////////////////////
///
/// 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);
}
}
}
/// 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));
}
}
}
/// 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);
}
}
}
}
/// 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];
}
}
//
// 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);
}
}
}
/// 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);
}
/// 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);
}
/// Copy constructor from all types of base matrices
public ChMatrixDynamic(ChMatrix msource)//:this()
{
matrix = new ChMatrix();
this.matrix.rows = msource.GetRows();
this.matrix.columns = msource.GetColumns();
this.matrix.address = new double[this.matrix.rows * this.matrix.columns];
/*this.address = (double*)Marshal.AllocHGlobal(this.rows * this.columns * sizeof(double));
* double[] array = new double[this.rows * this.columns];
* for (int i = 0; i < array.Length; i++)
* {
* this.address[i] = array[i];
* }*/
for (int i = 0; i < this.matrix.rows * this.matrix.columns; ++i)
{
this.matrix.address[i] = msource.GetAddress()[i];
}
}
/// Copy constructor from all types of base matrices /// Note! Assumes that the source matrix has one column only! There is no run-time check for the one-column sizing.
public ChVectorDynamic(ChMatrix msource)
{
// Debug.Assert(msource.GetColumns() == 1);
matrix = new ChMatrix();
this.matrix.rows = msource.GetRows();
this.matrix.columns = 1;
this.matrix.address = new double[this.matrix.rows];
/*this.address = (double*)Marshal.AllocHGlobal(this.rows * sizeof(double));
* double[] array = new double[this.rows];
* for (int i = 0; i < array.Length; i++)
* {
* this.address[i] = array[i];
* }*/
// ElementsCopy(this->address, msource.GetAddress(), this->rows);
for (int i = 0; i < this.matrix.rows; ++i)
{
this.matrix.address[i] = msource.GetAddress()[i];
}
}
/// Multiplies two matrices (the first is considered transposed): [this]=[A]'*[B]
/// Faster than doing A.MatrTranspose(); result.MatrMultiply(A,B);
public void MatrTMultiply(ChMatrix matra, ChMatrix matrb)
{
//Debug.Assert(matra.GetRows() == matrb.GetRows());
//Debug.Assert(this.rows == matra.GetColumns());
// Debug.Assert(this.columns == matrb.GetColumns());
int col, row, colres;
double sum;
for (colres = 0; colres < matrb.GetColumns(); ++colres)
{
for (row = 0; row < matra.GetColumns(); ++row)
{
sum = 0;
for (col = 0; col < (matra.GetRows()); ++col)
{
sum += (matra.Element(col, row) * matrb.Element(col, colres));
}
SetElement(row, colres, sum);
}
}
}