void BuildQuadtree(List <IAgentBehaviour> agents, Quadtree <QTData> tree)
{
// foreach(KeyValuePair< int, List < IAgentBehaviour >> kv in _agents)
{
tree.Clear();
if (agents.Count > 0)
{
QTBound bounds = QTBound.MinMaxQTBound(agents[0].position, agents[0].position);
int count = agents.Count;
for (int i = 1; i < count; i++)
{
//if(agents[i]==null || agents[i]==null)
//{
// continue;
//}
TSVector p = agents[i].position;
bounds = QTBound.MinMaxQTBound(TSVector.Min(bounds.min, p), TSVector.Max(bounds.max, p));
}
tree.SetBounds(bounds);
for (int i = 0; i < count; i++)
{
tree.Insert(agents[i]);
}
// tree.DebugDraw();
}
tree.CalculateSpeeds();
}
}
public void QueryRec(int i, QTBound r)
{
var radius = TSMath.Min(TSMath.Max((nodes[i].maxSpeed + speed) * timeHorizon, TRadius), maxRadius); //+ TRadius,warning
if (nodes[i].childNode1 == i)
{
// Leaf node
for (T a = nodes[i].nextData; a != null; a = (T)a.next)
{
FP v = T.InsertNeighbour(a, radius * radius);
//
if (v < maxRadius * maxRadius)
{
maxRadius = TSMath.Sqrt(v);
}
}
}
else
{
TSVector min = TSVector.zero, max = TSVector.zero;
// Not a leaf node
TSVector c = r.center;
if (p.x - radius < c.x)
{
if (p.z - radius < c.z)
{
QueryRec(nodes[i].childNode1, QTBound.MinMaxQTBound(r.min, c));
radius = TSMath.Min(radius, maxRadius);
}
if (p.z + radius > c.z)
{
min.Set(r.min.x, 0, c.z);
max.Set(c.x, 0, r.max.z);
QueryRec(nodes[i].childNode2, QTBound.MinMaxQTBound(min, max));
radius = TSMath.Min(radius, maxRadius);
}
}
if (p.x + radius > c.x)
{
if (p.z - radius < c.z)
{
max.Set(r.max.x, 0, c.z);
min.Set(c.x, 0, r.min.z);
QueryRec(nodes[i].childNode3, QTBound.MinMaxQTBound(min, max));
radius = TSMath.Min(radius, maxRadius);
}
if (p.z + radius > c.z)
{
QueryRec(nodes[i].childNode4, QTBound.MinMaxQTBound(c, r.max));
}
}
}
}
public void Insert(T T)
{
int i = 0;
QTBound r = bounds;
TSVector p = T.position;
T.next = null;
maxRadius = TSMath.Max(T.neighbourRadius, maxRadius);
int depth = 0;
TSVector min = TSVector.zero;
TSVector max = TSVector.zero;
while (true)
{
depth++;
if (nodes[i].childNode1 == i)
{
// Leaf node.
if (nodes[i].count < LeafSize || depth > 10)
{
nodes[i].Add(T);
// nodes[i].count++;
break;
}
else
{
// Split
QTNode node = nodes[i];
node.childNode1 = GetNodeIndex();
node.childNode2 = GetNodeIndex();
node.childNode3 = GetNodeIndex();
node.childNode4 = GetNodeIndex();
nodes[i] = node;
nodes[i].Distribute(nodes, r);
}
}
// Note, no else
if (nodes[i].childNode1 != i)
{
// Not a leaf node
TSVector c = r.center;
if (p.x > c.x)
{
if (p.z > c.z)
{
i = nodes[i].childNode4;
r = QTBound.MinMaxQTBound(c, r.max);
}
else
{
i = nodes[i].childNode3;
min.Set(c.x, 0, r.min.z);
max.Set(r.max.x, 0, c.z);
r = QTBound.MinMaxQTBound(min, max);
}
}
else
{
if (p.z > c.z)
{
i = nodes[i].childNode2;
min.Set(r.min.x, 0, c.z);
max.Set(c.x, 0, r.max.z);
r = QTBound.MinMaxQTBound(min, max);
}
else
{
i = nodes[i].childNode1;
r = QTBound.MinMaxQTBound(r.min, c);
}
}
}
}
}