solveVelocityConstraints method

void solveVelocityConstraints(
  1. SolverData data
)
override

Internal

Implementation

void solveVelocityConstraints(final SolverData data) {
  Vector2 vA = data.velocities[_indexA].v;
  double wA = data.velocities[_indexA].w;
  Vector2 vB = data.velocities[_indexB].v;
  double wB = data.velocities[_indexB].w;

  double mA = _invMassA, mB = _invMassB;
  double iA = _invIA, iB = _invIB;

  final Vector2 temp = pool.popVec2();

  // Solve linear motor constraint.
  if (_enableMotor && _limitState != LimitState.EQUAL) {
    temp
      ..setFrom(vB)
      ..sub(vA);
    double Cdot = _axis.dot(temp) + _a2 * wB - _a1 * wA;
    double impulse = _motorMass * (_motorSpeed - Cdot);
    double oldImpulse = _motorImpulse;
    double maxImpulse = data.step.dt * _maxMotorForce;
    _motorImpulse = MathUtils.clampDouble(
        _motorImpulse + impulse, -maxImpulse, maxImpulse);
    impulse = _motorImpulse - oldImpulse;

    final Vector2 P = pool.popVec2();
    P
      ..setFrom(_axis)
      ..scale(impulse);
    double LA = impulse * _a1;
    double LB = impulse * _a2;

    vA.x -= mA * P.x;
    vA.y -= mA * P.y;
    wA -= iA * LA;

    vB.x += mB * P.x;
    vB.y += mB * P.y;
    wB += iB * LB;

    pool.pushVec2(1);
  }

  final Vector2 Cdot1 = pool.popVec2();
  temp
    ..setFrom(vB)
    ..sub(vA);
  Cdot1.x = _perp.dot(temp) + _s2 * wB - _s1 * wA;
  Cdot1.y = wB - wA;
  // System.out.println(Cdot1);

  if (_enableLimit && _limitState != LimitState.INACTIVE) {
    // Solve prismatic and limit constraint in block form.
    double Cdot2;
    temp
      ..setFrom(vB)
      ..sub(vA);
    Cdot2 = _axis.dot(temp) + _a2 * wB - _a1 * wA;

    final Vector3 Cdot = pool.popVec3();
    Cdot.setValues(Cdot1.x, Cdot1.y, Cdot2);

    final Vector3 f1 = pool.popVec3();
    final Vector3 df = pool.popVec3();

    f1.setFrom(_impulse);
    Matrix3.solve(_K, df, Cdot..negate());

    // Cdot.negateLocal(); not used anymore
    _impulse.add(df);

    if (_limitState == LimitState.AT_LOWER) {
      _impulse.z = Math.max(_impulse.z, 0.0);
    } else if (_limitState == LimitState.AT_UPPER) {
      _impulse.z = Math.min(_impulse.z, 0.0);
    }

    // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +
    // f1(1:2)
    final Vector2 b = pool.popVec2();
    final Vector2 f2r = pool.popVec2();

    temp
      ..setValues(_K.entry(0, 2), _K.entry(1, 2))
      ..scale(_impulse.z - f1.z);
    b
      ..setFrom(Cdot1)
      ..negate()
      ..sub(temp);

    Matrix3.solve2(_K, f2r, b);
    f2r.add(new Vector2(f1.x, f1.y));
    _impulse.x = f2r.x;
    _impulse.y = f2r.y;

    df
      ..setFrom(_impulse)
      ..sub(f1);

    final Vector2 P = pool.popVec2();
    temp
      ..setFrom(_axis)
      ..scale(df.z);
    P
      ..setFrom(_perp)
      ..scale(df.x)
      ..add(temp);

    double LA = df.x * _s1 + df.y + df.z * _a1;
    double LB = df.x * _s2 + df.y + df.z * _a2;

    vA.x -= mA * P.x;
    vA.y -= mA * P.y;
    wA -= iA * LA;

    vB.x += mB * P.x;
    vB.y += mB * P.y;
    wB += iB * LB;

    pool.pushVec2(3);
    pool.pushVec3(3);
  } else {
    // Limit is inactive, just solve the prismatic constraint in block form.
    final Vector2 df = pool.popVec2();
    Matrix3.solve2(_K, df, Cdot1..negate());
    Cdot1.negate();

    _impulse.x += df.x;
    _impulse.y += df.y;

    final Vector2 P = pool.popVec2();
    P
      ..setFrom(_perp)
      ..scale(df.x);
    double LA = df.x * _s1 + df.y;
    double LB = df.x * _s2 + df.y;

    vA.x -= mA * P.x;
    vA.y -= mA * P.y;
    wA -= iA * LA;

    vB.x += mB * P.x;
    vB.y += mB * P.y;
    wB += iB * LB;

    pool.pushVec2(2);
  }

  // data.velocities[_indexA].v.set(vA);
  data.velocities[_indexA].w = wA;
  // data.velocities[_indexB].v.set(vB);
  data.velocities[_indexB].w = wB;

  pool.pushVec2(2);
}