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.cross2(n, rixn);
  rj.cross2(n, rjxn);

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

  // Calculate the penetration vector
  penetrationVec.setFrom(bj.position);
  penetrationVec.add2(rj, penetrationVec);
  penetrationVec.sub2(bi.position, penetrationVec);
  penetrationVec.sub2(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;
}