HoloSpatialUnderstandingSampleGameScannerAnalyzer.cs

using System.Collections.Generic;
using HoloToolkit.Unity.InputModule;
using UnityEngine;
using HoloToolkit.Unity;

public class ScannerAnalyzer : MonoBehaviour, IInputClickHandler
{
    const int QueryResultMaxCount = 512;
    const int DisplayResultMaxCount = 32;

    private List<AnimatedBox> _lineBoxList = new List<AnimatedBox>();
    private SpatialUnderstandingDllTopology.TopologyResult[] _resultsTopology = new SpatialUnderstandingDllTopology.TopologyResult[QueryResultMaxCount];
    private LineData _lineData = new LineData();
    private string _spaceQueryDescription;

    public TextMesh DebugDisplay;
    public static bool AnalyzerEnabled;
    public Material MaterialLine;

    void Update()
    {
        if (!AnalyzerEnabled) return;

        if (DebugDisplay != null)
            DebugDisplay.text = _spaceQueryDescription;

        // Queries
        if (SpatialUnderstanding.Instance.ScanState == SpatialUnderstanding.ScanStates.Done)
        {
            //Update_Queries();
        }

        // Lines: Begin
        LineDraw_Begin();

        // Drawers
        var needsUpdate = false;
        needsUpdate |= Draw_LineBoxList();

        // Lines: Finish up
        LineDraw_End(needsUpdate);
    }

    public void OnInputClicked(InputClickedEventData eventData)
    {
        if (!SpatialUnderstanding.Instance.AllowSpatialUnderstanding)
        {
            return;
        }
        var minWidthOfWallSpace = 1f;
        var minHeightAboveFloor = 1f;

        // Query
        var resultsTopologyPtr = SpatialUnderstanding.Instance.UnderstandingDLL.PinObject(_resultsTopology);
        var locationCount = SpatialUnderstandingDllTopology.QueryTopology_FindPositionsOnFloor(
            minWidthOfWallSpace, minHeightAboveFloor,
            _resultsTopology.Length, resultsTopologyPtr);

        // Output
        var visDesc = "Find Positions On Floor";
        var boxFullDims = new Vector3(minWidthOfWallSpace, 0.025f, minHeightAboveFloor);
        var color = Color.red;
        ClearGeometry();

        // Add the line boxes (we may have more results than boxes - pick evenly across the results in that case)
        var lineInc = Mathf.CeilToInt((float)locationCount / (float)DisplayResultMaxCount);
        var boxesDisplayed = 0;
        for (var i = 0; i < locationCount; i += lineInc)
        {
            var timeDelay = (float)_lineBoxList.Count * AnimatedBox.DelayPerItem;
            _lineBoxList.Add(
                new AnimatedBox(
                    timeDelay,
                    _resultsTopology[i].position,
                    Quaternion.LookRotation(_resultsTopology[i].normal, Vector3.up),
                    color,
                    boxFullDims * 0.5f)
            );
            ++boxesDisplayed;
        }

        // Vis description
        if (locationCount == boxesDisplayed)
        {
            _spaceQueryDescription = string.Format("{0} ({1})", visDesc, locationCount);
        }
        else
        {
            _spaceQueryDescription = string.Format("{0} (found={1}, displayed={2})", visDesc, locationCount, boxesDisplayed);
        }
    }

    #region Line and Box Drawing

    protected void LineDraw_Begin()
    {
        _lineData.LineIndex = 0;
        for (var i = 0; i < _lineData.Lines.Count; ++i)
        {
            _lineData.Lines[i].isValid = false;
        }
    }

    private bool Draw_LineBoxList()
    {
        var needsUpdate = false;
        for (var i = 0; i < _lineBoxList.Count; ++i)
        {
            needsUpdate |= Draw_AnimatedBox(_lineBoxList[i]);
        }
        return needsUpdate;
    }

    protected void LineDraw_End(bool needsUpdate)
    {
        if (_lineData == null)
        {
            return;
        }

        // Check if we have any not dirty
        var i = 0;
        while (i < _lineData.Lines.Count)
        {
            if (!_lineData.Lines[i].isValid)
            {
                needsUpdate = true;
                _lineData.Lines.RemoveAt(i);
                continue;
            }
            ++i;
        }

        // Do the update (if needed)
        if (needsUpdate)
        {
            Lines_LineDataToMesh();
        }
    }

