getJointSpeed method

double getJointSpeed()

Get the current joint translation, usually in meters.

Implementation

double getJointSpeed() {
  Body bA = _bodyA;
  Body bB = _bodyB;

  Vector2 temp = pool.popVec2();
  Vector2 rA = pool.popVec2();
  Vector2 rB = pool.popVec2();
  Vector2 p1 = pool.popVec2();
  Vector2 p2 = pool.popVec2();
  Vector2 d = pool.popVec2();
  Vector2 axis = pool.popVec2();
  Vector2 temp2 = pool.popVec2();
  Vector2 temp3 = pool.popVec2();

  temp
    ..setFrom(_localAnchorA)
    ..sub(bA._sweep.localCenter);
  Rot.mulToOutUnsafe(bA._transform.q, temp, rA);

  temp
    ..setFrom(_localAnchorB)
    ..sub(bB._sweep.localCenter);
  Rot.mulToOutUnsafe(bB._transform.q, temp, rB);

  p1
    ..setFrom(bA._sweep.c)
    ..add(rA);
  p2
    ..setFrom(bB._sweep.c)
    ..add(rB);

  d
    ..setFrom(p2)
    ..sub(p1);
  Rot.mulToOutUnsafe(bA._transform.q, _localXAxisA, axis);

  Vector2 vA = bA._linearVelocity;
  Vector2 vB = bB._linearVelocity;
  double wA = bA._angularVelocity;
  double wB = bB._angularVelocity;

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

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

  pool.pushVec2(9);

  return speed;
}