1/*
2 * KdTree2d.cpp
3 * RVO2 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 * http://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 * <http://gamma.cs.unc.edu/RVO2/>
31 */
32
33#include "KdTree2d.h"
34
35#include "Agent2d.h"
36#include "RVOSimulator2d.h"
37#include "Obstacle2d.h"
38
39namespace RVO2D {
40 KdTree2D::KdTree2D(RVOSimulator2D *sim) : obstacleTree_(NULL), sim_(sim) { }
41
42 KdTree2D::~KdTree2D()
43 {
44 deleteObstacleTree(obstacleTree_);
45 }
46
47 void KdTree2D::buildAgentTree(std::vector<Agent2D *> agents)
48 {
49 agents_.swap(agents);
50
51 if (!agents_.empty()) {
52 agentTree_.resize(2 * agents_.size() - 1);
53 buildAgentTreeRecursive(0, agents_.size(), 0);
54 }
55 }
56
57 void KdTree2D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
58 {
59 agentTree_[node].begin = begin;
60 agentTree_[node].end = end;
61 agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x();
62 agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y();
63
64 for (size_t i = begin + 1; i < end; ++i) {
65 agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x());
66 agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x());
67 agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y());
68 agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y());
69 }
70
71 if (end - begin > MAX_LEAF_SIZE) {
72 /* No leaf node. */
73 const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY);
74 const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY));
75
76 size_t left = begin;
77 size_t right = end;
78
79 while (left < right) {
80 while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) {
81 ++left;
82 }
83
84 while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) {
85 --right;
86 }
87
88 if (left < right) {
89 std::swap(agents_[left], agents_[right - 1]);
90 ++left;
91 --right;
92 }
93 }
94
95 if (left == begin) {
96 ++left;
97 ++right;
98 }
99
100 agentTree_[node].left = node + 1;
101 agentTree_[node].right = node + 2 * (left - begin);
102
103 buildAgentTreeRecursive(begin, left, agentTree_[node].left);
104 buildAgentTreeRecursive(left, end, agentTree_[node].right);
105 }
106 }
107
108 void KdTree2D::buildObstacleTree(std::vector<Obstacle2D *> obstacles)
109 {
110 deleteObstacleTree(obstacleTree_);
111
112 obstacleTree_ = buildObstacleTreeRecursive(obstacles);
113 }
114
115
116 KdTree2D::ObstacleTreeNode *KdTree2D::buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &obstacles)
117 {
118 if (obstacles.empty()) {
119 return NULL;
120 }
121 else {
122 ObstacleTreeNode *const node = new ObstacleTreeNode;
123
124 size_t optimalSplit = 0;
125 size_t minLeft = obstacles.size();
126 size_t minRight = obstacles.size();
127
128 for (size_t i = 0; i < obstacles.size(); ++i) {
129 size_t leftSize = 0;
130 size_t rightSize = 0;
131
132 const Obstacle2D *const obstacleI1 = obstacles[i];
133 const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_;
134
135 /* Compute optimal split node. */
136 for (size_t j = 0; j < obstacles.size(); ++j) {
137 if (i == j) {
138 continue;
139 }
140
141 const Obstacle2D *const obstacleJ1 = obstacles[j];
142 const Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_;
143
144 const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_);
145 const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_);
146
147 if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
148 ++leftSize;
149 }
150 else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
151 ++rightSize;
152 }
153 else {
154 ++leftSize;
155 ++rightSize;
156 }
157
158 if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
159 break;
160 }
161 }
162
163 if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
164 minLeft = leftSize;
165 minRight = rightSize;
166 optimalSplit = i;
167 }
168 }
169
170 /* Build split node. */
171 std::vector<Obstacle2D *> leftObstacles(minLeft);
172 std::vector<Obstacle2D *> rightObstacles(minRight);
173
174 size_t leftCounter = 0;
175 size_t rightCounter = 0;
176 const size_t i = optimalSplit;
177
178 const Obstacle2D *const obstacleI1 = obstacles[i];
179 const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_;
180
181 for (size_t j = 0; j < obstacles.size(); ++j) {
182 if (i == j) {
183 continue;
184 }
185
186 Obstacle2D *const obstacleJ1 = obstacles[j];
187 Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_;
188
189 const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_);
190 const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_);
191
192 if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
193 leftObstacles[leftCounter++] = obstacles[j];
194 }
195 else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
196 rightObstacles[rightCounter++] = obstacles[j];
197 }
198 else {
199 /* Split obstacle j. */
200 const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_);
201
202 const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_);
203
204 Obstacle2D *const newObstacle = new Obstacle2D();
205 newObstacle->point_ = splitpoint;
206 newObstacle->prevObstacle_ = obstacleJ1;
207 newObstacle->nextObstacle_ = obstacleJ2;
208 newObstacle->isConvex_ = true;
209 newObstacle->unitDir_ = obstacleJ1->unitDir_;
210
211 newObstacle->id_ = sim_->obstacles_.size();
212
213 sim_->obstacles_.push_back(newObstacle);
214
215 obstacleJ1->nextObstacle_ = newObstacle;
216 obstacleJ2->prevObstacle_ = newObstacle;
217
218 if (j1LeftOfI > 0.0f) {
219 leftObstacles[leftCounter++] = obstacleJ1;
220 rightObstacles[rightCounter++] = newObstacle;
221 }
222 else {
223 rightObstacles[rightCounter++] = obstacleJ1;
224 leftObstacles[leftCounter++] = newObstacle;
225 }
226 }
227 }
228
229 node->obstacle = obstacleI1;
230 node->left = buildObstacleTreeRecursive(leftObstacles);
231 node->right = buildObstacleTreeRecursive(rightObstacles);
232 return node;
233 }
234 }
235
236 void KdTree2D::computeAgentNeighbors(Agent2D *agent, float &rangeSq) const
237 {
238 queryAgentTreeRecursive(agent, rangeSq, 0);
239 }
240
241 void KdTree2D::computeObstacleNeighbors(Agent2D *agent, float rangeSq) const
242 {
243 queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_);
244 }
245
246 void KdTree2D::deleteObstacleTree(ObstacleTreeNode *node)
247 {
248 if (node != NULL) {
249 deleteObstacleTree(node->left);
250 deleteObstacleTree(node->right);
251 delete node;
252 }
253 }
254
255 void KdTree2D::queryAgentTreeRecursive(Agent2D *agent, float &rangeSq, size_t node) const
256 {
257 if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) {
258 for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
259 agent->insertAgentNeighbor(agents_[i], rangeSq);
260 }
261 }
262 else {
263 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));
264
265 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));
266
267 if (distSqLeft < distSqRight) {
268 if (distSqLeft < rangeSq) {
269 queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
270
271 if (distSqRight < rangeSq) {
272 queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
273 }
274 }
275 }
276 else {
277 if (distSqRight < rangeSq) {
278 queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
279
280 if (distSqLeft < rangeSq) {
281 queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
282 }
283 }
284 }
285
286 }
287 }
288
289 void KdTree2D::queryObstacleTreeRecursive(Agent2D *agent, float rangeSq, const ObstacleTreeNode *node) const
290 {
291 if (node == NULL) {
292 return;
293 }
294 else {
295 const Obstacle2D *const obstacle1 = node->obstacle;
296 const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_;
297
298 const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_);
299
300 queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right));
301
302 const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_);
303
304 if (distSqLine < rangeSq) {
305 if (agentLeftOfLine < 0.0f) {
306 /*
307 * Try obstacle at this node only if agent is on right side of
308 * obstacle (and can see obstacle).
309 */
310 agent->insertObstacleNeighbor(node->obstacle, rangeSq);
311 }
312
313 /* Try other side of line. */
314 queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left));
315
316 }
317 }
318 }
319
320 bool KdTree2D::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const
321 {
322 return queryVisibilityRecursive(q1, q2, radius, obstacleTree_);
323 }
324
325 bool KdTree2D::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const
326 {
327 if (node == NULL) {
328 return true;
329 }
330 else {
331 const Obstacle2D *const obstacle1 = node->obstacle;
332 const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_;
333
334 const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1);
335 const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2);
336 const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_);
337
338 if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) {
339 return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right));
340 }
341 else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) {
342 return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left));
343 }
344 else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) {
345 /* One can see through obstacle from left to right. */
346 return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right);
347 }
348 else {
349 const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_);
350 const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_);
351 const float invLengthQ = 1.0f / absSq(q2 - q1);
352
353 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));
354 }
355 }
356 }
357}
358