public void Insert ( Agent agent ) {
int i = 0;
Rect r = bounds;
Vector2 p = new Vector2(agent.position.x, agent.position.z);
agent.next = null;
maxRadius = System.Math.Max (agent.radius, maxRadius);
int depth = 0;
while (true) {
depth++;
if ( nodes[i].child00 == i ) {
// Leaf node. Break at depth 10 in case lots of agents ( > LeafSize ) are in the same spot
if ( nodes[i].count < LeafSize || depth > 10 ) {
nodes[i].Add (agent);
nodes[i].count++;
break;
} else {
// Split
Node node = nodes[i];
node.child00 = GetNodeIndex();
node.child01 = GetNodeIndex();
node.child10 = GetNodeIndex();
node.child11 = GetNodeIndex();
nodes[i] = node;
nodes[i].Distribute ( nodes, r );
}
}
// Note, no else
if ( nodes[i].child00 != i ) {
// Not a leaf node
Vector2 c = r.center;
if (p.x > c.x) {
if (p.y > c.y) {
i = nodes[i].child11;
r = Rect.MinMaxRect ( c.x, c.y, r.xMax, r.yMax );
} else {
i = nodes[i].child10;
r = Rect.MinMaxRect ( c.x, r.yMin, r.xMax, c.y );
}
} else {
if (p.y > c.y) {
i = nodes[i].child01;
r = Rect.MinMaxRect ( r.xMin, c.y, c.x, r.yMax );
} else {
i = nodes[i].child00;
r = Rect.MinMaxRect ( r.xMin, r.yMin, c.x, c.y );
}
}
}
}
}
public void Distribute ( Node[] nodes, Rect r ) {
Vector2 c = r.center;
while (linkedList != null) {
Agent nx = linkedList.next;
if (linkedList.position.x > c.x) {
if (linkedList.position.z > c.y) {
nodes[child11].Add (linkedList);
} else {
nodes[child10].Add (linkedList);
}
} else {
if (linkedList.position.z > c.y) {
nodes[child01].Add (linkedList);
} else {
nodes[child00].Add (linkedList);
}
}
linkedList = nx;
}
count = 0;
}
public float InsertAgentNeighbour (Agent agent, float rangeSq) {
if (this == agent) return rangeSq;
if ( (agent.layer & collidesWith) == 0 ) return rangeSq;
//2D Dist
float dist = Sqr(agent.position.x-position.x) + Sqr(agent.position.z - position.z);
if (dist < rangeSq) {
if (neighbours.Count < maxNeighbours) {
neighbours.Add (agent);
neighbourDists.Add (dist);
}
int i = neighbours.Count-1;
if ( dist < neighbourDists[i] ) {
while ( i != 0 && dist < neighbourDists[i-1]) {
neighbours[i] = neighbours[i-1];
neighbourDists[i] = neighbourDists[i-1];
i--;
}
neighbours[i] = agent;
neighbourDists[i] = dist;
}
if (neighbours.Count == maxNeighbours) {
rangeSq = neighbourDists[neighbourDists.Count-1];
}
}
return rangeSq;
}
public void Add ( Agent agent ) {
agent.next = linkedList;
linkedList = agent;
}
float QueryRec ( int i, Vector2 p, float radius, Agent agent, Rect r ) {
if ( nodes[i].child00 == i ) {
// Leaf node
Agent a = nodes[i].linkedList;
while (a != null) {
float v = agent.InsertAgentNeighbour ( a, radius*radius );
if ( v < radius*radius ) {
radius = Mathf.Sqrt(v);
}
//Debug.DrawLine (a.position, new Vector3(p.x,0,p.y),Color.black);
/*float dist = (new Vector2(a.position.x, a.position.z) - p).sqrMagnitude;
if ( dist < radius*radius && a != agent ) {
}*/
a = a.next;
}
} else {
// Not a leaf node
Vector2 c = r.center;
if (p.x-radius < c.x) {
if (p.y-radius < c.y) {
radius = QueryRec ( nodes[i].child00, p, radius, agent, Rect.MinMaxRect ( r.xMin, r.yMin, c.x, c.y ) );
}
if (p.y+radius > c.y) {
radius = QueryRec ( nodes[i].child01, p, radius, agent, Rect.MinMaxRect ( r.xMin, c.y, c.x, r.yMax ) );
}
}
if (p.x+radius > c.x) {
if (p.y-radius < c.y) {
radius = QueryRec ( nodes[i].child10, p, radius, agent, Rect.MinMaxRect ( c.x, r.yMin, r.xMax, c.y ) );
}
if (p.y+radius > c.y) {
radius = QueryRec ( nodes[i].child11, p, radius, agent, Rect.MinMaxRect ( c.x, c.y, r.xMax, r.yMax ) );
}
}
}
return radius;
}
public void Query ( Vector2 p, float radius, Agent agent ) {
QueryRec ( 0, p, radius, agent, bounds );
}
/** Enqueue an agent at the specified position.
* You can use the returned interface to read several parameters such as position and velocity
* and set for example radius and desired velocity.
*
* \see RemoveAgent
*/
public IAgent AddAgent (Vector3 position) {
Agent agent = new Agent (position);
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering) for (int j=0;j<workers.Length;j++) workers[j].WaitOne();
agents.Add (agent);
agent.simulator = this;
return agent;
}
