private void queryPointTreeRecursive(Vector2 _pos, double _range, int node, List <int> list)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
Agent agent = agents_[i];
if (agent.type == AgentType.SkillPush || agent.type == AgentType.SkillObstacle || agent.type == AgentType.SkillUnit)
{
continue;
}
double dist = Vector2.Distance(agent.position_, _pos);
if (dist < _range + agent.radius_)
{
list.Add(agent.uid);
}
}
}
else
{
double distSqLeft = RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].left_].minX_ - _pos.x)) + RVOMath.sqr(Math.Max(0.0, _pos.x - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].left_].minY_ - _pos.y)) + RVOMath.sqr(Math.Max(0.0, _pos.y - agentTree_[agentTree_[node].left_].maxY_));
double distSqRight = RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].right_].minX_ - _pos.x)) + RVOMath.sqr(Math.Max(0.0, _pos.x - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].right_].minY_ - _pos.y)) + RVOMath.sqr(Math.Max(0.0, _pos.y - agentTree_[agentTree_[node].right_].maxY_));
double rangeSq = (_range + simulator.getMaxRadius()) * (_range + simulator.getMaxRadius());
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
queryPointTreeRecursive(_pos, _range, agentTree_[node].left_, list);
if (distSqRight < rangeSq)
{
queryPointTreeRecursive(_pos, _range, agentTree_[node].right_, list);
}
}
}
else
{
if (distSqRight < rangeSq)
{
queryPointTreeRecursive(_pos, _range, agentTree_[node].right_, list);
if (distSqLeft < rangeSq)
{
queryPointTreeRecursive(_pos, _range, agentTree_[node].left_, list);
}
}
}
}
}