    private void Lines_LineDataToMesh()
    {
        // Alloc them up
        var verts = new Vector3[_lineData.Lines.Count * 8];
        var tris = new int[_lineData.Lines.Count * 12 * 3];
        var colors = new Color[verts.Length];

        // Build the data
        for (var i = 0; i < _lineData.Lines.Count; ++i)
        {
            // Base index calcs
            var vert = i * 8;
            var v0 = vert;
            var tri = i * 12 * 3;

            // Setup
            var dirUnit = (_lineData.Lines[i].p1 - _lineData.Lines[i].p0).normalized;
            var normX = Vector3.Cross((Mathf.Abs(dirUnit.y) >= 0.99f) ? Vector3.right : Vector3.up, dirUnit).normalized;
            var normy = Vector3.Cross(normX, dirUnit);

            // Verts
            verts[vert] = _lineData.Lines[i].p0 + normX * _lineData.Lines[i].lineWidth + normy * _lineData.Lines[i].lineWidth; colors[vert] = _lineData.Lines[i].c0; ++vert;
            verts[vert] = _lineData.Lines[i].p0 - normX * _lineData.Lines[i].lineWidth + normy * _lineData.Lines[i].lineWidth; colors[vert] = _lineData.Lines[i].c0; ++vert;
            verts[vert] = _lineData.Lines[i].p0 - normX * _lineData.Lines[i].lineWidth - normy * _lineData.Lines[i].lineWidth; colors[vert] = _lineData.Lines[i].c0; ++vert;
            verts[vert] = _lineData.Lines[i].p0 + normX * _lineData.Lines[i].lineWidth - normy * _lineData.Lines[i].lineWidth; colors[vert] = _lineData.Lines[i].c0; ++vert;

            verts[vert] = _lineData.Lines[i].p1 + normX * _lineData.Lines[i].lineWidth + normy * _lineData.Lines[i].lineWidth; colors[vert] = _lineData.Lines[i].c1; ++vert;
            verts[vert] = _lineData.Lines[i].p1 - normX * _lineData.Lines[i].lineWidth + normy * _lineData.Lines[i].lineWidth; colors[vert] = _lineData.Lines[i].c1; ++vert;
            verts[vert] = _lineData.Lines[i].p1 - normX * _lineData.Lines[i].lineWidth - normy * _lineData.Lines[i].lineWidth; colors[vert] = _lineData.Lines[i].c1; ++vert;
            verts[vert] = _lineData.Lines[i].p1 + normX * _lineData.Lines[i].lineWidth - normy * _lineData.Lines[i].lineWidth; colors[vert] = _lineData.Lines[i].c1; ++vert;

            // Indices
            tris[tri + 0] = (v0 + 0); tris[tri + 1] = (v0 + 5); tris[tri + 2] = (v0 + 4); tri += 3;
            tris[tri + 0] = (v0 + 1); tris[tri + 1] = (v0 + 5); tris[tri + 2] = (v0 + 0); tri += 3;

            tris[tri + 0] = (v0 + 1); tris[tri + 1] = (v0 + 6); tris[tri + 2] = (v0 + 5); tri += 3;
            tris[tri + 0] = (v0 + 2); tris[tri + 1] = (v0 + 6); tris[tri + 2] = (v0 + 1); tri += 3;

            tris[tri + 0] = (v0 + 2); tris[tri + 1] = (v0 + 7); tris[tri + 2] = (v0 + 6); tri += 3;
            tris[tri + 0] = (v0 + 3); tris[tri + 1] = (v0 + 7); tris[tri + 2] = (v0 + 2); tri += 3;

            tris[tri + 0] = (v0 + 3); tris[tri + 1] = (v0 + 7); tris[tri + 2] = (v0 + 4); tri += 3;
            tris[tri + 0] = (v0 + 3); tris[tri + 1] = (v0 + 4); tris[tri + 2] = (v0 + 0); tri += 3;

            tris[tri + 0] = (v0 + 0); tris[tri + 1] = (v0 + 3); tris[tri + 2] = (v0 + 2); tri += 3;
            tris[tri + 0] = (v0 + 0); tris[tri + 1] = (v0 + 2); tris[tri + 2] = (v0 + 1); tri += 3;

            tris[tri + 0] = (v0 + 5); tris[tri + 1] = (v0 + 6); tris[tri + 2] = (v0 + 7); tri += 3;
            tris[tri + 0] = (v0 + 5); tris[tri + 1] = (v0 + 7); tris[tri + 2] = (v0 + 4); tri += 3;
        }

        // Create up the components
        if (_lineData.Renderer == null)
        {
            _lineData.Renderer = gameObject.AddComponent<MeshRenderer>() ??
                                 gameObject.GetComponent<Renderer>() as MeshRenderer;
            _lineData.Renderer.material = MaterialLine;
        }

        if (_lineData.Filter == null)
        {
            _lineData.Filter = gameObject.AddComponent<MeshFilter>() ?? gameObject.GetComponent<MeshFilter>();
        }
        // Create or clear the mesh
        Mesh mesh;
        if (_lineData.Filter.mesh != null)
        {
            mesh = _lineData.Filter.mesh;
            mesh.Clear();
        }
        else
        {
            mesh = new Mesh { name = "Lines_LineDataToMesh" };
        }

        // Set them into the mesh
        mesh.vertices = verts;
        mesh.triangles = tris;
        mesh.colors = colors;
        mesh.RecalculateBounds();
        mesh.RecalculateNormals();

        _lineData.Filter.mesh = mesh;

        // If no tris, hide it
        _lineData.Renderer.enabled = (_lineData.Lines.Count != 0);

        // Line index reset
        _lineData.LineIndex = 0;
    }

