diff options
Diffstat (limited to 'thirdparty/rvo2/rvo2_2d/KdTree2d.cpp')
| -rw-r--r-- | thirdparty/rvo2/rvo2_2d/KdTree2d.cpp | 357 |
1 files changed, 357 insertions, 0 deletions
diff --git a/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp b/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp new file mode 100644 index 0000000000..184bc74fe2 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp @@ -0,0 +1,357 @@ +/* + * KdTree2d.cpp + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * 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 + * + * http://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 + * + * <http://gamma.cs.unc.edu/RVO2/> + */ + +#include "KdTree2d.h" + +#include "Agent2d.h" +#include "RVOSimulator2d.h" +#include "Obstacle2d.h" + +namespace RVO2D { + KdTree2D::KdTree2D(RVOSimulator2D *sim) : obstacleTree_(NULL), sim_(sim) { } + + KdTree2D::~KdTree2D() + { + deleteObstacleTree(obstacleTree_); + } + + void KdTree2D::buildAgentTree(std::vector<Agent2D *> agents) + { + agents_.swap(agents); + + if (!agents_.empty()) { + agentTree_.resize(2 * agents_.size() - 1); + buildAgentTreeRecursive(0, agents_.size(), 0); + } + } + + void KdTree2D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) + { + agentTree_[node].begin = begin; + agentTree_[node].end = end; + agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x(); + agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y(); + + for (size_t i = begin + 1; i < end; ++i) { + agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x()); + agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x()); + agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y()); + agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y()); + } + + if (end - begin > MAX_LEAF_SIZE) { + /* No leaf node. */ + const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY); + const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY)); + + size_t left = begin; + size_t right = end; + + while (left < right) { + while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) { + ++left; + } + + while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) { + --right; + } + + if (left < right) { + std::swap(agents_[left], agents_[right - 1]); + ++left; + --right; + } + } + + if (left == begin) { + ++left; + ++right; + } + + agentTree_[node].left = node + 1; + agentTree_[node].right = node + 2 * (left - begin); + + buildAgentTreeRecursive(begin, left, agentTree_[node].left); + buildAgentTreeRecursive(left, end, agentTree_[node].right); + } + } + + void KdTree2D::buildObstacleTree(std::vector<Obstacle2D *> obstacles) + { + deleteObstacleTree(obstacleTree_); + + obstacleTree_ = buildObstacleTreeRecursive(obstacles); + } + + + KdTree2D::ObstacleTreeNode *KdTree2D::buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &obstacles) + { + if (obstacles.empty()) { + return NULL; + } + else { + ObstacleTreeNode *const node = new ObstacleTreeNode; + + size_t optimalSplit = 0; + size_t minLeft = obstacles.size(); + size_t minRight = obstacles.size(); + + for (size_t i = 0; i < obstacles.size(); ++i) { + size_t leftSize = 0; + size_t rightSize = 0; + + const Obstacle2D *const obstacleI1 = obstacles[i]; + const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_; + + /* Compute optimal split node. */ + for (size_t j = 0; j < obstacles.size(); ++j) { + if (i == j) { + continue; + } + + const Obstacle2D *const obstacleJ1 = obstacles[j]; + const Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_; + + const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); + const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); + + if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { + ++leftSize; + } + else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { + ++rightSize; + } + else { + ++leftSize; + ++rightSize; + } + + if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { + break; + } + } + + if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { + minLeft = leftSize; + minRight = rightSize; + optimalSplit = i; + } + } + + /* Build split node. */ + std::vector<Obstacle2D *> leftObstacles(minLeft); + std::vector<Obstacle2D *> rightObstacles(minRight); + + size_t leftCounter = 0; + size_t rightCounter = 0; + const size_t i = optimalSplit; + + const Obstacle2D *const obstacleI1 = obstacles[i]; + const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_; + + for (size_t j = 0; j < obstacles.size(); ++j) { + if (i == j) { + continue; + } + + Obstacle2D *const obstacleJ1 = obstacles[j]; + Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_; + + const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); + const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); + + if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { + leftObstacles[leftCounter++] = obstacles[j]; + } + else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { + rightObstacles[rightCounter++] = obstacles[j]; + } + else { + /* Split obstacle j. */ + const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_); + + const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); + + Obstacle2D *const newObstacle = new Obstacle2D(); + newObstacle->point_ = splitpoint; + newObstacle->prevObstacle_ = obstacleJ1; + newObstacle->nextObstacle_ = obstacleJ2; + newObstacle->isConvex_ = true; + newObstacle->unitDir_ = obstacleJ1->unitDir_; + + newObstacle->id_ = sim_->obstacles_.size(); + + sim_->obstacles_.push_back(newObstacle); + + obstacleJ1->nextObstacle_ = newObstacle; + obstacleJ2->prevObstacle_ = newObstacle; + + if (j1LeftOfI > 0.0f) { + leftObstacles[leftCounter++] = obstacleJ1; + rightObstacles[rightCounter++] = newObstacle; + } + else { + rightObstacles[rightCounter++] = obstacleJ1; + leftObstacles[leftCounter++] = newObstacle; + } + } + } + + node->obstacle = obstacleI1; + node->left = buildObstacleTreeRecursive(leftObstacles); + node->right = buildObstacleTreeRecursive(rightObstacles); + return node; + } + } + + void KdTree2D::computeAgentNeighbors(Agent2D *agent, float &rangeSq) const + { + queryAgentTreeRecursive(agent, rangeSq, 0); + } + + void KdTree2D::computeObstacleNeighbors(Agent2D *agent, float rangeSq) const + { + queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); + } + + void KdTree2D::deleteObstacleTree(ObstacleTreeNode *node) + { + if (node != NULL) { + deleteObstacleTree(node->left); + deleteObstacleTree(node->right); + delete node; + } + } + + void KdTree2D::queryAgentTreeRecursive(Agent2D *agent, float &rangeSq, size_t node) const + { + if (agentTree_[node].end - agentTree_[node].begin <= 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].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY)); + + const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY)); + + 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); + } + } + } + + } + } + + void KdTree2D::queryObstacleTreeRecursive(Agent2D *agent, float rangeSq, const ObstacleTreeNode *node) const + { + if (node == NULL) { + return; + } + else { + const Obstacle2D *const obstacle1 = node->obstacle; + const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_; + + const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_); + + queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right)); + + const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_); + + if (distSqLine < rangeSq) { + if (agentLeftOfLine < 0.0f) { + /* + * Try obstacle at this node only if agent is on right side of + * obstacle (and can see obstacle). + */ + agent->insertObstacleNeighbor(node->obstacle, rangeSq); + } + + /* Try other side of line. */ + queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left)); + + } + } + } + + bool KdTree2D::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const + { + return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); + } + + bool KdTree2D::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const + { + if (node == NULL) { + return true; + } + else { + const Obstacle2D *const obstacle1 = node->obstacle; + const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_; + + const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1); + const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2); + const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_); + + if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { + return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right)); + } + else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { + return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left)); + } + else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { + /* One can see through obstacle from left to right. */ + return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right); + } + else { + const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_); + const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_); + const float invLengthQ = 1.0f / absSq(q2 - q1); + + return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right)); + } + } + } +} |