computeGiMGt method

double computeGiMGt()

Computes G*inv(M)*G'

Implementation

double computeGiMGt() {
  final ga = jacobianElementA;
  final gb = jacobianElementB;
  final bi = this.bi;
  final bj = this.bj;
  final invMassi = bi.invMassSolve;
  final invMassj = bj.invMassSolve;
  final invIi = bi.invInertiaWorldSolve;
  final invIj = bj.invInertiaWorldSolve;
  double result = invMassi + invMassj;
  final tmp = Vec3();
  invIi.vmult(ga.rotational, tmp);
  result += tmp.dot(ga.rotational);

  invIj.vmult(gb.rotational, tmp);
  result += tmp.dot(gb.rotational);

  return result;
}