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