|
public InsertAgentNeighbour ( |
||
| agent | ||
| rangeSq | float | |
| return | float | |
public void QueryRec(int i, Rect 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)
{
float v = agent.InsertAgentNeighbour(a, radius * radius);
// Limit the search if the agent has hit the max number of nearby agents threshold
if (v < maxRadius * maxRadius)
{
maxRadius = Mathf.Sqrt(v);
}
}
}
else
{
// Not a leaf node
Vector2 c = r.center;
if (p.x - radius < c.x)
{
if (p.y - radius < c.y)
{
QueryRec(nodes[i].child00 + 0, Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y));
radius = System.Math.Min(radius, maxRadius);
}
if (p.y + radius > c.y)
{
QueryRec(nodes[i].child00 + 1, Rect.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)
{
QueryRec(nodes[i].child00 + 2, Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y));
radius = System.Math.Min(radius, maxRadius);
}
if (p.y + radius > c.y)
{
QueryRec(nodes[i].child00 + 3, Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax));
}
}
}
}
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);
}
private void QueryAgentTreeRecursive(Agent agent, ref float rangeSq, int node)
{
if (agentTree[node].end - agentTree[node].start <= MAX_LEAF_SIZE)
{
for (int i = agentTree[node].start; i < agentTree[node].end; i++)
{
rangeSq = agent.InsertAgentNeighbour(agents[i], rangeSq);
}
}
else
{
//Calculate squared distance to the closest point inside the bounding boxes of the child nodes
float distSqLeft = Sqr(Math.Max(0.0f, agentTree[agentTree[node].left].xmin - agent.position.x))
+ Sqr(Math.Max(0.0f, agent.position.x - agentTree[agentTree[node].left].xmax))
+ Sqr(Math.Max(0.0f, agentTree[agentTree[node].left].ymin - agent.position.z))
+ Sqr(Math.Max(0.0f, agent.position.z - agentTree[agentTree[node].left].ymax));
float distSqRight = Sqr(Math.Max(0.0f, agentTree[agentTree[node].right].xmin - agent.position.x))
+ Sqr(Math.Max(0.0f, agent.position.x - agentTree[agentTree[node].right].xmax))
+ Sqr(Math.Max(0.0f, agentTree[agentTree[node].right].ymin - agent.position.z))
+ Sqr(Math.Max(0.0f, agent.position.z - agentTree[agentTree[node].right].ymax));
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
QueryAgentTreeRecursive(agent, ref rangeSq, agentTree[node].left);
if (distSqRight < rangeSq)
{
QueryAgentTreeRecursive(agent, ref rangeSq, agentTree[node].right);
}
}
}
else
{
if (distSqRight < rangeSq)
{
QueryAgentTreeRecursive(agent, ref rangeSq, agentTree[node].right);
if (distSqLeft < rangeSq)
{
QueryAgentTreeRecursive(agent, ref rangeSq, agentTree[node].left);
}
}
}
}
}
// Token: 0x0600030A RID: 778 RVA: 0x00018310 File Offset: 0x00016710
private float QueryRec(int i, Vector2 p, float radius, Agent agent, Rect r)
{
if (this.nodes[i].child00 == i)
{
for (Agent agent2 = this.nodes[i].linkedList; agent2 != null; agent2 = agent2.next)
{
float num = agent.InsertAgentNeighbour(agent2, radius * radius);
if (num < radius * radius)
{
radius = Mathf.Sqrt(num);
}
}
}
else
{
Vector2 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, Rect.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, Rect.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, Rect.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, Rect.MinMaxRect(center.x, center.y, r.xMax, r.yMax));
}
}
}
return(radius);
}
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 num = agent.InsertAgentNeighbour(agent2, radius * radius);
if (num < radius * radius)
{
radius = (long)IntMath.Sqrt(num);
}
}
}
else
{
VInt2 center = r.center;
if ((long)p.x - radius < (long)center.x)
{
if ((long)p.y - radius < (long)center.y)
{
radius = this.QueryRec(this.nodes[i].child00, p, radius, agent, VRect.MinMaxRect(r.xMin, r.yMin, center.x, center.y));
}
if ((long)p.y + radius > (long)center.y)
{
radius = this.QueryRec(this.nodes[i].child01, p, radius, agent, VRect.MinMaxRect(r.xMin, center.y, center.x, r.yMax));
}
}
if ((long)p.x + radius > (long)center.x)
{
if ((long)p.y - radius < (long)center.y)
{
radius = this.QueryRec(this.nodes[i].child10, p, radius, agent, VRect.MinMaxRect(center.x, r.yMin, r.xMax, center.y));
}
if ((long)p.y + radius > (long)center.y)
{
radius = this.QueryRec(this.nodes[i].child11, p, radius, agent, VRect.MinMaxRect(center.x, center.y, r.xMax, r.yMax));
}
}
}
return(radius);
}
private void QueryAgentTreeRecursive (Agent agent, ref float rangeSq, int node) {
if (agentTree[node].end - agentTree[node].start <= MAX_LEAF_SIZE) {
for (int i = agentTree[node].start; i < agentTree[node].end; i++) {
rangeSq = agent.InsertAgentNeighbour (agents[i], rangeSq);
}
} else {
//Calculate squared distance to the closest point inside the bounding boxes of the child nodes
float distSqLeft = Sqr (Math.Max (0.0f, agentTree[agentTree[node].left].xmin - agent.position.x))
+ Sqr (Math.Max (0.0f, agent.position.x - agentTree[agentTree[node].left].xmax))
+ Sqr (Math.Max (0.0f, agentTree[agentTree[node].left].ymin - agent.position.z))
+ Sqr (Math.Max (0.0f, agent.position.z - agentTree[agentTree[node].left].ymax));
float distSqRight =Sqr (Math.Max (0.0f, agentTree[agentTree[node].right].xmin - agent.position.x))
+ Sqr (Math.Max (0.0f, agent.position.x - agentTree[agentTree[node].right].xmax))
+ Sqr (Math.Max (0.0f, agentTree[agentTree[node].right].ymin - agent.position.z))
+ Sqr (Math.Max (0.0f, agent.position.z - agentTree[agentTree[node].right].ymax));
if (distSqLeft < distSqRight) {
if (distSqLeft < rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, agentTree[node].left);
if (distSqRight < rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, agentTree[node].right);
}
}
} else {
if (distSqRight < rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, agentTree[node].right);
if (distSqLeft < rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, agentTree[node].left);
}
}
}
}
}
