//////////////////////////////////// /// /// 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); } } }