computeGq method

double computeGq()

Computes G*q, where q are the generalized body coordinates

Implementation

double computeGq() {
  final ga = jacobianElementA;
  final gb = jacobianElementB;
  final bi = this.bi;
  final bj = this.bj;
  final xi = bi.position;
  final xj = bj.position;
  return ga.spatial.dot(xi) + gb.spatial.dot(xj);
}