Snake.cs

Go to the documentation of this file.

using UnityEngine;
using System;
using CSML;

public class Snake : MonoBehaviour {

    Renderer _renderer;
    Texture2D _I;

    public Texture2D _dIdx;
    public Texture2D _dIdy;

    Matrix _Vx;
    Matrix _Vy;

    Matrix _initVx;
    Matrix _initVy;

    public float _alpha = 0.01f;
    public float _beta = 0.001f;
    public float _gamma = 1.5f;
    public float _kappa = 0.9f;

    private float _touchTime = -0.5f;

    Matrix _AInv;

    bool init = true;

    void OnEnable () {
        _renderer = GetComponent<Renderer> ();
        _I = GameObject.Find ("2texCam").GetComponent<grabPicture> ().binaryImageAll;

        _Vx = new Matrix();
        _Vy = new Matrix();

        _initVx = new Matrix();
        _initVy = new Matrix();

        colorIndex = 0;
    }
    
    void Update () {}
    
    public Vector3[] GetSnakeWorldPoints() {
        Vector3[] points = new Vector3[_Vx.RowCount];
        
        for(int i = 0; i < _Vx.RowCount; i++) {
            points[i] = new Vector3((float)_Vx[i + 1].Re, (float)_Vy[i + 1].Re, 0.0f);
            points[i] = GameObject.Find("2texCam").GetComponent<Camera>().ScreenToWorldPoint(points[i]);
        }
        return points;
    }
    
    public Vector3[] GetStartWorldPoints() {
        Vector3[] points = new Vector3[_Vx.RowCount];
        
        for(int i = 0; i < _Vx.RowCount; i++) {
            points[i] = new Vector3((float)_initVx[i + 1].Re, (float)_initVy[i + 1].Re, 0.0f);
            points[i] = GameObject.Find("2texCam").GetComponent<Camera>().ScreenToWorldPoint(points[i]);
        }
        return points;
    }

    Vector3 startPoints;

    public void SetControlPoints(Vector3[] controlPoints){

        _initVx = new Matrix();
        _initVy = new Matrix();

        foreach (Vector3 cp in controlPoints) {
            RaycastHit hit;
            Vector3 position = GameObject.Find("2texCam").GetComponent<Camera>().WorldToScreenPoint(cp);
            
            Matrix tmpX, tmpY;
            if (Physics.Raycast (GameObject.Find("2texCam").GetComponent<Camera>().ScreenPointToRay (position), out hit)) {
                Vector2 uv = hit.textureCoord;
                
                tmpX = new Matrix (new double[] {uv.x * _I.width});
                tmpY = new Matrix (new double[] {uv.y * _I.height});
                
                if (_initVx.RowCount > 0) {
                    _initVx = Matrix.HorizontalConcat (_initVx, tmpX);
                    _initVy = Matrix.HorizontalConcat (_initVy, tmpY);
                } else {
                    _initVx = tmpX;
                    _initVy = tmpY;
                }
            }
        }
    }
    
    public void DisplayImagePlane(bool display) {
        _renderer.enabled = display;
    }
    
    public void ShowContour () {
        drawContour (_I, _Vx, _Vy, 1.0f);
    }

    int colorIndex = 0;
    Texture2D drawContour(Texture2D image, Matrix contourX, Matrix contourY, float alpha) {
        Color[] c = new Color[] { 
            Color.blue, Color.cyan, Color.green, Color.yellow, Color.red, 
            Color.magenta,  Color.gray
        };
        int N = contourX.RowCount;
        int x, y, ci = 0;
        for (int i = 0; i < N; i++) {

            c[ci] = c[ci] * alpha;

            x = (int)Math.Round(contourX[i + 1].Re);
            y = (int)Math.Round(contourY[i + 1].Re);
            ci = colorIndex;

            image.SetPixel(x,    y,   c[ci]);

            image.SetPixel(x +1, y,   c[ci]);
            image.SetPixel(x,    y +1,c[ci]);
            image.SetPixel(x -1, y,   c[ci]);
            image.SetPixel(x,    y -1,c[ci]);

            image.SetPixel(x +1, y +1,c[ci]);
            image.SetPixel(x -1, y +1,c[ci]);
            image.SetPixel(x -1, y -1,c[ci]);
            image.SetPixel(x +1, y -1,c[ci]);

            image.SetPixel(x +1, y +2,c[ci]);
            image.SetPixel(x -1, y +2,c[ci]);
            image.SetPixel(x -1, y -2,c[ci]);
            image.SetPixel(x +1, y -2,c[ci]);
            image.SetPixel(x +2, y +1,c[ci]);
            image.SetPixel(x -2, y +1,c[ci]);
            image.SetPixel(x -2, y -1,c[ci]);
            image.SetPixel(x +2, y -1,c[ci]);

            image.SetPixel(x +2, y +2,c[ci]);
            image.SetPixel(x -2, y +2,c[ci]);
            image.SetPixel(x -2, y -2,c[ci]);
            image.SetPixel(x +2, y -2,c[ci]);

            image.SetPixel(x +3, y +3,c[ci]);
            image.SetPixel(x -3, y +3,c[ci]);
            image.SetPixel(x -3, y -3,c[ci]);
            image.SetPixel(x +3, y -3,c[ci]);
        }
        image.Apply ();


        colorIndex = (colorIndex + 1)% c.Length;
        return image;
    }

