void BuildQuadtree()
{
Quadtree.Clear();
if (agents.Count > 0)
{
//GG
//Rect bounds = Rect.MinMaxRect(agents[0].position.x, agents[0].position.y, agents[0].position.x, agents[0].position.y);
VRect bounds = VRect.MinMaxRect(agents[0].position.x, agents[0].position.y, agents[0].position.x, agents[0].position.y);
for (int i = 1; i < agents.Count; i++)
{
//GG
/*Vector2 p = agents[i].position;
* bounds = Rect.MinMaxRect(Mathf.Min(bounds.xMin, p.x), Mathf.Min(bounds.yMin, p.y), Mathf.Max(bounds.xMax, p.x), Mathf.Max(bounds.yMax, p.y));*/
VInt2 p = agents[i].position;
bounds = VRect.MinMaxRect(Mathf.Min(bounds.xMin, p.x), Mathf.Min(bounds.yMin, p.y), Mathf.Max(bounds.xMax, p.x), Mathf.Max(bounds.yMax, p.y));
}
Quadtree.SetBounds(bounds);
for (int i = 0; i < agents.Count; i++)
{
Quadtree.Insert(agents[i]);
}
//quadtree.DebugDraw ();
}
Quadtree.CalculateSpeeds();
}
public void Insert(Agent agent)
{
int index = 0;
VRect bounds = this.bounds;
VInt2 xz = agent.position.xz;
agent.next = null;
this.maxRadius = IntMath.Max((long)agent.radius.i, this.maxRadius);
int num3 = 0;
while (true)
{
num3++;
if (this.nodes[index].child00 == index)
{
if ((this.nodes[index].count < 15) || (num3 > 10))
{
this.nodes[index].Add(agent);
this.nodes[index].count = (byte)(this.nodes[index].count + 1);
return;
}
Node node = this.nodes[index];
node.child00 = this.GetNodeIndex();
node.child01 = this.GetNodeIndex();
node.child10 = this.GetNodeIndex();
node.child11 = this.GetNodeIndex();
this.nodes[index] = node;
this.nodes[index].Distribute(this.nodes, bounds);
}
if (this.nodes[index].child00 != index)
{
VInt2 center = bounds.center;
if (xz.x > center.x)
{
if (xz.y > center.y)
{
index = this.nodes[index].child11;
bounds = VRect.MinMaxRect(center.x, center.y, bounds.xMax, bounds.yMax);
}
else
{
index = this.nodes[index].child10;
bounds = VRect.MinMaxRect(center.x, bounds.yMin, bounds.xMax, center.y);
}
}
else if (xz.y > center.y)
{
index = this.nodes[index].child01;
bounds = VRect.MinMaxRect(bounds.xMin, center.y, center.x, bounds.yMax);
}
else
{
index = this.nodes[index].child00;
bounds = VRect.MinMaxRect(bounds.xMin, bounds.yMin, center.x, center.y);
}
}
}
}
private void BuildQuadtree()
{
this.quadtree.Clear();
if (this.agents.get_Count() > 0)
{
VRect bounds = VRect.MinMaxRect(this.agents.get_Item(0).position.x, this.agents.get_Item(0).position.y, this.agents.get_Item(0).position.x, this.agents.get_Item(0).position.y);
for (int i = 1; i < this.agents.get_Count(); i++)
{
VInt3 position = this.agents.get_Item(i).position;
bounds = VRect.MinMaxRect(Mathf.Min(bounds.xMin, position.x), Mathf.Min(bounds.yMin, position.z), Mathf.Max(bounds.xMax, position.x), Mathf.Max(bounds.yMax, position.z));
}
this.quadtree.SetBounds(bounds);
for (int j = 0; j < this.agents.get_Count(); j++)
{
this.quadtree.Insert(this.agents.get_Item(j));
}
}
}
private void BuildQuadtree()
{
this.quadtree.Clear();
if (this.agents.Count > 0)
{
VRect r = VRect.MinMaxRect(this.agents[0].position.x, this.agents[0].position.y, this.agents[0].position.x, this.agents[0].position.y);
for (int i = 1; i < this.agents.Count; i++)
{
Int3 position = this.agents[i].position;
r = VRect.MinMaxRect(Mathf.Min(r.xMin, position.x), Mathf.Min(r.yMin, position.z), Mathf.Max(r.xMax, position.x), Mathf.Max(r.yMax, position.z));
}
this.quadtree.SetBounds(r);
for (int j = 0; j < this.agents.Count; j++)
{
this.quadtree.Insert(this.agents[j]);
}
}
}
private long QueryRec(int i, VInt2 p, long radius, Agent agent, VRect r)
{
if (this.nodes[i].child00 == i)
{
for (Agent agent2 = this.nodes[i].linkedList; agent2 != null; agent2 = agent2.next)
{
long a = agent.InsertAgentNeighbour(agent2, radius * radius);
if (a < (radius * radius))
{
radius = IntMath.Sqrt(a);
}
}
return(radius);
}
VInt2 center = r.center;
if ((p.x - radius) < center.x)
{
if ((p.y - radius) < center.y)
{
radius = this.QueryRec(this.nodes[i].child00, p, radius, agent, VRect.MinMaxRect(r.xMin, r.yMin, center.x, center.y));
}
if ((p.y + radius) > center.y)
{
radius = this.QueryRec(this.nodes[i].child01, p, radius, agent, VRect.MinMaxRect(r.xMin, center.y, center.x, r.yMax));
}
}
if ((p.x + radius) > center.x)
{
if ((p.y - radius) < center.y)
{
radius = this.QueryRec(this.nodes[i].child10, p, radius, agent, VRect.MinMaxRect(center.x, r.yMin, r.xMax, center.y));
}
if ((p.y + radius) > center.y)
{
radius = this.QueryRec(this.nodes[i].child11, p, radius, agent, VRect.MinMaxRect(center.x, center.y, r.xMax, r.yMax));
}
}
return(radius);
}
public void Insert(Agent agent)
{
int num = 0;
VRect r = this.bounds;
VInt2 xz = agent.position.xz;
agent.next = null;
this.maxRadius = IntMath.Max((long)agent.radius.i, this.maxRadius);
int num2 = 0;
while (true)
{
num2++;
if (this.nodes[num].child00 == num)
{
if (this.nodes[num].count < 15 || num2 > 10)
{
break;
}
RVOQuadtree.Node node = this.nodes[num];
node.child00 = this.GetNodeIndex();
node.child01 = this.GetNodeIndex();
node.child10 = this.GetNodeIndex();
node.child11 = this.GetNodeIndex();
this.nodes[num] = node;
this.nodes[num].Distribute(this.nodes, r);
}
if (this.nodes[num].child00 != num)
{
VInt2 center = r.center;
if (xz.x > center.x)
{
if (xz.y > center.y)
{
num = this.nodes[num].child11;
r = VRect.MinMaxRect(center.x, center.y, r.xMax, r.yMax);
}
else
{
num = this.nodes[num].child10;
r = VRect.MinMaxRect(center.x, r.yMin, r.xMax, center.y);
}
}
else if (xz.y > center.y)
{
num = this.nodes[num].child01;
r = VRect.MinMaxRect(r.xMin, center.y, center.x, r.yMax);
}
else
{
num = this.nodes[num].child00;
r = VRect.MinMaxRect(r.xMin, r.yMin, center.x, center.y);
}
}
}
this.nodes[num].Add(agent);
RVOQuadtree.Node[] array = this.nodes;
int num3 = num;
array[num3].count = array[num3].count + 1;
}
//GG
//public void QueryRec(int i, Rect r)
public void QueryRec(int i, VRect r)
{
// Determine the radius that we need to search to take all agents into account
// Note: the second agentRadius usage should actually be the radius of the other agents, not this agent
// but for performance reasons and for simplicity we assume that agents have approximately the same radius.
// Thus an agent with a very small radius may in some cases detect an agent with a very large radius too late
// however this effect should be minor.
var radius = System.Math.Min(System.Math.Max((nodes[i].maxSpeed + speed) * timeHorizon, agentRadius) + agentRadius, maxRadius);
if (nodes[i].child00 == i)
{
// Leaf node
for (Agent a = nodes[i].linkedList; a != null; a = a.next)
{
//GG
//float v = agent.InsertAgentNeighbour(a, radius * radius);
long v = agent.InsertAgentNeighbour(a, (int)(radius * radius));
// Limit the search if the agent has hit the max number of nearby agents threshold
if (v < maxRadius * maxRadius)
{
//GG
//maxRadius = Mathf.Sqrt(v);
maxRadius = IntMath.Sqrt(v);
}
}
}
else
{
// Not a leaf node
//GG
//Vector2 c = r.center;
VInt2 c = r.center;
if (p.x - radius < c.x)
{
if (p.y - radius < c.y)
{
//GG
//QueryRec(nodes[i].child00, Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y));
QueryRec(nodes[i].child00, VRect.MinMaxRect(r.xMin, r.yMin, c.x, c.y));
radius = System.Math.Min(radius, maxRadius);
}
if (p.y + radius > c.y)
{
//GG
//QueryRec(nodes[i].child01, Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax));
QueryRec(nodes[i].child01, VRect.MinMaxRect(r.xMin, c.y, c.x, r.yMax));
radius = System.Math.Min(radius, maxRadius);
}
}
if (p.x + radius > c.x)
{
if (p.y - radius < c.y)
{
//GG
//QueryRec(nodes[i].child10, Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y));
QueryRec(nodes[i].child10, VRect.MinMaxRect(c.x, r.yMin, r.xMax, c.y));
radius = System.Math.Min(radius, maxRadius);
}
if (p.y + radius > c.y)
{
//GG
//QueryRec(nodes[i].child11, Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax));
QueryRec(nodes[i].child11, VRect.MinMaxRect(c.x, c.y, r.xMax, r.yMax));
}
}
}
}
/** Add a new agent to the tree.
* \warning Agents must not be added multiple times to the same tree
*/
public void Insert(Agent agent)
{
int i = 0;
//GG
/*Rect r = bounds;
* Vector2 p = new Vector2(agent.position.x, agent.position.y);*/
VRect r = bounds;
VInt2 p = new VInt2(agent.position.x, agent.position.y);
agent.next = null;
//GG
//maxRadius = System.Math.Max(agent.radius, maxRadius);
maxRadius = System.Math.Max((int)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
//GG
//Vector2 c = r.center;
VInt2 c = r.center;
if (p.x > c.x)
{
if (p.y > c.y)
{
i = nodes[i].child11;
//GG
//r = Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax);
r = VRect.MinMaxRect(c.x, c.y, r.xMax, r.yMax);
}
else
{
i = nodes[i].child10;
//GG
//r = Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y);
r = VRect.MinMaxRect(c.x, r.yMin, r.xMax, c.y);
}
}
else
{
if (p.y > c.y)
{
i = nodes[i].child01;
//GG
//r = Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax);
r = VRect.MinMaxRect(r.xMin, c.y, c.x, r.yMax);
}
else
{
i = nodes[i].child00;
//GG
//r = Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y);
r = VRect.MinMaxRect(r.xMin, r.yMin, c.x, c.y);
}
}
}
}
}