solve2 method

Implementation

void solve2(BoneChain fk1, BoneChain fk2, Vec2D worldTargetTranslation) {
  ActorBone b1 = fk1.bone;
  ActorBone b2 = fk2.bone;
  BoneChain firstChild = _fkChain[fk1.index + 1];

  Mat2D iworld = fk1.parentWorldInverse;

  Vec2D pA = b1.getWorldTranslation(Vec2D());
  Vec2D pC = firstChild.bone.getWorldTranslation(Vec2D());
  Vec2D pB = b2.getTipWorldTranslation(Vec2D());

  Vec2D pBT = Vec2D.clone(worldTargetTranslation);

  pA = Vec2D.transformMat2D(pA, pA, iworld);
  pC = Vec2D.transformMat2D(pC, pC, iworld);
  pB = Vec2D.transformMat2D(pB, pB, iworld);
  pBT = Vec2D.transformMat2D(pBT, pBT, iworld);

  // http://mathworld.wolfram.com/LawofCosines.html
  Vec2D av = Vec2D.subtract(Vec2D(), pB, pC);
  double a = Vec2D.length(av);

  Vec2D bv = Vec2D.subtract(Vec2D(), pC, pA);
  double b = Vec2D.length(bv);

  Vec2D cv = Vec2D.subtract(Vec2D(), pBT, pA);
  double c = Vec2D.length(cv);

  double A = acos(max(-1, min(1, (-a * a + b * b + c * c) / (2 * b * c))));
  double C = acos(max(-1, min(1, (a * a + b * b - c * c) / (2 * a * b))));

  double r1, r2;
  if (b2.parent != b1) {
    BoneChain secondChild = _fkChain[fk1.index + 2];

    Mat2D secondChildWorldInverse = secondChild.parentWorldInverse;

    pC = firstChild.bone.getWorldTranslation(Vec2D());
    pB = b2.getTipWorldTranslation(Vec2D());

    Vec2D avec = Vec2D.subtract(Vec2D(), pB, pC);
    Vec2D avLocal =
        Vec2D.transformMat2(Vec2D(), avec, secondChildWorldInverse);
    double angleCorrection = -atan2(avLocal[1], avLocal[0]);

    if (_invertDirection) {
      r1 = atan2(cv[1], cv[0]) - A;
      r2 = -C + pi + angleCorrection;
    } else {
      r1 = A + atan2(cv[1], cv[0]);
      r2 = C - pi + angleCorrection;
    }
  } else if (_invertDirection) {
    r1 = atan2(cv[1], cv[0]) - A;
    r2 = -C + pi;
  } else {
    r1 = A + atan2(cv[1], cv[0]);
    r2 = C - pi;
  }

  constrainRotation(fk1, r1);
  constrainRotation(firstChild, r2);
  if (firstChild != fk2) {
    ActorBone bone = fk2.bone;
    Mat2D.multiply(
        bone.worldTransform, bone.parent!.worldTransform, bone.transform);
  }

  // Simple storage, need this for interpolation.
  fk1.angle = r1;
  firstChild.angle = r2;
}