Homography(射影変換) in Unity C#

homography.cs

public class GameManager : MonoBehaviour
{
    Texture2D srcTexture;
    Texture2D dstTexture;
    Color32[] clearColors;
    
    void Start()
    {
        var width = 500;
        var height = 500;

        dstTexture = new Texture2D(width, height, TextureFormat.RGBA32, false);
        var dstSprite = Sprite.Create(dstTexture, new Rect(0, 0, width, height), new Vector2(0.5f, 0.5f));

        var render = GetComponent<SpriteRenderer>();
        render.sprite = dstSprite;

        var srcSprite = Resources.Load<Sprite>("sprite");
        srcTexture = srcSprite.texture;  

        clearColors = new Color32[dstTexture.width * dstTexture.height];
        for (int i = 0; i < clearColors.Length; i++) 
        {
            clearColors[i] = Color.clear;
        }
    }
   
    // dstPoints: コピー先の座標4点, 左下から時計回り
    // https://matcha-choco010.net/2018/09/15/unity-graphic-gl/
    void copyTextureFree(Texture2D src, RectInt srcRect, Texture2D dst, Vector2[] dstPoints)
    {
        var srcPoints = new[] {new Vector2(0f, 0f), new Vector2(0f, srcRect.height), new Vector2(srcRect.width, srcRect.height), new Vector2(srcRect.width, 0f)};
        
        Vector4 srcXs, srcYs, dstXs, dstYs;
        var offsetX = (dstPoints[2].x - dstPoints[0].x) / 2;
        var offsetY = (dstPoints[2].y - dstPoints[0].y) / 2;
        {
            var s = srcPoints;
            var d = dstPoints;
            srcXs = new Vector4(s[0].x, s[1].x, s[2].x, s[3].x);
            srcYs = new Vector4(s[0].y, s[1].y, s[2].y, s[3].y);
            dstXs = new Vector4(d[0].x, d[1].x, d[2].x, d[3].x);
            dstYs = new Vector4(d[0].y, d[1].y, d[2].y, d[3].y);
            
            // dstPointsが(0, 0)だとTx, Tyの逆行列を求めることができなくなるので、(1, 1)に設定
            for (int i = 0; i < 4; i++)
            {
                if (srcXs[i] == 0f)
                {
                    srcXs[i] = 1f;
                }
                if (srcYs[i] == 0f)
                {
                    srcYs[i] = 1f;
                }

                if (dstXs[i] == 0f)
                {
                    dstXs[i] = 1f;
                }
                if (dstYs[i] == 0f)
                {
                    dstYs[i] = 1f;
                }
            }
        }

        // パラメータA~Hを求める
        // https://ja.osdn.net/projects/nyartoolkit/docs/tech_document0001/ja/tech_document0001.pdf
        float A, B, C, D, E, F, G, H;
        {
            Vector4 X = dstXs, Y = dstYs, x = srcXs, y = srcYs;

            var Tx = new Matrix4x4();
            for (int i = 0; i < 4; i++) 
            {
                Tx.SetRow(i, new Vector4(X[i], Y[i], -X[i] * x[i], -Y[i] * x[i]));
            }
            var Ty = new Matrix4x4();
            for (int i = 0; i < 4; i++) 
            {
                Ty.SetRow(i, new Vector4(X[i], Y[i], -X[i] * y[i], -Y[i] * y[i]));
            }

            if (Tx.determinant == 0f)
            {
                return;
            }
            if (Ty.determinant == 0f)
            {
                return;
            }

            {
                var tx = Tx.inverse;
                var ty = Ty.inverse;

                var a = Vector4.Dot(tx.GetRow(2), x);
                var b = Vector4.Dot(tx.GetRow(2), Vector4.one);
                var c = Vector4.Dot(tx.GetRow(3), x);
                var d = Vector4.Dot(tx.GetRow(3), Vector4.one);

                var e = Vector4.Dot(ty.GetRow(2), y);
                var f = Vector4.Dot(ty.GetRow(2), Vector4.one);
                var g = Vector4.Dot(ty.GetRow(3), y);
                var h = Vector4.Dot(ty.GetRow(3), Vector4.one);

                var bhfd = b * h - f * d;
                if (bhfd == 0f)
                {
                    return;
                }

                C = (h * (a - e) - f * (c - g)) / bhfd;
                F = (d * (a - e) - b * (c - g)) / bhfd;

                var Cvec = new Vector4(C, C, C, C);
                var Fvec = new Vector4(F, F, F, F);

                A = Vector4.Dot(tx.GetRow(0), x - Cvec);
                B = Vector4.Dot(tx.GetRow(1), x - Cvec);
                G = Vector4.Dot(tx.GetRow(2), x - Cvec);
                H = Vector4.Dot(tx.GetRow(3), x - Cvec);

                D = Vector4.Dot(ty.GetRow(0), y - Fvec);
                E = Vector4.Dot(ty.GetRow(1), y - Fvec);

                /*
                var G2 = Vector4.Dot(ty.GetRow(2), y - Fvec);
                var H2 = Vector4.Dot(ty.GetRow(3), y - Fvec);
                Debug.Log($"{G} == {G2}, {H} == {H2}");
                */
            }
        }

        var dstXsAry = new[] { dstXs[0], dstXs[1], dstXs[2], dstXs[3] };
        var dstYsAry = new[] { dstYs[0], dstYs[1], dstYs[2], dstYs[3] };
        var minX = (int)Mathf.Min(dstXsAry);
        var minY = (int)Mathf.Min(dstYsAry);
        var maxX = (int)Mathf.Max(dstXsAry);
        var maxY = (int)Mathf.Max(dstYsAry);

        var srcPixel = src.GetPixels();

        for (int y = minY; y < maxY; y++) 
        {
            if (y < 0 || dst.height <= y) 
            {
                continue;
            }

            for (int x = minX; x < maxX; x++)
            {
                if (x < 0 || dst.width <= x)
                {
                    continue;
                }

                float srcX = srcRect.x + (A * x + B * y + C) / (G * x + H * y + 1f);
                float srcY = srcRect.y + (D * x + E * y + F) / (G * x + H * y + 1f);

                var imgX = Mathf.RoundToInt(srcX);
                var imgY = Mathf.RoundToInt(srcY);
                if (0 <= imgX && imgX < src.width && 0 <= imgY && imgY < src.height)
                {
                    var c = srcPixel[imgY * src.width + imgX];

                    if (c.a > 0f)
                    {
                        dst.SetPixel(x, y, c);
                    }
                }
            }
        }
    }
    
