void TrackObstableStates()
{
//track col between two radar sends, if one collider is still detected, track id if become undetected, recycle id
utilColList.Clear();
utilColList.AddRange(lastColliders.Keys);
utilColList.RemoveAll(c => c == null);
for (int i = 0; i < utilColList.Count; i++)
{
Collider col = utilColList[i];
if (radarDetectedColliders.ContainsKey(col))
{
radarDetectedColliders[col] = new ObjectTrackInfo()
{
id = lastColliders[col].id, point = radarDetectedColliders[col].point
};
}
else
{
//put id back to min heap
IDHeap.Add(lastColliders[col].id);
//Debug.Log("id " + lastColliders[col].id + " has been put back");
}
}
//if one collider become detected, get min available id
utilColList.Clear();
utilColList.AddRange(radarDetectedColliders.Keys);
utilColList.RemoveAll(c => c == null);
for (int i = 0; i < utilColList.Count; i++)
{
Collider col = utilColList[i];
if (!lastColliders.ContainsKey(col))
{
//get id out of min heap
if (IDHeap.Size < 1)
{
Debug.Log($"{nameof(IDHeap)} size become empty, logic error.");
}
radarDetectedColliders[col] = new ObjectTrackInfo()
{
id = IDHeap.Pop(), point = radarDetectedColliders[col].point, newDetection = true
};
//Debug.Log("get new available id " + a + " in heap and assign");
}
}
}
private void Start()
{
foreach (var rrt in radarRangeTriggers)
{
rrt.SetCallback(OnObjectDetected);
}
exclusionColliders = new HashSet <Collider>(new List <Collider>(agentSetup?.GetComponentsInChildren <Collider>()));
IDHeap = new Utils.MinHeap(maxObjs);
for (int i = 0; i < maxObjs; i++)
{
IDHeap.Add(i);
}
Enable(false);
}
void Start()
{
foreach (var rrt in radarRangeTriggers)
{
rrt.SetCallback(OnObjectDetected);
}
var robot = GetComponentInParent <RobotSetup>();
if (robot != null)
{
exclusionColliders = new HashSet <Collider>(new List <Collider>(robot.GetComponentsInChildren <Collider>()));
}
IDHeap = new Utils.MinHeap(maxObjs);
for (int i = 0; i < maxObjs; i++)
{
IDHeap.Add(i);
}
Enable(false);
}