SlamFlexAdapter.cs

Go to the documentation of this file.

#define TRANSLATION
//#define GYRO
//#define GYRO_WITH_TRANSLATION
//#define NO_GYRO
//#define NO_GYRO_WITH_ROT
//#define CORNER_TEX

using UnityEngine;
using System;
using System.Collections;
using System.IO;
using System.Collections.Generic;
using System.Runtime.InteropServices;


public class SlamFlexAdapter : MonoBehaviour
{
    private GameObject _bg_panel;
    private GameObject _bg_panel_corner_points;

    protected Color32[] _buf;
    private WebCamTexture w;
    private GCHandle m_PixelsHandle;


    public GameObject _ground_plane;
    private int screen_width = 640;
    private int screen_height = 480;
    private float aspect = 1.38188006064344f;
    private bool autoend = false;
    private Vector3 orginalPlainePosition;

    private bool textureOk = false;
    private bool planeRotationFound = false;
    private Vector3 planeRotation, planeTranslation = Vector3.zero;
    private string userMessagesTextInstr = "Translate the camera slowly sideways";
    private string userMessagesText = "";
    private string userMessagesText1 = "";

    private Texture2D cornerPoints;
    private GyroCam gyroCam;

    private string logFileData = "";
    public bool enable_logging = false;

    private SlamFlexWrapper.DetectionState current_State = SlamFlexWrapper.DetectionState.Finished;


    private SlamFlexWrapper.SendStringDelegate mSendStringDelegate;
    private SlamFlexWrapper.SendPoseDelegate mSendPoseDelegate;
    private SlamFlexWrapper.SendLogDelegate mSendLogDelegate;
    private SlamFlexWrapper.SendArrayPointsDelegate mSendArrayPointsDelegate;

    void Awake()
    {
        //setup unity3D webcam
        WebCamDevice[] devices= WebCamTexture.devices;
        if (devices.Length > 0)
        {
            w=new WebCamTexture(screen_width, screen_height, 15);

            //setup background
            camera.aspect = aspect;
            this._bg_panel=GameObject.Find("Plane");
            this._bg_panel.renderer.material.mainTexture=w;
            w.Play();

#if CORNER_TEX
            //setup texture for rendering corner points on separate texture
            this._bg_panel_corner_points = GameObject.Find ("TrackingCornerPoints");

            cornerPoints = new Texture2D(screen_width, screen_height);
            Color[] data = new Color[screen_width*screen_height];
            Color c = new Color(255, 255, 255, 0);
            for (int i = 0; i<data.Length; i++)
            {
                data[i] = c;
            }

            cornerPoints.SetPixels(data);
            cornerPoints.Apply();

            this._bg_panel_corner_points.renderer.material.mainTexture = cornerPoints;
#endif
    
        }
        else
        {
            Debug.LogError("No Webcam.");
        }

#if GYRO || GYRO_WITH_TRANSLATION
        gyroCam = transform.GetComponent<GyroCam>();
        gyroCam.enabled = true;
#endif
    }


    void Start ()
    {
        //Setup logger
        //load previous settings
        if (enable_logging)
        {
            StreamReader fileReader = null;
            string logFile = Application.persistentDataPath + "/log.txt";


            if (File.Exists(logFile))
            {
                fileReader = new StreamReader(logFile);
                
                logFileData = fileReader.ReadToEnd();
                fileReader.Close();
            }
        }

        //first few frames is not valid, have to wait for valid frame
        StartCoroutine (CheckTheValidityOfTexture());

        //implement SlamFlex hooks 
        mSendStringDelegate = new SlamFlexWrapper.SendStringDelegate(ReciveStringFromPlugin);
        mSendPoseDelegate = new SlamFlexWrapper.SendPoseDelegate(RecivePoseFromPlugin);
        mSendLogDelegate = new SlamFlexWrapper.SendLogDelegate(Logging);
        mSendArrayPointsDelegate = new SlamFlexWrapper.SendArrayPointsDelegate(ReciveArrayPointsFromPlugin);

        //initialize SLAMflex plugin
        SlamFlexWrapper.StartSlam (mSendStringDelegate, mSendPoseDelegate, mSendLogDelegate, mSendArrayPointsDelegate);


    }