    void Update()
    {
        dstTexture.SetPixels32(clearColors);

        // https://geolog.mydns.jp/1st.geocities.jp/shift486909/program/CurveLazer.html
        var P0 = new Vector2(0f, 0f);
        var P1 = new Vector2(100f, 200f);
        var P2 = new Vector2(200f, 100f);
        const int N = 64;
        float px = P0.x, py = P0.y;
        var pb2 = Vector2.zero;
        var pb3 = Vector2.zero;
        for (int i = 1; i < N; i++) {
            var t = (float)i / N;
            var w0 = (1f - t) * (1f - t);
            var w1 = 2 * (1f - t) * t;
            var w2 = t * t;

            var x = w0 * P0.x + w1 * P1.x + w2 * P2.x;
            var y = w0 * P0.y + w1 * P1.y + w2 * P2.y;

            var angle = Mathf.Atan2(y - py, x - px) + Mathf.PI / 2f;
            var w = srcTexture.width;

            if (i == 1) 
            {
                pb2 = new Vector2(px - Mathf.Cos(angle) * w, py - Mathf.Sin(angle) * w);
                pb3 = new Vector2(px + Mathf.Cos(angle) * w, py + Mathf.Sin(angle) * w);
            }

            var b2 = new Vector2(x + Mathf.Cos(angle) * w, y + Mathf.Sin(angle) * w);
            var b3 = new Vector2(x - Mathf.Cos(angle) * w, y - Mathf.Sin(angle) * w);
            
            var dstPoints = new [] {
                pb2,
                b2,
                b3,
                pb3,
            };
            copyTextureFree(srcTexture, new RectInt(0, srcTexture.height / N * (i - 1), srcTexture.width, srcTexture.height / N), dstTexture, dstPoints);

            foreach (var p in dstPoints) 
            {
                plot(dstTexture, p);
            }

            pb2 = b2;
            pb3 = b3;

            px = x;
            py = y;
        }

        dstTexture.Apply();
    }
}