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); } } } } }