diff options
Diffstat (limited to 'thirdparty/rvo2/rvo2_2d/KdTree2d.cpp')
| -rw-r--r-- | thirdparty/rvo2/rvo2_2d/KdTree2d.cpp | 357 |
1 files changed, 0 insertions, 357 deletions
diff --git a/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp b/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp deleted file mode 100644 index 184bc74fe2..0000000000 --- a/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp +++ /dev/null @@ -1,357 +0,0 @@ -/* - * 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)); - } - } - } -} |