getJointSpeed method

Implementation

double getJointSpeed() {
  final temp = Vector2.zero();
  final rA = Vector2.zero();
  final rB = Vector2.zero();
  final p1 = Vector2.zero();
  final p2 = Vector2.zero();
  final d = Vector2.zero();
  final axis = Vector2.zero();
  final temp2 = Vector2.zero();
  final temp3 = Vector2.zero();

  temp
    ..setFrom(localAnchorA)
    ..sub(bodyA.sweep.localCenter);
  rA.setFrom(Rot.mulVec2(bodyA.transform.q, temp));

  temp
    ..setFrom(localAnchorB)
    ..sub(bodyB.sweep.localCenter);
  rB.setFrom(Rot.mulVec2(bodyB.transform.q, temp));

  p1
    ..setFrom(bodyA.sweep.c)
    ..add(rA);
  p2
    ..setFrom(bodyB.sweep.c)
    ..add(rB);

  d
    ..setFrom(p2)
    ..sub(p1);
  axis.setFrom(Rot.mulVec2(bodyA.transform.q, localXAxisA));

  final vA = bodyA.linearVelocity;
  final vB = bodyB.linearVelocity;
  final wA = bodyA.angularVelocity;
  final wB = bodyB.angularVelocity;

  axis.scaleOrthogonalInto(wA, temp);
  rB.scaleOrthogonalInto(wB, temp2);
  rA.scaleOrthogonalInto(wA, temp3);

  temp2
    ..add(vB)
    ..sub(vA)
    ..sub(temp3);
  final speed = d.dot(temp) + axis.dot(temp2);

  return speed;
}