/// Computes the time derivative of rotation matrix, mAdt.
public void Compute_Adt(ref ChMatrix33 <Real> mA_dt)
{
// [A_dt]=2[dFp/dt][Fm]'=2[Fp(q_dt)][Fm(q)]'
//ChMatrixNM<IntInterface.Three, IntInterface.Four> Fpdt = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0); //ChMatrixNM<IntInterface.Four, IntInterface.Four>.NMNULL3_4;
// ChMatrixNM<IntInterface.Three, IntInterface.Four> Fm = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
ChFrame <Real> .SetMatrix_Fp(ref Fpdt, coord_dt.rot);
ChFrame <Real> .SetMatrix_Fm(ref Fm, this.coord.rot);
mA_dt.nm.matrix.MatrMultiplyT(Fpdt.matrix, Fm.matrix);
mA_dt.nm.matrix.MatrScale(2);
}
/// Computes the 2nd time derivative of rotation matrix, mAdtdt.
public void Compute_Adtdt(ref ChMatrix33 <Real> mA_dtdt)
{
// [A_dtdt]=2[Fp(q_dtdt)][Fm(q)]'+2[Fp(q_dt)][Fm(q_dt)]'
// ChMatrixNM<IntInterface.Three, IntInterface.Four> ma = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
// ChMatrixNM<IntInterface.Three, IntInterface.Four> mb = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
// ChMatrix33<double> mr = new ChMatrix33<double>(0);
ChFrame <Real> .SetMatrix_Fp(ref ma, coord_dtdt.rot);
ChFrame <Real> .SetMatrix_Fm(ref mb, this.coord.rot);
mr.nm.matrix.MatrMultiplyT(ma.matrix, mb.matrix);
ChFrame <Real> .SetMatrix_Fp(ref ma, coord_dt.rot);
ChFrame <Real> .SetMatrix_Fm(ref mb, coord_dt.rot);
mA_dtdt.nm.matrix.MatrMultiplyT(ma.matrix, mb.matrix);
mA_dtdt.nm.matrix.MatrInc(mr.nm.matrix);
mA_dtdt.nm.matrix.MatrScale(2);
}
