public void CreateKdtree(KdTree tree, IList <Obstacle> obslist)
{
treenodes.Clear();
obstacles.Clear();
Dictionary <int, KdtreeObstacle> dic = new Dictionary <int, KdtreeObstacle>();
for (int i = 0; i < obslist.Count; ++i)
{
Obstacle obs = obslist[i];
KdtreeObstacle kdobs = new KdtreeObstacle();
kdobs.convex_ = obs.convex_;
kdobs.id_ = obs.id_;
kdobs.point_ = obs.point_;
kdobs.direction_ = obs.direction_;
kdobs.nextID = obs.next_.id_;
kdobs.previousID = obs.previous_.id_;
obstacles.Add(kdobs);
dic[obs.id_] = kdobs;
}
obstacleTree_ = AssignTreeNode(tree.obstacleTree_, dic);
LogMgr.Log("dic " + dic.Count);
}
private Obstacle CreateObstacle(KdtreeObstacle obs, Dictionary <int, KdtreeObstacle> dic, Dictionary <int, Obstacle> obsdic)
{
if (obs != null)
{
Obstacle target = new Obstacle();
target.id_ = obs.id_;
obsdic[obs.id_] = target;
if (obs.nextID != -1)
{
if (obsdic.ContainsKey(obs.nextID))
{
target.next_ = obsdic[obs.nextID];
}
else
{
target.next_ = CreateObstacle(dic[obs.nextID], dic, obsdic);
}
}
else
{
target.next_ = null;
}
if (obs.previousID != -1)
{
if (obsdic.ContainsKey(obs.previousID))
{
target.previous_ = obsdic[obs.previousID];
}
else
{
target.previous_ = CreateObstacle(dic[obs.previousID], dic, obsdic);
}
}
else
{
target.previous_ = null;
}
target.direction_ = obs.direction_;
target.point_ = obs.point_;
target.convex_ = obs.convex_;
return(target);
}
return(null);
}