js/poseAnimator.js

const monitor = [];
function eachMonitor(p, f) {
  monitor.forEach((m) => {
    if (m[0] === p[0] && m[1] === p[1]) {
      f(m);
    }
  });
}
function isMonitored(p) {
  let result = false;
  eachMonitor(p, (m) => {
    result = true;
  });
  return result;
}


// 2D projection
function worldToLocal(pointA, pointB, pointC) {
  const vectorAB = [pointB[0] - pointA[0], pointB[1] - pointA[1]];
  const vectorAC = [pointC[0] - pointA[0], pointC[1] - pointA[1]];
  const dot1 = dot2D(vectorAB, vectorAC);
  const dot2 = dot2D(vectorAB, vectorAB);
  const localX = dot1 / dot2;
  const pointD = [pointA[0] + localX * vectorAB[0], pointA[1] + localX * vectorAB[1]];

  // localY = distance from pointC to pointD
  const vectorCD = [pointD[0] - pointC[0], pointD[1] - pointC[1]];
  let localY = Math.sqrt(dot2D(vectorCD, vectorCD));

  // if pointC is on the right side of vectorAB, localY is negative
  const cross = cross2D(vectorAB, vectorAC);
  if (0 < cross) {
      localY = -localY;
  }

  return [localX, localY];
}

function distPointFromSeg(pointA, pointB, pointC, lx, ly) {
  if (lx < 0) {
    const vectorAC = [pointC[0] - pointA[0], pointC[1] - pointA[1]];
    return Math.sqrt(dot2D(vectorAC, vectorAC));
  }
  if (1.0 < lx) {
    const vectorBC = [pointC[0] - pointB[0], pointC[1] - pointB[1]];
    return Math.sqrt(dot2D(vectorBC, vectorBC));
  }
  return Math.abs(ly);
}

function localToWorld([pointA, pointB], pointC) { // C is local coordinate
  const vectorAB = [pointB[0] - pointA[0], pointB[1] - pointA[1]];
  // pointD = pointA + C.x * vectorAB(垂線との交点)
  const pointD = [pointA[0] + pointC[0] * vectorAB[0], pointA[1] + pointC[0] * vectorAB[1]];
  const v = perpendicular2D(vectorAB);
  const vLength = Math.sqrt(dot2D(v, v));
  if (vLength < 0.0001) { return pointD; }

  const newLength = pointC[1];
  const factor = newLength / vLength;
  return [pointD[0] + factor * v[0], pointD[1] + factor * v[1]];
}

function makeEdgeSegments(pose) {
  return limbSeq.map((segment) => {
    const p0 = pose[segment[0]];
    const p1 = pose[segment[1]];
    return [p0, p1];
  });
}

function makeNormalizeEdgeSegments(edges) {
  return edges.map((edge) => normalize2D([edge[1][0] - edge[0][0], edge[1][1] - edge[0][1]]));
}

const fieldDepth = 5;
const FO0i = 0;
const FO0t = 1;
const FO0u = 2;
const FO0d = 3;

// c current
// p prev
// n next
// 0 primary
// 1 secondary
// e edge
// s segment
// i in
// o out
// t time
// l local
// q point
// m main
// d distance
// w weight

function calcJointRange(
  edgeSegments, normalizedEdgeSegments, 
  edgeSegmentsForSideDecision,
  ep, ec, m) {
  // parent, child
  const np = normalizedEdgeSegments[ep];
  const nc = normalizedEdgeSegments[ec];
  let pnp = perpendicular2D(np);
  let pnc = perpendicular2D(nc);
  let side = getSide(edgeSegmentsForSideDecision[ep][0], edgeSegmentsForSideDecision[ep][1], edgeSegmentsForSideDecision[ec][1], m);
  if (side < 0) {
    pnp = reverse2D(pnp);
    pnc = reverse2D(pnc);
  }
  return [pnp, pnc];
}

function calcJointRangeWithOrder(oldEdgeSegments, oldNormalizedEdgeSegments, newEdgeSegments, e0, m, order) {
  if (order < 0) {
    const e1 = findPrevEdgeIndex(e0);
    if (e1 < 0) { return null; }
    const [pn1, pn0] = calcJointRange(oldEdgeSegments, oldNormalizedEdgeSegments, newEdgeSegments,e1, e0, m);
    const oldPivot = oldEdgeSegments[e0][0];
    const newPivot = newEdgeSegments[e0][0];
    return [pn0, pn1, oldPivot, newPivot, e1];
  } else if (0 < order) {
    const e1 = findNextEdgeIndex(e0);
    if (e1 < 0) { return null; }
    const [pn0, pn1] = calcJointRange(oldEdgeSegments, oldNormalizedEdgeSegments, newEdgeSegments, e0, e1, m);
    const oldPivot = oldEdgeSegments[e0][1];
    const newPivot = newEdgeSegments[e0][1];
    return [pn0, pn1, oldPivot, newPivot, e1];
  } else {
    return null;
  }
}