    public void UpdateSnake(int iterations, bool drawSteps) {

        GameObject dFdxQuad = GameObject.Find("dFdxCam");

        dFdxQuad.GetComponent<Render2Texture> ().enabled = true;
        _dIdx = dFdxQuad.GetComponent<Render2Texture>().textOut;
        
        GameObject dFdyQuad = GameObject.Find("dFdyCam");

        dFdyQuad.GetComponent<Render2Texture> ().enabled = true;
        _dIdy = dFdyQuad.GetComponent<Render2Texture>().textOut;

        Color[] dVdx = _dIdx.GetPixels ();
        Color[] dVdy = _dIdy.GetPixels ();

        Matrix vfx,vfy;

        if(!drawSteps)
            drawContour(_I, _Vx, _Vy, 1.0f);
        for (int i = 0; i < iterations; i++) {

            vfx = interpolate(dVdx, _Vx, _Vy);
            vfy = interpolate(dVdy, _Vx, _Vy);

            _Vx = _AInv * ( _gamma * _Vx - _kappa * vfx);
            _Vy = _AInv * ( _gamma * _Vy - _kappa * vfy);

            if(i % 50 == 0 && drawSteps) {
                drawContour(_I, _Vx, _Vy, 1.0f);
            }
        }
        if(!drawSteps)
            drawContour(_I, _Vx, _Vy, 1.0f);
    }

    Matrix interpolate(Color[] gradient, Matrix Vx, Matrix Vy) {

        int N = Vx.RowCount;
        double[] p = new double[N];
        Color d;

        int x, y = 0;
        for (int i = 0; i < N; i++) {
            x = (int)Math.Round(Vx[i + 1].Re);
            y = (int)Math.Round(Vy[i + 1].Re);

            int j = x + y * _I.width;
            if(j > gradient.Length)
                j = gradient.Length-1;

            if(j<0)
                j=0;

            d = gradient[j];

            p[i] = (((d.r * 2.0) - 1.0) + ((d.g * 2.0) - 1.0) + ((d.b * 2.0) - 1.0)) / 3.0;
            p[i] *= -255.0;
            p[i] -= 1.0;

        }

        return new Matrix (p);
    }
    
    public void InitSnake(bool expanding = false) {

        _Vx = _initVx.Clone ();
        _Vy = _initVy.Clone ();

        int N = _Vx.RowCount;
        Matrix alphaV = _alpha * Matrix.Ones (1, N);
        Matrix betaV = _beta * Matrix.Ones (1, N);
        Matrix gammaV = _gamma * Matrix.Ones (1, N);

        //Invert alpha to get a expanding Snake
        if(expanding) {
            alphaV *= -1.0f;
        }

        Matrix diag1 = 2 * alphaV + 6 * betaV;
        Matrix diag2 = -alphaV - 4 * betaV;
        Matrix diag3 = betaV;

        /*
        creates a matrix in the form:

            -2a-6b  a+4b    -b      0       0       0       ...     -b      a+4b
            
            a+4b    -2a-6b  a+4b    -b      0       0       ...     0       -b
            
            -b      a+4b    -2a-6b  a+4b    -b      0       ...     0       0
            
            0       -b      a+4b    -2a-6b  a+4b    -b      ...     0       0
            
            0       0       -b      a+4b    -2a-6b  a+4b    ...     0       0
            
            ...     ...     ...     ...     ...     ...     ...     ...     ...
            
            -b      0       0       0       0       0       ...     -2a-6b  a+4b
            
            a+4b    -b      0       0       0       0       ...     a+4b    -2a-6b
         */

        Matrix A = Matrix.Diag (diag1);
        Matrix Diag2 = Matrix.Diag (diag2, 1) + Matrix.Diag (diag2, -1);
        Matrix Diag2a = Matrix.Diag (diag2, N - 1) + Matrix.Diag (diag2, -(N - 1));

        Matrix Diag3 = Matrix.Diag (diag3, 2) + Matrix.Diag (diag3, -2);
        Matrix Diag3a = Matrix.Diag (diag3, N - 2) + Matrix.Diag (diag3, -(N - 2));

        A += Diag2.Extract (1, N, 1, N);
        A += Diag2a.Extract (1, N, 1, N);
        A += Diag3.Extract (1, N, 1, N);
        A += Diag3a.Extract (1, N, 1, N);
        A += Matrix.Diag (gammaV);

        _AInv = A.Inverse ();
    }
}