    void Update ()
    {
        //we have valid texture as input, each update send one frame to plugin
        if (textureOk && !planeRotationFound) 
        {
            this._buf = (Color32[])this.w.GetPixels32 ();
            m_PixelsHandle = GCHandle.Alloc (this._buf, GCHandleType.Pinned);
            current_State = SlamFlexWrapper.SetNewFrame (m_PixelsHandle.AddrOfPinnedObject (), this.w.width, this.w.height);
            m_PixelsHandle.Free ();
        }
    }


    private IEnumerator CheckTheValidityOfTexture()
    {
        while (true) 
        {
            if (w.height < 100)
            {
                Debug.Log("WebTexture not initialized");
                yield return null;
            }
            else
            {
                textureOk = true;
                break;
            }
        }
    }



    private void ReciveStringFromPlugin(string message)
    {

        if(message == "Tracker_ResetMM")
        {
            StopAllCoroutines();
            StartCoroutine(AutoEnd());
        }
        else if(message.StartsWith("Corners"))
        {
            userMessagesText = message;
        }
        else
        {
            Debug.Log("Message from plugin:  " + message);
        }
    }


    private void ReciveArrayPointsFromPlugin(IntPtr pointer, int size )
    {
        int [] corners = new int[size];
        Marshal.Copy(pointer, corners, 0, size);

        SetCornerTexturePoints(corners);
    }


    private void SetCornerTexturePoints(int[] corners)
    {

        Color[] data = new Color[screen_width*screen_height];
        Color c = new Color(255, 255, 255, 0);
        for (int i = 0; i<data.Length; i++)
        {
            data[i] = c;
        }
    
        for(int i = 0; i < corners.Length; i++)
        {
            data[corners[i]] = Color.red;
        }

        cornerPoints.SetPixels(data);
        cornerPoints.Apply();
    }

    private void RecivePoseFromPlugin (float r1, float r2, float r3, double t1, double t2, double t3)
    {
        
        planeRotation.x = r1;
        planeRotation.y = r2;
        planeRotation.z = r3;
        
        planeTranslation.x = (float)t2;
        planeTranslation.y = (float)t1;
//      planeTranslation.z = (float)t3; for later
        

        if(autoend)
        {
            //Debug.Log("Pose:  "+r1.ToString()+" "+r2.ToString()+" "+r3.ToString()+" "+t1.ToString()+" "+t2.ToString()+" "+t3.ToString());
            this._ground_plane.SetActive (true);
            this._ground_plane.transform.eulerAngles = planeRotation + this._bg_panel.transform.eulerAngles;



#if GYRO || GYRO_WITH_TRANSLATION

            Vector3 planePosition = this._bg_panel.transform.position;

            if (planePosition.x > 0) 
                planePosition.x -= 1000f;
            else
                planePosition.x += 1000f;
            
            if (planePosition.y > 0) 
                planePosition.y -= 1000f;
            else
                planePosition.y += 1000f;
            
            if (planePosition.z > 0) 
                planePosition.z -= 1000f;
            else
                planePosition.z += 1000f;
#endif
            

            Debug.Log ("Unity_ Plane found");
    //      SlamFlexWrapper.Stop ();

            StopAllCoroutines();

#if GYRO || GYRO_WITH_TRANSLATION
            this._ground_plane.transform.position = planePosition;
            orginalPlainePosition = planePosition;
#else

            orginalPlainePosition = this._ground_plane.transform.position;
#endif
//      planeRotationFound = true;
        }
        autoend = false;

#if GYRO_WITH_TRANSLATION
        planeTranslation.x *= 4140f;
        planeTranslation.y *= -3000f;
        if(!gyroCam.GyroAngleDetected)
        {
            this._ground_plane.transform.position = orginalPlainePosition + planeTranslation;
        }
#endif

#if TRANSLATION
        float offset = 1;
        if((float)t3 < 1.75f)
        {
            offset = 2;
        }
        planeTranslation.x *= 4140f * offset;
        planeTranslation.y *= -3000f * offset;
        this._ground_plane.transform.position = orginalPlainePosition + planeTranslation;

#endif

#if NO_GYRO_WITH_ROT
        this._ground_plane.transform.eulerAngles = planeRotation + this._bg_panel.transform.eulerAngles;
#endif
        //userMessagesText1 = "t1: " + t1.ToString()+"    t2: " + t2.ToString() + "\nt3: " + t3.ToString();


    }
    

