computeGiMf method

double computeGiMf()

Computes G*inv(M)*f, where M is the mass matrix with diagonal blocks for each body, and f are the forces on the bodies.

Implementation

double computeGiMf() {
  final ga = jacobianElementA;
  final gb = jacobianElementB;
  final bi = this.bi;
  final bj = this.bj;
  final fi = bi.force;
  final ti = bi.torque;
  final fj = bj.force;
  final tj = bj.torque;
  final invMassi = bi.invMassSolve;
  final invMassj = bj.invMassSolve;

  fi.scale(invMassi, _iMfi);
  fj.scale(invMassj, _iMfj);

  bi.invInertiaWorldSolve.vmult(ti, _invIiVmultTaui);
  bj.invInertiaWorldSolve.vmult(tj, _invIjVmultTauj);

  return ga.multiplyVectors(_iMfi, _invIiVmultTaui) + gb.multiplyVectors(_iMfj, _invIjVmultTauj);
}