////////////////////////////////////
///
/// 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);
}
}
}
/// Get the link coordinate system in absolute reference.
/// This represents the 'main' reference of the link: reaction forces
/// and reaction torques are expressed in this coordinate system.
/// Child classes should implement this.
public override ChCoordsys GetLinkAbsoluteCoords()
{
return(ChCoordsys.BitShiftRight(GetLinkRelativeCoords(), Body2.GetCoord()));
}