function buildWeightMap(size, pose) {
  const threshold = 512;

  const [w, h] = size;

  // この状態で、canvasに対して、  
  // 同じ大きさの3次元配列[x,y,z]を作成する
  // x,yは画像の2次元座標

  // 各Pixelに対して、
  // そのPixelに最も近いエッジのindexを[0]に、
  // そのときのエッジ座標系におけるtを[1]に記録する
  // どの骨格とも近くないものは[0]=255とする

  const field = new Float32Array(w * h * fieldDepth)
  function fieldIndex(x, y) {
    return (y * w + x) * fieldDepth;
  }
  
  const edgeSegments = makeEdgeSegments(pose);
  const normalizedEdgeSegments = makeNormalizeEdgeSegments(edgeSegments);

  // 各Pixelに対して、一番近いエッジを探す
  // またそのエッジ座標系におけるtを保存する
  for (let y = 0 ; y < h ; y++) {
    for (let x = 0 ; x < w ; x++) {
      const m = [x, y];
      let minDist = threshold;
      const fidx = fieldIndex(x, y);
      field[fidx+FO0i] = -1;
      for (let i = 0 ; i < limbSeq.length ; i++) {
        // 線分から点への距離を計算
        const s = edgeSegments[i];
        const lq = worldToLocal(s[0], s[1], m);
        const dist = distPointFromSeg(s[0], s[1], m, lq[0], lq[1]);
        if (dist < minDist) {
          minDist = dist;
          field[fidx+FO0i] = i;
          field[fidx+FO0t] = lq[0];
          field[fidx+FO0u] = lq[1];
          field[fidx+FO0d] = dist;
        }
      }
    }
  }

  return field;
}

function renderField(canvas2, size, field) {
  function blend(c0, w0, c1, w1) {
    const r = (c0[0] * w0 + c1[0] * w1);
    const g = (c0[1] * w0 + c1[1] * w1);
    const b = (c0[2] * w0 + c1[2] * w1);
    const a = (c0[3] * w0 + c1[3] * w1);
    return [r, g, b, a];
  }

  const [w, h] = size;
  function fieldIndex(x, y) {
    return (y * w + x) * fieldDepth;
  }

  canvas2.width = w;
  canvas2.height = h;
  const ctx2 = canvas2.getContext('2d');

  bindings = ctx2.createImageData(w, h);
  const data = bindings.data;
  for (let y = 0 ; y < h ; y++) {
    for (let x = 0 ; x < w ; x++) {
      const fidx = fieldIndex(x, y);
      const oi = (y * w + x) * 4;

      let c = [0, 0, 0, 64];

      const e0 = field[fidx+FO0i];
      if (0 <= e0) { 
        c = colors[e0];
        c[3] = 255;
        // c[3] = 255 - field[fidx+FO0d] * 4;
      }
      data[oi + 0] = c[0];
      data[oi + 1] = c[1];
      data[oi + 2] = c[2];
      data[oi + 3] = c[3];
    }
  }
  ctx2.putImageData(bindings, 0, 0);

  return field;
}

function makeImageDataFromPicture(canvas3, size, picture) {
  const [w, h] = size;
  canvas3.width = w;
  canvas3.height = h;

  const ctx3 = canvas3.getContext("2d");
  // ctx3.drawImage(picture, sampleOffset[0], sampleOffset[1]);
  ctx3.drawImage(picture, 0, 0);

  return new ImageData(new Uint8ClampedArray(ctx3.getImageData(0, 0, w, h).data), w, h);
}