    protected bool Draw_AnimatedBox(AnimatedBox box)
    {
        // Update the time
        if (!box.Update(Time.deltaTime))
        {
            return false;
        }
        if (box.IsAnimationComplete)
        {
            // Animation is done, just pass through
            return Draw_Box(box.Center, box.Rotation, box.Color, box.HalfSize, box.LineWidth);
        }

        // Draw it using the current anim state
        return Draw_Box(
            box.AnimPosition.Evaluate(box.Time),
            box.Rotation * Quaternion.AngleAxis(360.0f * box.AnimRotation.Evaluate(box.Time), Vector3.up),
            box.Color,
            box.HalfSize * box.AnimScale.Evaluate(box.Time),
            box.LineWidth);
    }

    protected bool Draw_Box(Vector3 center, Quaternion rotation, Color color, Vector3 halfSize, float lineWidth = Line.DefaultLineWidth)
    {
        var needsUpdate = false;

        var basisX = rotation * Vector3.right;
        var basisY = rotation * Vector3.up;
        var basisZ = rotation * Vector3.forward;
        Vector3[] pts =
        {
            center + basisX * halfSize.x + basisY * halfSize.y + basisZ * halfSize.z,
            center + basisX * halfSize.x + basisY * halfSize.y - basisZ * halfSize.z,
            center - basisX * halfSize.x + basisY * halfSize.y - basisZ * halfSize.z,
            center - basisX * halfSize.x + basisY * halfSize.y + basisZ * halfSize.z,

            center + basisX * halfSize.x - basisY * halfSize.y + basisZ * halfSize.z,
            center + basisX * halfSize.x - basisY * halfSize.y - basisZ * halfSize.z,
            center - basisX * halfSize.x - basisY * halfSize.y - basisZ * halfSize.z,
            center - basisX * halfSize.x - basisY * halfSize.y + basisZ * halfSize.z
        };

        // Bottom
        needsUpdate |= Draw_Line(pts[0], pts[1], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[1], pts[2], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[2], pts[3], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[3], pts[0], color, color, lineWidth);

        // Top
        needsUpdate |= Draw_Line(pts[4], pts[5], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[5], pts[6], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[6], pts[7], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[7], pts[4], color, color, lineWidth);

        // Vertical lines
        needsUpdate |= Draw_Line(pts[0], pts[4], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[1], pts[5], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[2], pts[6], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[3], pts[7], color, color, lineWidth);

        return needsUpdate;
    }

    protected bool Draw_Line(Vector3 start, Vector3 end, Color colorStart, Color colorEnd, float lineWidth = Line.DefaultLineWidth)
    {
        // Create up a new line (unless it's already created)
        while (_lineData.LineIndex >= _lineData.Lines.Count)
        {
            _lineData.Lines.Add(new Line());
        }

        // Set it
        var needsUpdate = _lineData.Lines[_lineData.LineIndex].Set_IfDifferent(transform.InverseTransformPoint(start), transform.InverseTransformPoint(end), colorStart, colorEnd, lineWidth);

        // Inc out count
        ++_lineData.LineIndex;

        return needsUpdate;
    }

    public void ClearGeometry(bool clearAll = true)
    {
        _lineBoxList = new List<AnimatedBox>();
    }
    #endregion

}