computeB method

@override double computeB(double h)

Computes the right hand side of the SPOOK equation

Implementation

@override
double computeB(double h) {
  final a = this.a;
  final b = this.b;
  final bi = this.bi;
  final bj = this.bj;
  final ri = this.ri;
  final rj = this.rj;
  final rixn = _contactEquationComputeBTemp1;
  final rjxn = _contactEquationComputeBTemp2;
  final vi = bi.velocity;
  final wi = bi.angularVelocity;
  final vj = bj.velocity;
  final wj = bj.angularVelocity;
  final penetrationVec = _contactEquationComputeBTemp3;
  final ga = jacobianElementA;
  final gb = jacobianElementB;
  final n = ni;

  // Caluclate cross products
  ri.cross(n, rixn);
  rj.cross(n, rjxn);

  n.negate(ga.spatial);
  rixn.negate(ga.rotational);
  gb.spatial.copy(n);
  gb.rotational.copy(rjxn);

  // Calculate the penetration vector
  penetrationVec.copy(bj.position);
  penetrationVec.vadd(rj, penetrationVec);
  penetrationVec.vsub(bi.position, penetrationVec);
  penetrationVec.vsub(ri, penetrationVec);

  final g = n.dot(penetrationVec);

  // Compute iteration
  final ePlusOne = restitution + 1;
  final gw = ePlusOne * vj.dot(n) - ePlusOne * vi.dot(n) + wj.dot(rjxn) - wi.dot(rixn);
  final giMf = computeGiMf();

  final B = -g * a - gw * b - h * giMf;

  return B;
}