public void UpdateData(string action, string key, string psr)
{
bool isItemInDictionary = itemDictionary.ContainsKey(key);
if (action == "child_added" || action == "child_changed")
{
if (isItemInDictionary)
{
// Set existing data entry
ItemData existingItem;
bool exists = itemDictionary.TryGetValue(key, out existingItem);
// re-add the item again into the tree, as it is not possible to move it directly to another position
itemTree.Remove(existingItem);
existingItem.CalculatePSR(psr);
itemTree.Add(existingItem, existingItem.Position);
// set existing item game object, because we want nearby data update to be immediate
if (existingItem.DistanceTo(Camera.main.transform) <= visibleDistance)
{
SetChild(existingItem);
}
}
else
{
// Add data entry
var item = new ItemData(key, psr);
itemTree.Add(item, item.Position);
itemDictionary.Add(item.Key, item);
// create object if it is nearby
if (item.DistanceTo(Camera.main.transform) <= visibleDistance)
{
CreateChild(item);
}
}
}
else if (action == "child_removed")
{
if (isItemInDictionary)
{
var existingItem = itemDictionary[key];
itemTree.Remove(existingItem);
itemDictionary.Remove(key);
RemoveChild(existingItem);
}
}
// Start Proximity Check If not yet
if (coroutine == null)
{
coroutine = CoroutineSpawnDespawnChildren(manageGameObjectInterval);
StartCoroutine(coroutine);
}
}
public static void Register(ServerWorldEntity entity)
{
lock (octree)
{
octree.Add(entity, entity.Position);
}
}
// 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);
}
}
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);
}
}
#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 GetLanes()
{
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);
}
}
// 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);
}
private void UpdateTree()
{
timer += Time.deltaTime;
if (timer >= updateRate)
{
timer = 0f;
pointOctree.ClearTree();
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].transform.position = pos;
pointOctree.Add(items[i], pos);
}
}
}
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);
}
}
// 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
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 void Add(Vector3 vec)
{
WatchData data = new WatchData(vec);
pointTree.Add(data, vec);
}
public void CalculateAverageLists <T>(List <T> list, float x = 0, float y = 0, float z = 0, float radius = -1)
{
if (spawner == null)
{
spawner = FindObjectOfType <SpawnSpheres>();
}
averageSpectralM.Clear();
averageSpectralK.Clear();
averageSpectralG.Clear();
averageSpectralF.Clear();
averageSpectralA.Clear();
for (int i = 0; i < list.Count; i++)
{
List <Vector3> position = new List <Vector3>();
List <int> temperature = new List <int>();
List <float> size = new List <float>();
if (typeof(T) == typeof(CelestialBody))
{
if (radius == -1)
{
position.Add((list[i] as CelestialBody).position);
}
else
{
Vector3 roiCenterToPoint = (list[i] as CelestialBody).position - new Vector3(x, y, z);
float ratio = roiCenterToPoint.magnitude / radius;
Vector3 newPosition = roiCenterToPoint.normalized * ratio;
ROI_celestialBodyCloud[i] = new CelestialBody
{
position = newPosition,
temperature = (list[i] as CelestialBody).temperature,
distance = (list[i] as CelestialBody).distance,
radius = (list[i] as CelestialBody).radius,
source_id = (list[i] as CelestialBody).source_id
};
ROI_octree.Add(ROI_celestialBodyCloud[i], ROI_celestialBodyCloud[i].position);
position.Add(newPosition);
}
temperature.Add((list[i] as CelestialBody).temperature);
size.Add(spawner.particleSize);
}
if (typeof(T) == typeof(PointOctreeNode <CelestialBody>))
{
if (!(list[i] as PointOctreeNode <CelestialBody>).HasChildren)
{
foreach (PointOctreeNode <CelestialBody> .OctreeObject cel in (list[i] as PointOctreeNode <CelestialBody>).objects)
{
position.Add(cel.Obj.position);
temperature.Add(cel.Obj.temperature);
size.Add(spawner.particleSize);
}
}
else
{
position.Add((list[i] as PointOctreeNode <CelestialBody>).averagePositionOfNodes);
temperature.Add((list[i] as PointOctreeNode <CelestialBody>).averageTempOfNodes);
size.Add((list[i] as PointOctreeNode <CelestialBody>).distanceFromAverage);
}
}
for (int j = 0; j < temperature.Count; j++)
{
if (temperature[j] <= 3700)
{
averageSpectralM.Add(new Tuple <Vector3, float>(position[j], size[j]));
}
else if (temperature[j] > 3700 && temperature[j] <= 5200)
{
averageSpectralK.Add(new Tuple <Vector3, float>(position[j], size[j]));
}
else if (temperature[j] > 5200 && temperature[j] <= 6000)
{
averageSpectralG.Add(new Tuple <Vector3, float>(position[j], size[j]));
}
else if (temperature[j] > 6000 && temperature[j] <= 7500)
{
averageSpectralF.Add(new Tuple <Vector3, float>(position[j], size[j]));
}
else if (temperature[j] > 7500 && temperature[j] <= 10000)
{
averageSpectralA.Add(new Tuple <Vector3, float>(position[j], size[j]));
}
}
}
spawner.ApplyToParticleSystem('M', averageSpectralM);
spawner.ApplyToParticleSystem('K', averageSpectralK);
spawner.ApplyToParticleSystem('G', averageSpectralG);
spawner.ApplyToParticleSystem('F', averageSpectralF);
spawner.ApplyToParticleSystem('A', averageSpectralA);
}
/// <summary>
/// Runs current tracked object data through Octree.
/// </summary>
private TrackedObjectStates evaluateOctree(OctreeThreadParameters otp)
{
int alloc = WorldMonitor.Instance.AllocationSpace;
updatePositions = new Vector3[alloc]; // otp.ObjectIDs.Count
float[] thresholds = new float[alloc]; // otp.ObjectIDs.Count
//for (int i = 0; i < otp.ObjectIDs.Count; i++)
foreach (int id in otp.ObjectIDs)
{
KeyValuePair <float, Vector3> pos;
if (!otp.Coordinates.TryGetValue(id, out pos))
{
Debug.LogError("unknown object position request in octree eval");
}
thresholds[id] = pos.Key;
updatePositions[id] = pos.Value;
PointOctree.Remove(id);
PointOctree.Add(id, pos.Value);
}
List <int[]> enteringIDs = new List <int[]>();
List <int[]> leavingIDs = new List <int[]>();
List <int> parentIDLeavers = new List <int>();
List <int> parentIDEnterers = new List <int>();
int ind = 0;
foreach (int id in otp.ObjectIDs)
{
List <int> validConflicts = new List <int>();
List <int> stayers = new List <int>();
int[] areaObjects = PointOctree.GetNearby(updatePositions[id], thresholds[id]);
for (int j = 0; j < areaObjects.Length; j++)
{
string affiliateObj = WorldMonitor.Instance.TrackedObjectAffiliations[id];
string affiliateCompare = WorldMonitor.Instance.TrackedObjectAffiliations[areaObjects[j]];
/*
* run conflict validity checks: if not the same object && not an object of the same class type && is a new conflict
*/
if (areaObjects[j] != id && !MasterList[id].Contains(areaObjects[j]) && string.Compare(affiliateObj, affiliateCompare) != 0)
{
if (!parentIDEnterers.Contains(id))
{
parentIDEnterers.Add(id);
}
MasterList[id].Add(areaObjects[j]); // add conflicting object to master list of current conflicts
validConflicts.Add(areaObjects[j]); // *new* conflicts
stayers.Add(areaObjects[j]); // use to look for conflicts that have ended
}
else if (MasterList[id].Contains(areaObjects[j]))
{
stayers.Add(areaObjects[j]); // this is an object staying in conflict
}
}
bool leaverDetected = false;
List <int> leavers = new List <int>();
foreach (int _id in MasterList[id]) // look at master list's record of conflicts for this parent ID - if it isn't in stayers, it has left the conflict area or been destroyed
{
if (!stayers.Contains(_id))
{
leaverDetected = true;
leavers.Add(_id);
}
switch (WorldMonitor.Instance.ConflictEndMode)
{
case ConflictEndMode.OnAllConflictsEnded:
if (leavers.Count == MasterList[id].Count)
{
parentIDLeavers.Add(id);
}
break;
case ConflictEndMode.OnIndividualConflictEnded:
if (leaverDetected && !parentIDEnterers.Contains(id) && !parentIDLeavers.Contains(id))
{
parentIDLeavers.Add(id);
}
break;
}
}
foreach (int leaver in leavers)
{
MasterList[id].Remove(leaver);
}
int numValid = leavers.ToArray().Length;
if (numValid > 0)
{
leavingIDs.Add(leavers.ToArray());
}
numValid = validConflicts.ToArray().Length;
if (numValid > 0)
{
enteringIDs.Add(validConflicts.ToArray());
}
ind++;
}
return(new TrackedObjectStates
{
ParentIDEnterers = parentIDEnterers.ToArray(), // parent IDs of new or increased conflict states
ParentIDLeavers = parentIDLeavers.ToArray(), // parent IDs of expired or reduced conflict states
LeavingIDs = leavingIDs, // child IDs - ended conflict(s)
EnteringIDs = enteringIDs, // child IDs - the conflict(s)
PriorConflictingIDs = parentIDLeavers // parent IDs that need informed all conflicts have ceased
});
}
// 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;
}
}