// 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; } }