/**
* <summary>Recursive method for computing the agent neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which agent neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void queryAgentTreeRecursive(Agent agent, ref float rangeSq, int node)
{
if (agentTree[node].end - agentTree[node].begin <= MAXLEAFSIZE)
{
for (int i = agentTree[node].begin; i < agentTree[node].end; ++i)
{
agent.insertAgentNeighbor(agents[i], ref rangeSq);
}
}
else
{
float distSqLeft = RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].left].minCoord[0] - agent.position.x)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.x - agentTree[agentTree[node].left].maxCoord[0])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].left].minCoord[1] - agent.position.y)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.y - agentTree[agentTree[node].left].maxCoord[1])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].left].minCoord[2] - agent.position.z)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.z - agentTree[agentTree[node].left].maxCoord[2]));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[0] - agent.position.x)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.x - agentTree[agentTree[node].right].maxCoord[0])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[1] - agent.position.y)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.y - agentTree[agentTree[node].right].maxCoord[1])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[2] - agent.position.z)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.z - agentTree[agentTree[node].right].maxCoord[2]));
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);
}
}
}
}
}
/**
* <summary>Recursive method for computing the agent neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which agent neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void queryAgentTreeRecursive(Agent agent, ref float rangeSq, int node)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
agent.insertAgentNeighbor(agents_[i], ref rangeSq);
}
}
else
{
//在范围内部返回零，在外边返回距离矩形的最短长度平方
float distSqLeft = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minX_ - agent.position_.x_))
+ RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].left_].maxX_))
+ RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minY_ - agent.position_.y_))
+ RVOMath.sqr(Math.Max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].left_].maxY_));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minX_ - agent.position_.x_))
+ RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].right_].maxX_))
+ RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minY_ - agent.position_.y_))
+ RVOMath.sqr(Math.Max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].right_].maxY_));
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_);
}
}
}
}
}
/**
* <summary>Recursive method for computing the agent neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which agent neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void queryAgentTreeRecursive(Agent agent, ref KInt rangeSq, int node)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
agent.insertAgentNeighbor(agents_[i], ref rangeSq);
}
}
else
{
KInt distSqLeft = RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].left_].minx, agent.position_.IntX)) + RVOMath.sqr(ReduceMax(agent.position_.IntX, agentTree_[agentTree_[node].left_].maxx)) + RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].left_].miny, agent.position_.IntY)) + RVOMath.sqr(ReduceMax(agent.position_.IntY, agentTree_[agentTree_[node].left_].maxy));
KInt distSqRight = RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].right_].minx, agent.position_.IntX)) + RVOMath.sqr(ReduceMax(agent.position_.IntX, agentTree_[agentTree_[node].right_].maxx)) + RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].right_].miny, agent.position_.IntY)) + RVOMath.sqr(ReduceMax(agent.position_.IntY, agentTree_[agentTree_[node].right_].maxy));
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_);
}
}
}
}
}
void queryAgentTreeRecursive(Agent agent, ref float rangeSq, int node)
{
if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin; i < agentTree_[node].end; ++i)
{
agent.insertAgentNeighbor(agents_[i], ref rangeSq);
}
}
else
{
float distSqLeft = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left].minX - agent.position_.x_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].left].maxX)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left].minY - agent.position_.y_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].left].maxY));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right].minX - agent.position_.x_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].right].maxX)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right].minY - agent.position_.y_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].right].maxY));
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);
}
}
}
}
}
/**
* <summary>Recursive method for computing the agent neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which agent neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void queryAgentTreeRecursive(Agent agent, ref Fix64 rangeSq, int node)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
agent.insertAgentNeighbor(agents_[i], ref rangeSq);
}
}
else
{
Fix64 distSqLeft = RVOMath.sqr(MathEx.Max(0.0f, agentTree_[agentTree_[node].left_].minX_ - agent.position_.x)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agent.position_.x - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agentTree_[agentTree_[node].left_].minY_ - agent.position_.y)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agent.position_.y - agentTree_[agentTree_[node].left_].maxY_));
Fix64 distSqRight = RVOMath.sqr(MathEx.Max(0.0f, agentTree_[agentTree_[node].right_].minX_ - agent.position_.x)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agent.position_.x - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agentTree_[agentTree_[node].right_].minY_ - agent.position_.y)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agent.position_.y - agentTree_[agentTree_[node].right_].maxY_));
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_);
}
}
}
}
}
/**
* <summary>Recursive method for computing the agent neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which agent neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void queryAgentTreeRecursive(Agent agent, ref float rangeSq, int node)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
agent.insertAgentNeighbor(agents_[i], ref rangeSq);
}
}
else
{
float distSqLeft = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minX_ - agent.position_.x_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minY_ - agent.position_.y_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].left_].maxY_));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minX_ - agent.position_.x_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minY_ - agent.position_.y_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].right_].maxY_));
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_);
}
}
}
}
}
void queryAgentTreeRecursive(Agent agent, ref float rangeSq, int node)
{
if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin; i < agentTree_[node].end; ++i)
{
agent.insertAgentNeighbor(agents_[i], ref rangeSq);
}
}
else
{
float distSqLeft = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left].minX - agent.position_.X)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.X - agentTree_[agentTree_[node].left].maxX)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left].minY - agent.position_.Y)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.Y - agentTree_[agentTree_[node].left].maxY));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right].minX - agent.position_.X)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.X - agentTree_[agentTree_[node].right].maxX)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right].minY - agent.position_.Y)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.Y - agentTree_[agentTree_[node].right].maxY));
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);
}
}
}
}
}