多関節の作り方

/**
 * 多関節ノード
 */
class Node
{
public:
    float x;   // 座標(X)
    float y;   // 座標(Y)
    float rad; // 回転角度
    this(float x=0, float y=0, float rad=0)
    {
        this.x   = x;
        this.y   = y;
        this.rad = rad;
    }
}
 
/**
 * 多関節用ベクトル
 */
struct JointVector
{
    float x; // X成分
    float y; // Y成分
    float n; // 内積の結果
    /**
     * 内積を求めて結果を保持する
     */
    void dot(float dx, float dy)
    {
        n = x*dx + y*dy;
    }
}
 
/**
 * 関節の移動(Inverse Kinematics)
 * @param nodes    関節([0]根元～[$-1]先端)
 * @param vRad     関節の回転角度
 * @param lRad     回転角度の限界値
 * @param distance 関節間の距離
 * @param x        目標座標(X)
 * @param y        目標座標(Y)
 */
void MoveJoints(Node[] nodes, float vRad, float lRad, 
    float distance, float x, float y)
{
    {
        float cos = Hell_cos(vRad);
        float sin = Hell_sin(vRad);
        // 前半の処理(回転角度を求める)
        Node nTip = nodes[$ - 1]; // 先端ノード
        // 先端⇒根元
        // リバースイテレート。先端ノードは処理しない
        for(int i = nodes.length - 1; i ＞= 0; i--)
        {
            Node n = nodes[i];
            // 関節から自機へのベクトルの計算
            float dx = x - n.x;
            float dy = y - n.y;
            // 関節から先端へのベクトルと内積の計算
            JointVector jNode;
            jNode.x = nTip.x - n.x;
            jNode.y = nTip.y - n.y;
            jNode.dot(dx, dy);
             
            // 右回りのベクトルの計算
            JointVector jRight;
            if(n.rad + vRad <= lRad)
            {
                jRight.x = cos*jNode.x - sin*jNode.y;
                jRight.y = sin*jNode.x + cos*jNode.y;
                jRight.dot(dx, dy);
            }
            else
            {
                // 限界を超えたので回せない
                jRight.n = jNode.n;
            }
             
            // 左回りのベクトルの計算
            JointVector jLeft;
            if(n.rad - vRad >= -lRad)
            {
                jLeft.x =  cos*jNode.x + sin*jNode.y;
                jLeft.y = -sin*jNode.x + cos*jNode.y;
                jLeft.dot(dx, dy);
            }
            else
            {
                // 限界を超えたので回せない
                jLeft.n = jNode.n;
            }
             
            // 回転方向の選択
            // 内積を比較して、回転を３通りのなかから選ぶ
            // 先端を回転させて、新しい先端の位置を求める
            if(jRight.n ＞ jNode.n && jRight.n ＞ jLeft.n)
            {
                // 右回り
                n.rad  += vRad;
                nTip.x =  n.x + jRight.x;
                nTip.y =  n.y + jRight.y;
            }
            if(jLeft.n ＞ jNode.n && jLeft.n ＞ jRight.n)
            {
                // 左回り
                n.rad  -= vRad;
                nTip.x =  n.x + jLeft.x;
                nTip.y =  n.y + jLeft.y;
            }
        }
    }
     
    // 後半の処理(座標を決める)
    float px = distance;
    float py = 0;
    // 根元⇒先端
    // 根元は移動しない
    for(int i = 1; i ＜ nodes.length; i++)
    {
        Node n1 = nodes[i-1];
        Node n2 = nodes[i];
        float cos = Hell_cos(n1.rad);
        float sin = Hell_sin(n1.rad);
        float dx  = cos*px - sin*py;
        float dy  = sin*px + cos*py;
        n2.x = n1.x + dx;
        n2.y = n1.y + dy;
        px   = dx;
        py   = dy;
    }
}