public void Distribute(QTNode[] nodes, QTBound r)
{
TSVector c = r.center;
while (nextData != null)
{
T nx = (T)nextData.next;
if (nextData.position.x > c.x)
{
if (nextData.position.z > c.z)
{
nodes[childNode4].Add(nextData);
}
else
{
nodes[childNode3].Add(nextData);
}
}
else
{
if (nextData.position.z > c.z)
{
nodes[childNode2].Add(nextData);
}
else
{
nodes[childNode1].Add(nextData);
}
}
nextData = (T)nx;
}
count = 0;
}
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 static QTBound MinMaxQTBound(TSVector _min, TSVector _max)
{
QTBound bound = zero;
bound.min = _min;
bound.max = _max;
// bound.center
return(bound);
}
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 bool Intersect(QTBound Other)
{
if ((min.x > Other.max.x) || (Other.min.x > max.x))
{
return(false);
}
if ((min.z > Other.max.z) || (Other.min.z > max.z))
{
return(false);
}
return(true);
}
public static QTBound MinMaxQTBound(int minX, int minY, int maxX, int maxY)
{
QTBound bound = zero;
TSVector min = TSVector.zero;
min.x = minX;
min.z = minY;
TSVector max = TSVector.zero;
max.x = maxX;
max.z = maxY;
bound.min = min;
bound.max = max;
// bound.center
return(bound);
}
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);
}
}
}
}
}
public void SetBounds(QTBound r)
{
bounds = r;
}
public override bool Equals(object other)
{
QTBound bound = (QTBound)other;
return(bound == this);
}
public bool IsInside(QTBound Other)
{
return(IsInside(Other.min) && IsInside(Other.max));
}
