daruma3940の日記

理解や文章に間違い等あればどんなことでもご指摘お願いします

スプライン補完

シューティングで敵に複雑な動きをさせたい時ありますよね...ありませんか?
こんな感じで
f:id:daruma3940:20180724002732p:plain
ぐるりんっ


f:id:daruma3940:20180724002755p:plain

x(t),とy(t)を求めてやればこのように動かすための関数が求まるため、このような動きを実現できることがわかります。
今回はスプライン補完を使ってこのような曲線を求めてみましょう

www.yamamo10.jp
3次スプライン補完の場合
(x_i,t_i)から(x_i+1,t_i+1)までの範囲を1次微分と2次微分が連続的になるようにしながら3次多項式で近似してやろうという感じです
xj(t)=aj(x-xj)**3+bj(x-xj)**2+cj(x-xj)+dj
の係数a,b,c,dはxj''(tj)を使って表現できるので
xj''(tj)を求めればよくそれは連立方程式を解けばよいという感じです

実装してみたらこんな感じで動くようになりました
f:id:daruma3940:20180724003523g:plain

コードを貼ろうかと思ったんですけどライセンスとかややこしいですよね...ライセンスどうするか決めたら貼ります
(2018,8/17)ソースは自由に使ってもらって構いませんがこれを利用したことによるいかなる損害や不利益に対する責任や賠償は一切とらないものとします。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;


static public class Fundation  {


  

    delegate float D2(int i);


    public static void MakeSprine(float[] ts, float[] xs, float[] ys,
             ref float[] ax, ref float[] bx, ref float[] cx, ref float[] dx,
               ref float[] ay, ref float[] by, ref float[] cy, ref float[] dy
            )
    {
        D2 h = (j) => { return ts[j + 1] - ts[j]; };
        D2 vy = (j) => { return 6.0f* ((ys[j + 1] - ys[j]) / h(j) - (ys[j] - ys[j - 1]) / h(j - 1)); };
        D2 vx = (j) => { return 6.0f * ((xs[j + 1] - xs[j]) / h(j) - (xs[j] - xs[j - 1]) / h(j - 1)); };
        //float[] xs;
        //float[] ys;
        //float[] ts;
        float[,] Ax_, Ay_;
        float[] bx_, by_;

        int N = ts.Length - 1;
        Debug.Assert(N == xs.Length - 1 && N == ys.Length - 1);
        //xについて
        Ax_ = new float[N - 1, N - 1];
        bx_ = new float[N - 1];
        for (int i = 0; i < N - 1; i++)
        {
            //Console.WriteLine(i);
            Ax_[i, i] = 2 * (h(i) + h(i + 1));
            if (i - 1 >= 0) { Ax_[i, i - 1] = (h(i)); }
            if (i + 1 < N - 1) { Ax_[i, i + 1] = (h(i + 1)); }

            bx_[i] = vx(i + 1);
        }
        Ay_ = new float[N - 1, N - 1];
        by_ = new float[N - 1];
        for (int i = 0; i < N - 1; i++)
        {
           // Console.WriteLine(i);
            Ay_[i, i] = 2 * (h(i) + h(i + 1));
            if (i - 1 >= 0) { Ay_[i, i - 1] = (h(i)); }
            if (i + 1 < N - 1) { Ay_[i, i + 1] = (h(i + 1)); }

            by_[i] = vy(i + 1);
        }
        //ちぇっく



        List<float> us = Gauss_Jordan(Ax_, bx_);
        //u0=0、un=0の条件を用いなければならない
        us.Insert(0, 0);
        us.Add(0);

        ax = new float[N]; bx = new float[N]; cx = new float[N]; dx = new float[N];
        ay = new float[N]; by = new float[N]; cy = new float[N]; dy = new float[N];


        for (int i = 0; i < N; i++)
        {
            bx[i] = us[i] / 2.0f;
            dx[i] = xs[i];
            ax[i] = (us[i + 1] - us[i]) / (6f * (ts[i + 1] - ts[i]));
            cx[i] = (xs[i + 1] - xs[i]) / (ts[i + 1] - ts[i]) - (ts[i + 1] - ts[i]) * (2f * us[i] + us[i + 1]) / 6.0f;
        }
        //for (int i = 0; i < N; i++)
        //{
        //    Console.WriteLine(ax[i].ToString() + " " + bx[i].ToString() + " " + cx[i].ToString() + " " + dx[i].ToString());
        //}
        us = Gauss_Jordan(Ay_, by_);
        us.Insert(0, 0);
        us.Add(0);
        for (int i = 0; i < N; i++)
        {
            by[i] = us[i] / 2.0f;
            dy[i] = ys[i];
            ay[i] = (us[i + 1] - us[i]) / (6f * (ts[i + 1] - ts[i]));
            cy[i] = (ys[i + 1] - ys[i]) / (ts[i + 1] - ts[i]) - (ts[i + 1] - ts[i]) * (2f * us[i] + us[i + 1]) / 6.0f;
        }
        //for (int i = 0; i < N; i++)
        //{
        //    Console.WriteLine(ay[i].ToString() + " " + by[i].ToString() + " " + cy[i].ToString() + " " + dy[i].ToString());
        //}
    }

    //ガウスジョルダン 行列計算
    static List<float> Gauss_Jordan(float[,] A, float[] b)
    {

        float EPS = 1e-10f;
        int n = b.Length;
        //Console.Write(n.ToString());
        //Debug.Assert(n==A[0);
        Debug.Assert(n == b.Length);

        //Aの後ろにbをつけたのをBとする
        float[,] B = new float[n, n + 1];
        for (int i = 0; i < n; i++)
        {
            for (int j = 0; j < n; j++)
            {
                B[i, j] = A[i, j];
            }
        }
        for (int i = 0; i < n; i++)
        {
            B[i, n] = b[i];
        }

        for (int i = 0; i < n; i++)
        {

            int pivot = i;

            for (int j = i; j < n; j++)
            {
                if (Mathf.Abs(B[i, j]) > Mathf.Abs(B[pivot, i])) { pivot = j; }
            }
            //Swap(B[i],B[pivot]);
            for (int kk = 0; kk < n + 1; kk++)
            {
                float t = B[i, kk];
                B[i, kk] = B[pivot, kk];
                B[pivot, kk] = t;
            }
            if (Mathf.Abs(B[i, i]) < EPS) { return new List<float>(); }

            for (int j = i + 1; j < n + 1; j++) { B[i, j] /= B[i, i]; }

            for (int j = 0; j < n; j++)
            {
                if (j != i)
                {
                    for (int k = i + 1; k < n + 1; k++)
                    {
                        B[j, k] -= B[j, i] * B[i, k];
                    }
                }
            }
        }
        List<float> x = new List<float>();
        for (int i = 0; i < n; i++)
        {
            x.Add(B[i, n]);
        }

        return x;
    }


}