1/*
2 * KdTree.cpp
3 * RVO2-3D Library
4 *
5 * Copyright 2008 University of North Carolina at Chapel Hill
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * https://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 * limitations under the License.
18 *
19 * Please send all bug reports to <geom@cs.unc.edu>.
20 *
21 * The authors may be contacted via:
22 *
23 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
24 * Dept. of Computer Science
25 * 201 S. Columbia St.
26 * Frederick P. Brooks, Jr. Computer Science Bldg.
27 * Chapel Hill, N.C. 27599-3175
28 * United States of America
29 *
30 * <https://gamma.cs.unc.edu/RVO2/>
31 */
32
33#include "KdTree3d.h"
34
35#include <algorithm>
36
37#include "Agent3d.h"
38#include "Definitions.h"
39#include "RVOSimulator3d.h"
40
41namespace RVO3D {
42 const size_t RVO3D_MAX_LEAF_SIZE = 10;
43
44 KdTree3D::KdTree3D(RVOSimulator3D *sim) : sim_(sim) { }
45
46 void KdTree3D::buildAgentTree(std::vector<Agent3D *> agents)
47 {
48 agents_.swap(agents);
49
50 if (!agents_.empty()) {
51 agentTree_.resize(2 * agents_.size() - 1);
52 buildAgentTreeRecursive(0, agents_.size(), 0);
53 }
54 }
55
56 void KdTree3D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
57 {
58 agentTree_[node].begin = begin;
59 agentTree_[node].end = end;
60 agentTree_[node].minCoord = agents_[begin]->position_;
61 agentTree_[node].maxCoord = agents_[begin]->position_;
62
63 for (size_t i = begin + 1; i < end; ++i) {
64 agentTree_[node].maxCoord[0] = std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
65 agentTree_[node].minCoord[0] = std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
66 agentTree_[node].maxCoord[1] = std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
67 agentTree_[node].minCoord[1] = std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
68 agentTree_[node].maxCoord[2] = std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
69 agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
70 }
71
72 if (end - begin > RVO3D_MAX_LEAF_SIZE) {
73 /* No leaf node. */
74 size_t coord;
75
76 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]) {
77 coord = 0;
78 }
79 else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
80 coord = 1;
81 }
82 else {
83 coord = 2;
84 }
85
86 const float splitValue = 0.5f * (agentTree_[node].maxCoord[coord] + agentTree_[node].minCoord[coord]);
87
88 size_t left = begin;
89
90 size_t right = end;
91
92 while (left < right) {
93 while (left < right && agents_[left]->position_[coord] < splitValue) {
94 ++left;
95 }
96
97 while (right > left && agents_[right - 1]->position_[coord] >= splitValue) {
98 --right;
99 }
100
101 if (left < right) {
102 std::swap(agents_[left], agents_[right - 1]);
103 ++left;
104 --right;
105 }
106 }
107
108 size_t leftSize = left - begin;
109
110 if (leftSize == 0) {
111 ++leftSize;
112 ++left;
113 ++right;
114 }
115
116 agentTree_[node].left = node + 1;
117 agentTree_[node].right = node + 2 * leftSize;
118
119 buildAgentTreeRecursive(begin, left, agentTree_[node].left);
120 buildAgentTreeRecursive(left, end, agentTree_[node].right);
121 }
122 }
123
124 void KdTree3D::computeAgentNeighbors(Agent3D *agent, float rangeSq) const
125 {
126 queryAgentTreeRecursive(agent, rangeSq, 0);
127 }
128
129 void KdTree3D::queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, size_t node) const
130 {
131 if (agentTree_[node].end - agentTree_[node].begin <= RVO3D_MAX_LEAF_SIZE) {
132 for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
133 agent->insertAgentNeighbor(agents_[i], rangeSq);
134 }
135 }
136 else {
137 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]));
138
139 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]));
140
141 if (distSqLeft < distSqRight) {
142 if (distSqLeft < rangeSq) {
143 queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
144
145 if (distSqRight < rangeSq) {
146 queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
147 }
148 }
149 }
150 else {
151 if (distSqRight < rangeSq) {
152 queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
153
154 if (distSqLeft < rangeSq) {
155 queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
156 }
157 }
158 }
159 }
160 }
161}
162