Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
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
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.