1/*
2 * KdTree2d.h
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#ifndef RVO2D_KD_TREE_H_
34#define RVO2D_KD_TREE_H_
35
36/**
37 * \file KdTree2d.h
38 * \brief Contains the KdTree class.
39 */
40
41#include "Definitions.h"
42
43namespace RVO2D {
44 /**
45 * \brief Defines <i>k</i>d-trees for agents and static obstacles in the
46 * simulation.
47 */
48 class KdTree2D {
49 public:
50 /**
51 * \brief Defines an agent <i>k</i>d-tree node.
52 */
53 class AgentTreeNode {
54 public:
55 /**
56 * \brief The beginning node number.
57 */
58 size_t begin;
59
60 /**
61 * \brief The ending node number.
62 */
63 size_t end;
64
65 /**
66 * \brief The left node number.
67 */
68 size_t left;
69
70 /**
71 * \brief The maximum x-coordinate.
72 */
73 float maxX;
74
75 /**
76 * \brief The maximum y-coordinate.
77 */
78 float maxY;
79
80 /**
81 * \brief The minimum x-coordinate.
82 */
83 float minX;
84
85 /**
86 * \brief The minimum y-coordinate.
87 */
88 float minY;
89
90 /**
91 * \brief The right node number.
92 */
93 size_t right;
94 };
95
96 /**
97 * \brief Defines an obstacle <i>k</i>d-tree node.
98 */
99 class ObstacleTreeNode {
100 public:
101 /**
102 * \brief The left obstacle tree node.
103 */
104 ObstacleTreeNode *left;
105
106 /**
107 * \brief The obstacle number.
108 */
109 const Obstacle2D *obstacle;
110
111 /**
112 * \brief The right obstacle tree node.
113 */
114 ObstacleTreeNode *right;
115 };
116
117 /**
118 * \brief Constructs a <i>k</i>d-tree instance.
119 * \param sim The simulator instance.
120 */
121 explicit KdTree2D(RVOSimulator2D *sim);
122
123 /**
124 * \brief Destroys this kd-tree instance.
125 */
126 ~KdTree2D();
127
128 /**
129 * \brief Builds an agent <i>k</i>d-tree.
130 */
131 void buildAgentTree(std::vector<Agent2D *> agents);
132
133 void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
134
135 /**
136 * \brief Builds an obstacle <i>k</i>d-tree.
137 */
138 void buildObstacleTree(std::vector<Obstacle2D *> obstacles);
139
140 ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &
141 obstacles);
142
143 /**
144 * \brief Computes the agent neighbors of the specified agent.
145 * \param agent A pointer to the agent for which agent
146 * neighbors are to be computed.
147 * \param rangeSq The squared range around the agent.
148 */
149 void computeAgentNeighbors(Agent2D *agent, float &rangeSq) const;
150
151 /**
152 * \brief Computes the obstacle neighbors of the specified agent.
153 * \param agent A pointer to the agent for which obstacle
154 * neighbors are to be computed.
155 * \param rangeSq The squared range around the agent.
156 */
157 void computeObstacleNeighbors(Agent2D *agent, float rangeSq) const;
158
159 /**
160 * \brief Deletes the specified obstacle tree node.
161 * \param node A pointer to the obstacle tree node to be
162 * deleted.
163 */
164 void deleteObstacleTree(ObstacleTreeNode *node);
165
166 void queryAgentTreeRecursive(Agent2D *agent, float &rangeSq,
167 size_t node) const;
168
169 void queryObstacleTreeRecursive(Agent2D *agent, float rangeSq,
170 const ObstacleTreeNode *node) const;
171
172 /**
173 * \brief Queries the visibility between two points within a
174 * specified radius.
175 * \param q1 The first point between which visibility is
176 * to be tested.
177 * \param q2 The second point between which visibility is
178 * to be tested.
179 * \param radius The radius within which visibility is to be
180 * tested.
181 * \return True if q1 and q2 are mutually visible within the radius;
182 * false otherwise.
183 */
184 bool queryVisibility(const Vector2 &q1, const Vector2 &q2,
185 float radius) const;
186
187 bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2,
188 float radius,
189 const ObstacleTreeNode *node) const;
190
191 std::vector<Agent2D *> agents_;
192 std::vector<AgentTreeNode> agentTree_;
193 ObstacleTreeNode *obstacleTree_;
194 RVOSimulator2D *sim_;
195
196 static const size_t MAX_LEAF_SIZE = 10;
197
198 friend class Agent2D;
199 friend class RVOSimulator2D;
200 };
201}
202
203#endif /* RVO2D_KD_TREE_H_ */
204