// Use this for initialization
void Start()
{
// Initial size (metres), initial centre position, minimum node size (metres), looseness
// boundsTree = new BoundsOctree<GameObject>(15, this.transform.position, 1, 1.25f);
// Initial size (metres), initial centre position, minimum node size (metres)
pointTree = new PointOctree <WatchData>(50, Vector3.zero, 0.5f);// this.transform.position, 1);
}
// Update is called once per frame
void Update()
{
po = new PointOctree <Transform>(10, Vector3.zero, .01f);
foreach (Transform go in gos)
{
po.Add(go, go.transform.position);
}
}
void Start()
{
itemTree = new PointOctree <ItemData>(octreeInitialNodeSize, Camera.main.transform.position, 1);
UnityBridgeManager.Watch(
this.system.GetEventName(this.system.listEventTemplate),
transform,
"OnData"
);
}
public void updateDatastructure(Point3d min, Point3d max, int minNodeSize, IList <T> spatialObjects)
{
float initialWorldSize = (float)min.DistanceTo(max);
float x = (float)(min.X + max.X) / 2;
float y = (float)(min.Y + max.Y) / 2;
float z = (float)(min.Z + max.Z) / 2;
Vector3 initialWorldPos = new Vector3(x, y, z);
this.octTree = new PointOctree <T>(initialWorldSize, initialWorldPos, minNodeSize); // DK: these values matter!
}
public void Initialize(IDataset dataset, IPostionTargetSource postionTargetSource)
{
TargetSource = postionTargetSource;
DataSet = dataset;
Octree = new PointOctree <Integer>(worldStartSize, Vector3.zero, nodeMinSize);
for (int i = 0; i < dataset.Count; i++)
{
Octree.Add(i, dataset[i].Position);
}
}
public void UpdateDatastructure(Point3d min, Point3d max, int minNodeSize, IList <T> spatialObjects)
{
float initialWorldSize = (float)min.DistanceTo(max);
BoundingBox box = new BoundingBox(min, max);
float x = (float)box.Center.X;
float y = (float)box.Center.Y;
float z = (float)box.Center.Z;
Vector3 initialWorldPos = new Vector3(x, y, z);
this.octTree = new PointOctree <T>(initialWorldSize, initialWorldPos, minNodeSize); // DK: these values matter!
foreach (T item in spatialObjects)
{
Point3d p = ((IPosition)item).GetPoint3D();
this.octTree.Add(item, new Vector3((float)p.X, (float)p.Y, (float)p.Z)); // DK: added
}
}
#pragma warning restore 0649
private void Start()
{
items = new GameObject[pointsCount];
pointOctree = new PointOctree <GameObject>(maxSize, transform.position, minSize);
for (int i = 0; i < pointsCount; i++)
{
Vector3 pos = new Vector3(
UnityEngine.Random.Range(transform.position.x - maxSize, transform.position.x + maxSize),
UnityEngine.Random.Range(transform.position.y - maxSize, transform.position.y + maxSize),
UnityEngine.Random.Range(transform.position.z - maxSize, transform.position.z + maxSize));
items[i] = Instantiate(cubePrefab, pos, Quaternion.identity) as GameObject;
items[i].name = i.ToString();
pointOctree.Add(items[i], pos);
}
}
private void ClearAll()
{
finalLanes = new PointOctree <CsvLane>(1000, Vector3.zero, 100);
csvPoints = new List <CsvPoint>();
csvNodes = new List <CsvNode>();
csvDtLanes = new List <CsvDtLane>();
csvLanes = new List <CsvLane>();
csvLines = new List <CsvLine>();
csvVectors = new List <CsvVector>();
csvSignalLights = new List <CsvSignalLight>();
csvStopLines = new List <CsvStopLine>();
csvStopLines = new List <CsvStopLine>();
csvWhiteLines = new List <CsvWhiteLine>();
csvRoadEdges = new List <CsvRoadEdge>();
csvCurbs = new List <CsvCurb>();
}
void postRender(Camera cam)
{
List <PCLPoint> bodyPoints = KinectPCL.instance.bodyPoints;
PointOctree <PCLPoint> po = KinectPCL.instance.bodyTree;
if (po.Count == 0 || po.Count == KinectPCL.instance.numPoints)
{
return;
}
if (!processLines)
{
return;
}
GL.Begin(GL.LINES);
lineMat.SetPass(0);
foreach (PCLPoint p in bodyPoints)
{
Ray r = new Ray(p.position, Vector3.forward);
PCLPoint[] np = po.GetNearby(r, maxDistance);
foreach (PCLPoint npp in np)
{
if (npp.position.x < p.position.x || Vector3.Distance(npp.position, p.position) > maxDistance)
{
continue;
}
if (drawLines)
{
float dist = Vector3.Distance(npp.position, p.position);
float targetAlpha = alphaDecay.Evaluate(dist / maxDistance);
GL.Color(Color.Lerp(nearColor, farColor, targetAlpha));
GL.TexCoord(new Vector3(Time.time * targetAlpha, 0, 0));
GL.Vertex(npp.position);
GL.TexCoord(new Vector3(Time.time * targetAlpha + 1, 0, 0));
GL.Vertex(p.position);
}
}
}
GL.End();
}
// Update is called once per frame
public override void Update()
{
base.Update();
po = new PointOctree <PCLPointObject>(.1f, roiCollider.transform.position, .05f);
pointObjects = new PCLPointObject[handler.numGoodPoints];
numValidPoints = 0;
for (int i = 0; i < handler.numGoodPoints; i++)
{
Vector3 realPos = transform.TransformPoint(handler.points[i]);
if (roiCollider.bounds.Contains(realPos))
{
pointObjects[numValidPoints] = new PCLPointObject(realPos, i);
po.Add(pointObjects[numValidPoints], realPos);
numValidPoints++;
}
}
}
// Use this for initialization
void Start()
{
// Initial size (metres), initial centre position, minimum node size (metres), looseness
boundsTree = new BoundsOctree <GameObject>(15, MyContainer.position, 1, 1.25f);
// Initial size (metres), initial centre position, minimum node size (metres)
pointTree = new PointOctree <GameObject>(15, MyContainer.position, 1);
boundsTree.Add(myObject, myBounds);
//boundsTree.Remove(myObject);
pointTree.Add(myObject, myVector3);
//boundsTree.Remove(myObject);
//bool result = boundsTree.IsColliding(bounds);
//GameObject[] result2 = boundsTree.GetColliding(bounds);
//pointTree.GetNearby(myRay, 4);
}
void OnDrawGizmos()
{
if (KinectPCL.instance == null)
{
return;
}
PointOctree <PCLPoint> po = KinectPCL.instance.bodyTree;
if (po == null)
{
return;
}
if (debugPoints)
{
po.DrawAllObjects();
}
if (debugBounds)
{
po.DrawAllBounds();
}
}
void OnDrawGizmos()
{
BoundsOctree <GameObject> boundsTree = new BoundsOctree <GameObject>(15, MyContainer.position, 1, 1.5f);
PointOctree <GameObject> pointTree = new PointOctree <GameObject>(15, MyContainer.position, 1.0f);
for (int i = 0; i < myObject.Count; i++)
{
boundsTree.Add(myObject[i], myBounds);
boundsTree.DrawAllBounds(); // Draw node boundaries
Gizmos.color = Color.yellow;
Gizmos.DrawSphere(transform.position, radius);
boundsTree.DrawAllObjects(); // Draw object boundaries
//.DrawCollisionChecks(); // Draw the last *numCollisionsToSave* collision check boundaries
pointTree.DrawAllBounds(); // Draw node boundaries
pointTree.DrawAllObjects(); // Mark object positions
if (track[i])
{
myBounds.center = track[i].position;
}
}
}
public static void GetCsvData(this StopLine stopLine,
PointOctree <CsvLane> finalLanes,
out List <CsvPoint> csvPoints,
out List <CsvLine> csvLines,
out List <CsvVector> csvVectors,
out List <CsvSignalLight> csvSignalLights,
out List <CsvStopLine> csvStopLines)
{
csvPoints = new List <CsvPoint>();
csvLines = new List <CsvLine>();
csvStopLines = new List <CsvStopLine>();
var csvSLPoints = new List <CsvPoint>();
csvVectors = new List <CsvVector>();
csvSignalLights = new List <CsvSignalLight>();
for (int i = 0; i < stopLine.Count; i++)
{
var csvPoint = new CsvPoint()
{
Position = stopLine.LineRenderer.GetPosition(i)
};
csvPoints.Add(csvPoint);
if (i > 0)
{
var csvLine = new CsvLine()
{
PointBegin = csvPoints[i - 1], PointFinal = csvPoint
};
if (i > 1)
{
csvLine.LineLast = csvLines.Last();
}
csvLines.Add(csvLine);
var signalLight = stopLine.signalLights[i - 1];
var csvSLPoint = new CsvPoint()
{
Position = signalLight.transform.position
};
csvSLPoints.Add(csvSLPoint);
var csvVector = new CsvVector()
{
Point = csvSLPoint, Rotation = signalLight.transform.rotation
};
csvVectors.Add(csvVector);
var csvSignalLight = new CsvSignalLight()
{
Vector = csvVector
};
csvSignalLights.Add(csvSignalLight);
var midPoint = (csvLine.PointBegin.Position + csvLine.PointFinal.Position) / 2;
var csvStopLine = new CsvStopLine()
{
Line = csvLine,
SignalLight = csvSignalLight,
Lane = finalLanes
.GetNearby(midPoint, Vector3.Distance(csvLine.PointBegin.Position, csvLine.PointFinal.Position))
?.OrderBy(_ => Vector3.Distance(_.FinalNode.Point.Position, midPoint))?
.FirstOrDefault()
};
csvStopLines.Add(csvStopLine);
}
}
csvPoints.AddRange(csvSLPoints);
}
public SpatialCollectionAsOctTree(ISpatialCollection <T> spatialCollection)
{
// TODO: Complete member initialization
this.spatialObjects = ((SpatialCollectionAsOctTree <T>)spatialCollection).spatialObjects;
this.octTree = ((SpatialCollectionAsOctTree <T>)spatialCollection).octTree;
}
public SpatialCollectionAsOctTree(SpatialCollectionAsOctTree <T> collection)
{
this.spatialObjects = collection.spatialObjects;
this.octTree = collection.octTree;
}
public void Init()
{
tree = new PointOctree <Boid>(InitialWorldSize, InitialPosition, MinSize);
}
void postRender(Camera cam)
{
//if (cam != Camera.main) return;
if (autoBodyOnly)
{
bodyOnly = KinectPCLK1.instance.bodyPoints.Count > 10;
realBodyOnly = bodyOnly;
}
List <PCLPoint> tPoints = bodyOnly ? KinectPCLK1.instance.bodyPoints : KinectPCLK1.instance.roiPoints;
PointOctree <PCLPoint> po = bodyOnly ? KinectPCLK1.instance.bodyTree : KinectPCLK1.instance.roiTree;
if (po.Count == 0 || (bodyOnly && po.Count == KinectPCLK1.instance.numPoints))
{
return;
}
if (!processLines)
{
return;
}
lineMat.SetPass(0);
foreach (PCLPoint p in tPoints)
{
if (!p.isInROI)
{
continue;
}
Ray r = new Ray(p.position, Vector3.forward);
PCLPoint[] np = po.GetNearby(r, maxDistance);
foreach (PCLPoint npp in np)
{
if (npp.position.x < p.position.x || Vector3.Distance(npp.position, p.position) > maxDistance)
{
continue;
}
if (drawLines)
{
float dist = Vector3.Distance(npp.position, p.position);
float targetAlpha = alphaDecay.Evaluate(dist / maxDistance);
GL.Begin(GL.LINES);
GL.Color(Color.Lerp(nearColor, farColor, targetAlpha));
GL.TexCoord(Vector3.zero);
GL.TexCoord(new Vector3(Time.time * texSpeed * targetAlpha, 0, 0));
GL.Vertex(npp.position);
GL.TexCoord(Vector3.one);
GL.TexCoord(new Vector3(Time.time * texSpeed * targetAlpha + 1, 0, 0));
GL.Vertex(p.position);
GL.End();
}
}
}
// GL.End();
}
/// <summary>
/// Generate Nition PointOctree
/// </summary>
public void Initialize(int initialWorldSize, Vector3 initialPosition, int smallestObjectSize)
{
PointOctree = new PointOctree <int>(initialWorldSize, initialPosition, smallestObjectSize);
}
// Use this for initialization
void Awake()
{
pointTree = new PointOctree <GameObject>(1000f, transform.position, 0.5f);
}
// Update is called once per frame
void Update()
{
//TMP
if (Input.GetKey(KeyCode.G))
{
generateOffsetRandoms();
}
if (lastDownSample != downSample)
{
updateDownSample();
lastDownSample = downSample;
return;
}
CameraSpacePoint[] realWorldMap = multiSourceManager.GetRealWorldData();
byte[] bodyIndexMap = multiSourceManager.GetBodyIndexData();
//Texture2D tex = multiSourceManager.GetColorTexture();
bodyTree = new PointOctree <PCLPoint>(10, bodyCenter, .01f); //take previous frame
bodyPoints.Clear();
bodyCenter = new Vector3();
for (int ix = 0; ix < pointsWidth; ix++)
{
for (int iy = 0; iy < pointsHeight; iy++)
{
int dsIndex = iy * pointsWidth + ix;
Vector2 rIndex = dsOffsetRandoms[dsIndex];
int index = Mathf.RoundToInt(rIndex.x * multiSourceManager.DepthHeight * multiSourceManager.DepthWidth) + Mathf.RoundToInt(rIndex.y * multiSourceManager.DepthWidth);
//Vector3 dv = new Vector3(rIndex.x * 10, rIndex.y * 10);
//Debug.DrawLine(dv, dv + Vector3.forward * .2f, Color.red);
if (index >= realWorldMap.Length)
{
continue;
}
CameraSpacePoint csp = realWorldMap[index];
Vector3 p = new Vector3(csp.X, csp.Y, csp.Z);
Vector3 tPoint = transform.TransformPoint(p);
int tIndex = iy * pointsWidth + ix;
bodyMask[tIndex] = bodyIndexMap[index] != 255;
bool isBody = bodyMask[tIndex];
bool isValid = true;
if (isBody)
{
if (bodyRandomProba < 1)
{
isBody = Random.value <= bodyRandomProba;
}
}
if (float.IsNaN(tPoint.x) || float.IsInfinity(tPoint.x))
{
isValid = false;
tPoint = Vector3.zero;
}
PCLPoint pp = new PCLPoint(tPoint, isBody, isValid, true, isBody ? Color.yellow : Color.white);
points[tIndex] = pp;
if (isBody && isValid)
{
bodyPoints.Add(pp);
bodyTree.Add(pp, tPoint);
bodyCenter += tPoint;
}
if (debug && isValid)
{
if (isBody || !debugBodyOnly)
{
Color c = isBody ? Color.yellow : Color.white;
Debug.DrawLine(tPoint, tPoint + Vector3.forward * .05f, c);
}
}
}
}
if (bodyPoints.Count > 0)
{
bodyCenter /= bodyPoints.Count;
}
}
public SpatialCollectionAsOctTree()
{
this.spatialObjects = new List <T>();
this.octTree = new PointOctree <T>(20, new Vector3(0, 0, 0), 10); // DK: these values matter!
}
public static void ExportMaps(string folder)
{
PointOctree <CsvLane> finalLanes = new PointOctree <CsvLane>(1000, Vector3.zero, 100);
List <CsvPoint> csvPoints = new List <CsvPoint>();
List <CsvNode> csvNodes = new List <CsvNode>();
List <CsvDtLane> csvDtLanes = new List <CsvDtLane>();
List <CsvLane> csvLanes = new List <CsvLane>();
List <CsvLine> csvLines = new List <CsvLine>();
List <CsvVector> csvVectors = new List <CsvVector>();
List <CsvSignalLight> csvSignalLights = new List <CsvSignalLight>();
List <CsvStopLine> csvStopLines = new List <CsvStopLine>();
List <CsvWhiteLine> csvWhiteLines = new List <CsvWhiteLine>();
List <CsvRoadEdge> csvRoadEdges = new List <CsvRoadEdge>();
List <CsvCurb> csvCurbs = new List <CsvCurb>();
var lastLane = csvLanes.LastOrDefault();
foreach (var item in Lane.List)
{
item.GetCsvData(
out List <CsvPoint> points,
out List <CsvNode> nodes,
out List <CsvDtLane> dtlanes,
out List <CsvLane> lanes);
csvPoints.AddRange(points);
csvNodes.AddRange(nodes);
csvDtLanes.AddRange(dtlanes);
csvLanes.AddRange(lanes);
finalLanes.Add(lanes.LastOrDefault(), lanes.LastOrDefault().FinalNode.Point.Position);
}
foreach (var item in StopLine.List)
{
item.GetCsvData(finalLanes,
out List <CsvPoint> points,
out List <CsvLine> lines,
out List <CsvVector> vectors,
out List <CsvSignalLight> signalLights,
out List <CsvStopLine> stopLines);
csvPoints.AddRange(points);
csvLines.AddRange(lines);
csvVectors.AddRange(vectors);
csvSignalLights.AddRange(signalLights);
csvStopLines.AddRange(stopLines);
}
foreach (var item in WhiteLine.List)
{
item.GetCsvData(
out List <CsvPoint> points,
out List <CsvLine> lines,
out List <CsvWhiteLine> whiteLines);
csvPoints.AddRange(points);
csvLines.AddRange(lines);
csvWhiteLines.AddRange(whiteLines);
}
foreach (var item in RoadEdge.List)
{
item.GetCsvData(
out List <CsvPoint> points,
out List <CsvLine> lines,
out List <CsvRoadEdge> roadEdges);
csvPoints.AddRange(points);
csvLines.AddRange(lines);
csvRoadEdges.AddRange(roadEdges);
}
foreach (var item in Curb.List)
{
item.GetCsvData(
out List <CsvPoint> points,
out List <CsvLine> lines,
out List <CsvCurb> curbs);
csvPoints.AddRange(points);
csvLines.AddRange(lines);
csvCurbs.AddRange(curbs);
}
for (int i = 0; i < csvPoints.Count; i++)
{
csvPoints[i].PID = i + 1;
}
for (int i = 0; i < csvNodes.Count; i++)
{
csvNodes[i].NID = i + 1;
}
for (int i = 0; i < csvDtLanes.Count; i++)
{
csvDtLanes[i].DID = i + 1;
}
for (int i = 0; i < csvLanes.Count; i++)
{
csvLanes[i].LnID = i + 1;
}
for (int i = 0; i < csvLines.Count; i++)
{
csvLines[i].LID = i + 1;
}
for (int i = 0; i < csvVectors.Count; i++)
{
csvVectors[i].VID = i + 1;
}
for (int i = 0; i < csvSignalLights.Count; i++)
{
csvSignalLights[i].ID = i + 1;
}
for (int i = 0; i < csvStopLines.Count; i++)
{
csvStopLines[i].ID = i + 1;
}
for (int i = 0; i < csvWhiteLines.Count; i++)
{
csvWhiteLines[i].ID = i + 1;
}
for (int i = 0; i < csvRoadEdges.Count; i++)
{
csvRoadEdges[i].ID = i + 1;
}
for (int i = 0; i < csvCurbs.Count; i++)
{
csvCurbs[i].ID = i + 1;
}
if (csvPoints.Count > 0)
{
var ls = new List <string>()
{
CsvPoint.header
};
ls.AddRange(csvPoints.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvPoint.fileName), ls);
}
if (csvNodes.Count > 0)
{
var ls = new List <string>()
{
CsvNode.header
};
ls.AddRange(csvNodes.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvNode.fileName), ls);
}
if (csvDtLanes.Count > 0)
{
var ls = new List <string>()
{
CsvDtLane.header
};
ls.AddRange(csvDtLanes.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvDtLane.fileName), ls);
}
if (csvLanes.Count > 0)
{
var ls = new List <string>()
{
CsvLane.header
};
ls.AddRange(csvLanes.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvLane.fileName), ls);
}
if (csvLines.Count > 0)
{
var ls = new List <string>()
{
CsvLine.header
};
ls.AddRange(csvLines.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvLine.fileName), ls);
}
if (csvVectors.Count > 0)
{
var ls = new List <string>()
{
CsvVector.header
};
ls.AddRange(csvVectors.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvVector.fileName), ls);
}
if (csvSignalLights.Count > 0)
{
var ls = new List <string>()
{
CsvSignalLight.header
};
ls.AddRange(csvSignalLights.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvSignalLight.fileName), ls);
}
if (csvStopLines.Count > 0)
{
var ls = new List <string>()
{
CsvStopLine.header
};
ls.AddRange(csvStopLines.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvStopLine.fileName), ls);
}
if (csvWhiteLines.Count > 0)
{
var ls = new List <string>()
{
CsvWhiteLine.header
};
ls.AddRange(csvWhiteLines.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvWhiteLine.fileName), ls);
}
if (csvRoadEdges.Count > 0)
{
var ls = new List <string>()
{
CsvRoadEdge.header
};
ls.AddRange(csvRoadEdges.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvRoadEdge.fileName), ls);
}
if (csvCurbs.Count > 0)
{
var ls = new List <string>()
{
CsvCurb.header
};
ls.AddRange(csvCurbs.Select(_ => _.CsvString));
File.WriteAllLines(Path.Combine(folder, CsvCurb.fileName), ls);
}
}
public SpatialCollectionAsOctTree(float initialWorldSize, Vector3 initialWorldPos, float minNodeSize)
{
this.spatialObjects = new List <T>();
this.octTree = new PointOctree <T>(initialWorldSize, initialWorldPos, minNodeSize); // DK: these values matter!
}
// Use this for initialization
IEnumerator Start()
{
{
for (int x = -99; x <= 99; x += 6)
{
for (int y = -99; y <= 99; y += 6)
{
for (int z = -99; z <= 99; z += 6)
{
positions.Add(new Vector3(x, y, z));
}
}
}
}
tree = new LooseOctree <GameObject>(200F, Vector3.zero, 1.25F, positions.Count);
numObjectsDisplay.text = "Objects per iteration: " + positions.Count;
float[] buildResults = new float[20];
float buildTotal = 0;
float[] destroyResults = new float[20];
float destroyTotal = 0;
Stopwatch timer = new Stopwatch();
for (int i = 0; i < buildResults.Length; i++)
{
iterationsDisplay.text = "Iterations: " + (i + 1);
buildResults[i] = PopulateTree(timer);
timer.Stop();
buildTotal += buildResults[i];
numNodesDisplay.text = "Nodes per iteration: " + tree.nodeCount;
destroyResults[i] = DestroyTree(timer);
destroyTotal += destroyResults[i];
averageTimeDisplay.text = "Average time: Build(" + Mathf.Round(buildTotal / (i + 1)) + "ms) - Destroy(" + Mathf.Round(destroyTotal / (i + 1)) + "ms)";
totalTimeDisplay.text = "Total time: Build(" + buildTotal + "ms) - Destroy(" + destroyTotal + "ms)";
yield return(new WaitForSeconds(0.1F));
}
PopulateTree(timer);
pointTree = new PointOctree <OctreeObject <GameObject> >(200F, Vector3.zero, 1.25F);
OctreeObject <GameObject> obj;
Queue <OctreeObject <GameObject> > remObj = new Queue <OctreeObject <GameObject> >();
timer.Reset();
while (treeObj.Count > 0)
{
obj = treeObj.Dequeue();
timer.Start();
pointTree.Add(obj, obj.boundsCenter);
timer.Stop();
remObj.Enqueue(obj);
}
pointTreeAddTimeDisplay.text = timer.ElapsedMilliseconds + "ms";
timer.Reset();
while (remObj.Count > 0)
{
obj = remObj.Dequeue();
timer.Start();
pointTree.Remove(obj);
timer.Stop();
}
pointTreeRemoveTimeDisplay.text = timer.ElapsedMilliseconds + "ms";
averageTimeDisplay.text = "Average time: Build(" + buildTotal / buildResults.Length + "ms) - Destroy(" + destroyTotal / destroyResults.Length + "ms)";
StartCoroutine(PopulateTreeSlow());
}
// Update is called once per frame
public override void Update()
{
//transform.localPosition = pos;
//transform.localRotation = Quaternion.Euler(rot);
if (Input.GetKeyDown(KeyCode.G))
{
regenerateRandom = !regenerateRandom;
}
if (Input.GetKeyDown(KeyCode.H))
{
regenerateRandom = true;
}
if (Input.GetKeyUp(KeyCode.H))
{
regenerateRandom = false;
}
if (lastDSXOffset != downSampleRandomXOffset || lastDSYOffset != downSampleRandomYOffset || regenerateRandom)
{
generateOffsetRandoms();
lastDSXOffset = downSampleRandomXOffset;
lastDSYOffset = downSampleRandomYOffset;
}
animateOffsetRandoms();
if (lastDownSample != downSample)
{
updateDownSample();
lastDownSample = downSample;
return;
}
ushort[] depthMap = manager.GetRawDepthMap();
bodyTree = new PointOctree <PCLPoint>(10, bodyCenter, .01f); //take previous frame
roiTree = new PointOctree <PCLPoint>(10, Vector3.zero, .01f); //take previous frame
roiPoints.Clear();
bodyPoints.Clear();
bodyCenter = new Vector3();
for (int ix = 0; ix < pointsWidth; ix++)
{
for (int iy = 0; iy < pointsHeight; iy++)
{
int dsIndex = iy * pointsWidth + ix;
Vector2 drIndex = dsOffsetRandomsAnimated[dsIndex];
int dIndexX = (int)(drIndex.x * DEPTH_WIDTH);
int dIndexY = (int)(drIndex.y * DEPTH_HEIGHT);
int index = dIndexY * DEPTH_WIDTH + dIndexX;
//Debug.DrawRay(new Vector3(dsOffsetRandomsAnimated[dsIndex].x, dsOffsetRandomsAnimated[dsIndex].y) * 10, Vector3.forward * .5f,Color.red) ;
if (index >= depthMap.Length)
{
continue;
}
ushort um = (ushort)(depthMap[index] & 7);
if (um == 0)
{
um = 255;
}
else
{
um = (byte)(um % 4);
}
ushort d = (ushort)(depthMap[index] >> 3);
bool isValid = d > 0;
Vector3 p;
if (isValid)
{
float z_metric = d * 0.001f;
float tx = z_metric * ((dIndexX - CENTRALPOINT_X) * FOCAL_X_INV);
float ty = z_metric * ((dIndexY - CENTRALPOINT_Y) * FOCAL_Y_INV);
p = new Vector3(tx, -ty, z_metric);
}
else
{
p = Vector3.zero;
}
bool isInROI = true;
if (!isValid || p.z < minDepth || p.z > maxDepth ||
p.x < leftLimit || p.x > rightLimit ||
p.y < bottomLimit || p.y > topLimit)
{
isInROI = false;
}
Vector3 tPoint = transform.TransformPoint(p);
int tIndex = iy * pointsWidth + ix;
bodyMask[tIndex] = um != 255;
bool isBody = bodyMask[tIndex];
if (isBody)
{
if (bodyRandomProba < 1)
{
isBody = Random.value <= bodyRandomProba;
}
}
PCLPoint pp = new PCLPoint(tPoint, isBody, isValid, isInROI, manager.usersMapColors[index]);
points[tIndex] = pp;
if (isValid)
{
roiTree.Add(pp, tPoint);
roiPoints.Add(pp);
if (isBody)
{
bodyPoints.Add(pp);
bodyTree.Add(pp, tPoint);
bodyCenter += tPoint;
}
}
if (debug && isValid)
{
if (isBody || !debugBodyOnly)
{
Color c = isInROI ? (isBody ? Color.yellow : Color.white) : Color.gray;
Debug.DrawRay(tPoint, Vector3.forward * .1f, c);
}
}
}
}
if (bodyPoints.Count > 0)
{
bodyCenter /= bodyPoints.Count;
}
}
