solve1 method

void solve1(
  1. BoneChain fk1,
  2. Vec2D worldTargetTranslation
)

Implementation

void solve1(BoneChain fk1, Vec2D worldTargetTranslation) {
  Mat2D iworld = fk1.parentWorldInverse;
  var pA = Vec2D();
  fk1.bone.getWorldTranslation(pA);
  var pBT = Vec2D.clone(worldTargetTranslation);

  // To target in worldspace
  Vec2D toTarget = Vec2D.subtract(Vec2D(), pBT, pA);
  // Note this is directional, hence not transformMat2d
  Vec2D toTargetLocal = Vec2D.transformMat2(Vec2D(), toTarget, iworld);
  double r = atan2(toTargetLocal[1], toTargetLocal[0]);

  constrainRotation(fk1, r);
  fk1.angle = r;
}