addToWlambda method

void addToWlambda(
  1. double deltalambda
)

Add constraint velocity to the bodies.

Implementation

void addToWlambda(double deltalambda) {
  final ga = jacobianElementA;
  final gb = jacobianElementB;
  final bi = this.bi;
  final bj = this.bj;
  final temp = _addToWlambdaTemp;

  // Add to linear velocity
  // v_lambda += inv(M) * delta_lamba * G
  bi.vlambda.addScaledVector(bi.invMassSolve * deltalambda, ga.spatial, bi.vlambda);
  bj.vlambda.addScaledVector(bj.invMassSolve * deltalambda, gb.spatial, bj.vlambda);

  // Add to angular velocity
  bi.invInertiaWorldSolve.vmult(ga.rotational, temp);
  bi.wlambda.addScaledVector(deltalambda, temp, bi.wlambda);

  bj.invInertiaWorldSolve.vmult(gb.rotational, temp);
  bj.wlambda.addScaledVector(deltalambda, temp, bj.wlambda);
}