    private IEnumerator AutoEnd()
    {
        autoend = true;
        while (autoend)
        {
            yield return new WaitForSeconds(1f);
            SlamFlexWrapper.StartPlaneDetection();
            Debug.Log ("AutoEnd finished");
        }

    }


    private void Logging (string message)
    {
        if(enable_logging)
        {
            logFileData += message;
        }
    }



    public void UserMessages (string message)
    {
        userMessagesText = message;
    }


    void OnGUI()
    {
        if( GUI.Button(new Rect(Screen.width*0.9f, 0, Screen.width*0.1f, Screen.width*0.1f), "Exit"))
        {
            Debug.Log("Unity_ App exits");
            Application.Quit();
        }
        
        if( GUI.Button(new Rect(Screen.width - (Screen.width*0.3f),(Screen.height-(Screen.width*0.3f)),Screen.width*0.3f,Screen.width*0.3f), "Start Plane Detection"))
        {
            Debug.Log("Unity_ Tracking started");
            planeRotationFound = false;
            this._ground_plane.SetActive(false);
            Camera.main.transform.eulerAngles = Vector3.zero;
            SlamFlexWrapper.StartPlaneDetection();
            StartCoroutine(AutoEnd());
        }

        if( GUI.Button(new Rect(0,(Screen.height-(Screen.width*0.3f)),Screen.width*0.3f,Screen.width*0.3f), "Stop Plane Detection"))
        {
            Debug.Log("Unity_ Tracking stoped");
            planeRotationFound = false;
            this._ground_plane.transform.position = orginalPlainePosition;
            this._ground_plane.SetActive(false);
            Camera.main.transform.eulerAngles = Vector3.zero;
            SlamFlexWrapper.StopSlam();
        }

        string text  = "";
        if(autoend)
        {
            text  = "\nMessages: "+userMessagesTextInstr+"\n"+userMessagesText+"\n" + userMessagesText1;
        }
        else
        {
            text  = "\nMessages: "+userMessagesText+"\n" + userMessagesText1;
        }


        GUI.Label (new Rect (0f, 0f, 200, 200), "Current tracking state: " + current_State.ToString());
        GUI.Label (new Rect (0, 100, 400, 400), text);
    }


    void OnApplicationQuit()
    {
        if(enable_logging)
        {
            string logFile = Application.persistentDataPath + "/log.txt";
            var sr = File.CreateText(logFile);
            sr.WriteLine (logFileData);
            sr.Flush();
            sr.Close();
        }
    }



    private List<Vector2> ParseCoordinates(string array)
    {
        
        List<Vector2> ret = new List<Vector2>();
        
        string[] coordinates = array.Split(new char[]{';'});
        
        foreach(string s in coordinates)
        {
            string[] coor = s.Split(new char[]{','});
            if(coor.Length == 2)
            {
                int x = Convert.ToInt32(coor[0]);
                int y = Convert.ToInt32(coor[1]);
                if(x<screen_width+1 && x>0 && y<screen_height+1 && y>0)
                {
                    ret.Add(new Vector2(x, y));
                }
                
            }
        }
        
        return ret;
    }

}