function animatePicture(canvas4, size, oldPose, newPose, srcImageData, initialPoseField) {
  const srcData = srcImageData.data;

  const [w, h] = size;

  const ctx4 = canvas4.getContext("2d");
  canvas4.width = w;
  canvas4.height = h;
  const dstCtx = ctx4;
  const dstImageData = dstCtx.createImageData(w, h);
  const dstData = dstImageData.data;

  const field = buildWeightMap(size, newPose);
  const canvas2 = document.getElementById("canvas2");
  // renderField(canvas2, size, field);
  drawBodyPoseTo(canvas2.getContext("2d"), [newPose]);

  function fieldIndex(x, y) {
    return (y * w + x) * fieldDepth;
  }

  const oldEdgeSegments = makeEdgeSegments(oldPose);
  const oldNormalizedEdgeSegments = makeNormalizeEdgeSegments(oldEdgeSegments);
  const newEdgeSegments = makeEdgeSegments(newPose);

  function oldEdgePoint(edgeIndex, p) {
    const v = oldEdgeSegments[edgeIndex];
    return localToWorld(v, p);
  }

  function newEdgePoint(edgeIndex, p) {
    const v = newEdgeSegments[edgeIndex];
    return localToWorld(v, p);
  }

  debugLines = [];

  for (let y = 0 ; y < h ; y++) {
    for (let x = 0 ; x < w ; x++) {
      // 各点に対して、fieldから近隣エッジ座標系でのローカル座標を取得する
      const oi = (y * w + x) * 4;

      const fidx = fieldIndex(x, y);
      const e0 = field[fidx+FO0i];
      if (e0 < 0) { 
        dstData[oi + 0] = 0;
        dstData[oi + 1] = 0;
        dstData[oi + 2] = 0;
        dstData[oi + 3] = 255;
        continue; 
      }

      const lq = [field[fidx+FO0t], field[fidx+FO0u]];
      let e1 = -1;
      let order = 0; // 1 == forward, -1 == backward
      if (lq[0] < 0.5) {
        e1 = findPrevEdgeIndex(e0);
      } else if (0.5 <= lq[0]) {
        e1 = findNextEdgeIndex(e0);
      }
      if (0 <= e1) {
        if (lq[0] < 0) {
          order = -1;
        } else if (1 < lq[0]) {
          order = 1;
        }
      }

      let iq = oldEdgePoint(e0, lq);

      if (0 != order) {
        // サイド判定は新ポーズで行う
        const mNew = [x,y];
        const jointRange = calcJointRangeWithOrder(
          oldEdgeSegments, oldNormalizedEdgeSegments, 
          newEdgeSegments,
          e0, mNew, order);
        const [pn0, pn1, oldPivot, newPivot, _] = jointRange;

        const pivotMNew = [x - newPivot[0], y - newPivot[1]];
        const [w0, w1]= [angleBetween(pn0, pivotMNew), angleBetween(pivotMNew, pn1)];

        let d0 = field[fidx+FO0d];
        let mt = slerp2D(pn0, pn1, w0 / (w0 + w1));
        iq = [oldPivot[0] + mt[0] * d0, oldPivot[1] + mt[1] * d0];
        eachMonitor([x,y], () => {
          console.log('animate', x, y, ix, iy, e1, w0, w1, pn0, pn1);
          debugLines.push([oldPivot, pn0]);
          debugLines.push([oldPivot, pn1]);
        });
      }

      iq = [Math.round(iq[0]), Math.round(iq[1])];

      var initialOwner = initialPoseField[fieldIndex(iq[0], iq[1]) + FO0i];
      if (0 <= initialOwner && initialOwner != e0 && initialOwner != e1) {
        // ほかの領土
        dstData[oi + 0] = 0;
        dstData[oi + 1] = 0;
        dstData[oi + 2] = 0;
        dstData[oi + 3] = 0;
      } else {
        // canvas3の[ix,iy]の色を取得して、canvas4の[x,y]に描画する
        const si = (iq[1] * w + iq[0]) * 4;
        dstData[oi + 0] = srcData[si + 0];
        dstData[oi + 1] = srcData[si + 1];
        dstData[oi + 2] = srcData[si + 2];
        dstData[oi + 3] = srcData[si + 3];
      }
    }
  }

  dstCtx.putImageData(dstImageData, 0, 0);
}

function handleMicroscopeMouseMove(e) {
  const [x,y] = mouseCursor;

  const imageData = ctx.getImageData(x - 2, y - 2, 5, 5);
  const data = imageData.data;

  const microscope = document.getElementById('microscope');
  const ctx2 = microscope.getContext('2d');
  ctx2.clearRect(0, 0, microscope.width, microscope.height);
  for (let i = 0 ; i < 5 ; i++) {
    for (let j = 0 ; j < 5 ; j++) {
      const idx = (i * 5 + j) * 4;
      ctx2.fillStyle = `rgba(${data[idx]}, ${data[idx+1]}, ${data[idx+2]}, ${data[idx+3]})`;
      ctx2.fillRect(j * 10, i * 10, 10, 10);
    }
  }
}

function initMicroscope() {
  // canvas.addEventListener('mousemove', handleMicroscopeMouseMove);
  // poseData[0][6] = [329, 148];
  // poseData[0][7] = [329, 194];
}