diff options
Diffstat (limited to 'thirdparty/rvo2/rvo2_3d/KdTree3d.cc')
| -rw-r--r-- | thirdparty/rvo2/rvo2_3d/KdTree3d.cc | 264 |
1 files changed, 264 insertions, 0 deletions
diff --git a/thirdparty/rvo2/rvo2_3d/KdTree3d.cc b/thirdparty/rvo2/rvo2_3d/KdTree3d.cc new file mode 100644 index 0000000000..ac64d1560a --- /dev/null +++ b/thirdparty/rvo2/rvo2_3d/KdTree3d.cc @@ -0,0 +1,264 @@ +/* + * KdTree3d.cc + * RVO2-3D Library + * + * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * 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, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to <geom@cs.unc.edu>. + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * <https://gamma.cs.unc.edu/RVO2/> + */ + +#include "KdTree3d.h" + +#include <algorithm> +#include <utility> + +#include "Agent3d.h" +#include "RVOSimulator3d.h" +#include "Vector3.h" + +namespace RVO3D { +namespace { +/** + * @brief The maximum size of a k-D leaf node. + */ +const std::size_t RVO3D_MAX_LEAF_SIZE = 10U; +} /* namespace */ + +/** + * @brief Defines an agent k-D tree node. + */ +class KdTree3D::AgentTreeNode { + public: + /** + * @brief Constructs an agent k-D tree node. + */ + AgentTreeNode(); + + /** + * @brief The beginning node number. + */ + std::size_t begin; + + /** + * @brief The ending node number. + */ + std::size_t end; + + /** + * @brief The left node number. + */ + std::size_t left; + + /** + * @brief The right node number. + */ + std::size_t right; + + /** + * @brief The maximum coordinates. + */ + Vector3 maxCoord; + + /** + * @brief The minimum coordinates. + */ + Vector3 minCoord; +}; + +KdTree3D::AgentTreeNode::AgentTreeNode() + : begin(0U), end(0U), left(0U), right(0U) {} + +KdTree3D::KdTree3D(RVOSimulator3D *sim) : sim_(sim) {} + +KdTree3D::~KdTree3D() {} + +void KdTree3D::buildAgentTree(std::vector<Agent3D *> agents) { + agents_.swap(agents_); + + if (!agents_.empty()) { + agentTree_.resize(2U * agents_.size() - 1U); + buildAgentTreeRecursive(0U, agents_.size(), 0U); + } +} + +void KdTree3D::buildAgentTreeRecursive(std::size_t begin, std::size_t end, + std::size_t node) { + agentTree_[node].begin = begin; + agentTree_[node].end = end; + agentTree_[node].minCoord = agents_[begin]->position_; + agentTree_[node].maxCoord = agents_[begin]->position_; + + for (std::size_t i = begin + 1U; 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. */ + std::size_t coord = 0U; + + 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 = 0U; + } else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > + agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) { + coord = 1U; + } else { + coord = 2U; + } + + const float splitValue = 0.5F * (agentTree_[node].maxCoord[coord] + + agentTree_[node].minCoord[coord]); + + std::size_t left = begin; + + std::size_t right = end; + + while (left < right) { + while (left < right && agents_[left]->position_[coord] < splitValue) { + ++left; + } + + while (right > left && + agents_[right - 1U]->position_[coord] >= splitValue) { + --right; + } + + if (left < right) { + std::swap(agents_[left], agents_[right - 1U]); + ++left; + --right; + } + } + + std::size_t leftSize = left - begin; + + if (leftSize == 0U) { + ++leftSize; + ++left; + } + + agentTree_[node].left = node + 1U; + agentTree_[node].right = node + 2U * leftSize; + + buildAgentTreeRecursive(begin, left, agentTree_[node].left); + buildAgentTreeRecursive(left, end, agentTree_[node].right); + } +} + +void KdTree3D::computeAgentNeighbors(Agent3D *agent, float rangeSq) const { + queryAgentTreeRecursive(agent, rangeSq, 0U); +} + +void KdTree3D::queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, + std::size_t node) const { + if (agentTree_[node].end - agentTree_[node].begin <= RVO3D_MAX_LEAF_SIZE) { + for (std::size_t i = agentTree_[node].begin; i < agentTree_[node].end; + ++i) { + agent->insertAgentNeighbor(agents_[i], rangeSq); + } + } else { + const float distSqLeftMinX = + std::max(0.0F, agentTree_[agentTree_[node].left].minCoord[0] - + agent->position_.x()); + const float distSqLeftMaxX = + std::max(0.0F, agent->position_.x() - + agentTree_[agentTree_[node].left].maxCoord[0]); + const float distSqLeftMinY = + std::max(0.0F, agentTree_[agentTree_[node].left].minCoord[1] - + agent->position_.y()); + const float distSqLeftMaxY = + std::max(0.0F, agent->position_.y() - + agentTree_[agentTree_[node].left].maxCoord[1]); + const float distSqLeftMinZ = + std::max(0.0F, agentTree_[agentTree_[node].left].minCoord[2] - + agent->position_.z()); + const float distSqLeftMaxZ = + std::max(0.0F, agent->position_.z() - + agentTree_[agentTree_[node].left].maxCoord[2]); + + const float distSqLeft = + distSqLeftMinX * distSqLeftMinX + distSqLeftMaxX * distSqLeftMaxX + + distSqLeftMinY * distSqLeftMinY + distSqLeftMaxY * distSqLeftMaxY + + distSqLeftMinZ * distSqLeftMinZ + distSqLeftMaxZ * distSqLeftMaxZ; + + const float distSqRightMinX = + std::max(0.0F, agentTree_[agentTree_[node].right].minCoord[0] - + agent->position_.x()); + const float distSqRightMaxX = + std::max(0.0F, agent->position_.x() - + agentTree_[agentTree_[node].right].maxCoord[0]); + const float distSqRightMinY = + std::max(0.0F, agentTree_[agentTree_[node].right].minCoord[1] - + agent->position_.y()); + const float distSqRightMaxY = + std::max(0.0F, agent->position_.y() - + agentTree_[agentTree_[node].right].maxCoord[1]); + const float distSqRightMinZ = + std::max(0.0F, agentTree_[agentTree_[node].right].minCoord[2] - + agent->position_.z()); + const float distSqRightMaxZ = + std::max(0.0F, agent->position_.z() - + agentTree_[agentTree_[node].right].maxCoord[2]); + + const float distSqRight = + distSqRightMinX * distSqRightMinX + distSqRightMaxX * distSqRightMaxX + + distSqRightMinY * distSqRightMinY + distSqRightMaxY * distSqRightMaxY + + distSqRightMinZ * distSqRightMinZ + distSqRightMaxZ * distSqRightMaxZ; + + 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); + + if (distSqLeft < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); + } + } + } + } +} +} /* namespace RVO3D */ |