summaryrefslogtreecommitdiff
path: root/thirdparty/rvo2/KdTree.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/rvo2/KdTree.cpp')
-rw-r--r--thirdparty/rvo2/KdTree.cpp184
1 files changed, 96 insertions, 88 deletions
diff --git a/thirdparty/rvo2/KdTree.cpp b/thirdparty/rvo2/KdTree.cpp
index bc224614f0..c857f299df 100644
--- a/thirdparty/rvo2/KdTree.cpp
+++ b/thirdparty/rvo2/KdTree.cpp
@@ -8,7 +8,7 @@
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
- * http://www.apache.org/licenses/LICENSE-2.0
+ * https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
@@ -27,7 +27,7 @@
* Chapel Hill, N.C. 27599-3175
* United States of America
*
- * <http://gamma.cs.unc.edu/RVO2/>
+ * <https://gamma.cs.unc.edu/RVO2/>
*/
#include "KdTree.h"
@@ -38,115 +38,123 @@
#include "Definitions.h"
namespace RVO {
-const size_t RVO_MAX_LEAF_SIZE = 10;
+ const size_t RVO3D_MAX_LEAF_SIZE = 10;
-KdTree::KdTree() {}
+ KdTree::KdTree() { }
-void KdTree::buildAgentTree(std::vector<Agent *> agents) {
- agents_.swap(agents);
+ void KdTree::buildAgentTree(std::vector<Agent *> agents)
+ {
+ agents_.swap(agents);
- if (!agents_.empty()) {
- agentTree_.resize(2 * agents_.size() - 1);
- buildAgentTreeRecursive(0, agents_.size(), 0);
+ if (!agents_.empty()) {
+ agentTree_.resize(2 * agents_.size() - 1);
+ buildAgentTreeRecursive(0, agents_.size(), 0);
+ }
}
-}
-void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) {
- agentTree_[node].begin = begin;
- agentTree_[node].end = end;
- agentTree_[node].minCoord = agents_[begin]->position_;
- agentTree_[node].maxCoord = agents_[begin]->position_;
-
- for (size_t i = begin + 1; i < end; ++i) {
- agentTree_[node].maxCoord[0] = std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
- agentTree_[node].minCoord[0] = std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
- agentTree_[node].maxCoord[1] = std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
- agentTree_[node].minCoord[1] = std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
- agentTree_[node].maxCoord[2] = std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
- agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
- }
-
- if (end - begin > RVO_MAX_LEAF_SIZE) {
- /* No leaf node. */
- size_t coord;
-
- if (agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] && agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
- coord = 0;
- } else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
- coord = 1;
- } else {
- coord = 2;
- }
-
- const float splitValue = 0.5f * (agentTree_[node].maxCoord[coord] + agentTree_[node].minCoord[coord]);
-
- size_t left = begin;
-
- size_t right = end;
-
- while (left < right) {
- while (left < right && agents_[left]->position_[coord] < splitValue) {
- ++left;
- }
-
- while (right > left && agents_[right - 1]->position_[coord] >= splitValue) {
- --right;
+ void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
+ {
+ agentTree_[node].begin = begin;
+ agentTree_[node].end = end;
+ agentTree_[node].minCoord = agents_[begin]->position_;
+ agentTree_[node].maxCoord = agents_[begin]->position_;
+
+ for (size_t i = begin + 1; i < end; ++i) {
+ agentTree_[node].maxCoord[0] = std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
+ agentTree_[node].minCoord[0] = std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
+ agentTree_[node].maxCoord[1] = std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
+ agentTree_[node].minCoord[1] = std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
+ agentTree_[node].maxCoord[2] = std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
+ agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
+ }
+
+ if (end - begin > RVO3D_MAX_LEAF_SIZE) {
+ /* No leaf node. */
+ size_t coord;
+
+ if (agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] && agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
+ coord = 0;
+ }
+ else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
+ coord = 1;
+ }
+ else {
+ coord = 2;
}
- if (left < right) {
- std::swap(agents_[left], agents_[right - 1]);
- ++left;
- --right;
+ const float splitValue = 0.5f * (agentTree_[node].maxCoord[coord] + agentTree_[node].minCoord[coord]);
+
+ size_t left = begin;
+
+ size_t right = end;
+
+ while (left < right) {
+ while (left < right && agents_[left]->position_[coord] < splitValue) {
+ ++left;
+ }
+
+ while (right > left && agents_[right - 1]->position_[coord] >= splitValue) {
+ --right;
+ }
+
+ if (left < right) {
+ std::swap(agents_[left], agents_[right - 1]);
+ ++left;
+ --right;
+ }
}
- }
- size_t leftSize = left - begin;
+ size_t leftSize = left - begin;
- if (leftSize == 0) {
- ++leftSize;
- ++left;
- ++right;
- }
+ if (leftSize == 0) {
+ ++leftSize;
+ ++left;
+ ++right;
+ }
- agentTree_[node].left = node + 1;
- agentTree_[node].right = node + 2 * leftSize;
+ agentTree_[node].left = node + 1;
+ agentTree_[node].right = node + 2 * leftSize;
- buildAgentTreeRecursive(begin, left, agentTree_[node].left);
- buildAgentTreeRecursive(left, end, agentTree_[node].right);
+ buildAgentTreeRecursive(begin, left, agentTree_[node].left);
+ buildAgentTreeRecursive(left, end, agentTree_[node].right);
+ }
}
-}
-void KdTree::computeAgentNeighbors(Agent *agent, float rangeSq) const {
- queryAgentTreeRecursive(agent, rangeSq, 0);
-}
+ void KdTree::computeAgentNeighbors(Agent *agent, float rangeSq) const
+ {
+ queryAgentTreeRecursive(agent, rangeSq, 0);
+ }
-void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const {
- if (agentTree_[node].end - agentTree_[node].begin <= RVO_MAX_LEAF_SIZE) {
- for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
- agent->insertAgentNeighbor(agents_[i], rangeSq);
+ void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const
+ {
+ if (agentTree_[node].end - agentTree_[node].begin <= RVO3D_MAX_LEAF_SIZE) {
+ for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
+ agent->insertAgentNeighbor(agents_[i], rangeSq);
+ }
}
- } else {
- const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].left].maxCoord[2]));
+ else {
+ const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].left].maxCoord[2]));
- const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].right].maxCoord[2]));
+ const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].right].maxCoord[2]));
- if (distSqLeft < distSqRight) {
- if (distSqLeft < rangeSq) {
- queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+ if (distSqLeft < distSqRight) {
+ if (distSqLeft < rangeSq) {
+ queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+ if (distSqRight < rangeSq) {
+ queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
+ }
+ }
+ }
+ else {
if (distSqRight < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
- }
- }
- } else {
- if (distSqRight < rangeSq) {
- queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
-
- if (distSqLeft < rangeSq) {
- queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+
+ if (distSqLeft < rangeSq) {
+ queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
+ }
}
}
}
}
}
-} // namespace RVO
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-13 10:08:34 +0000