« back to all changes in this revision
Viewing changes to src/renderer/entities/OccupancyGrid3D.hh
- browse files at revision 807
- compare with another revision
- download diff
- download tarball
- view history from revision 807
- Committer: Staffan Gimåker
- Date: 2010-06-08 14:51:28 UTC
- Revision ID: staffan@gimaker.se-20100608145128-df8d8it7ijxw03ib
Added experimental support for 3D occupancy grids.
Occupied cells (occupancy value larger than 0.5) are drawn as opaque cubes;
free and unexplored is not drawn at all.
Occupied cells (occupancy value larger than 0.5) are drawn as opaque cubes;
free and unexplored is not drawn at all.
- files added:
- src/OccupancyGrid3D.cc
- files modified:
- src/Makefile.am
added
removed
src/renderer/entities/OccupancyGrid3D.hh
1
/*
2
* Copyright Staffan Gimåker 2008-2010.
3
*
4
* ---
5
*
6
* This file is part of peekabot.
7
*
8
* peekabot is free software; you can redistribute it and/or modify
9
* it under the terms of the GNU General Public License as published by
10
* the Free Software Foundation; either version 3 of the License, or
11
* (at your option) any later version.
12
*
13
* peekabot is distributed in the hope that it will be useful,
14
* but WITHOUT ANY WARRANTY; without even the implied warranty of
15
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16
* GNU General Public License for more details.
17
*
18
* You should have received a copy of the GNU General Public License
19
* along with this program. If not, see <http://www.gnu.org/licenses/>.
20
*/
21
22
23
#ifndef PEEKABOT_RENDERER_PRIMITIVES_OCCUPANCY_GRID_2D_HH_INCLUDED
24
#define PEEKABOT_RENDERER_PRIMITIVES_OCCUPANCY_GRID_2D_HH_INCLUDED
25
26
27
#include <set>
28
#include <map>
29
#include <stdexcept>
30
#include <Eigen/Core>
31
32
#include "../../Types.hh"
33
#include "../CullableEntity.hh"
34
35
36
namespace peekabot
37
{
38
namespace renderer
39
{
40
class Camera;
41
class Frustum;
42
43
44
class OccupancyGrid3D : public CullableEntity
45
{
46
public:
47
OccupancyGrid3D(float cell_size, size_t max_cache_size);
48
49
~OccupancyGrid3D() throw();
50
51
void set_belief(const Eigen::Vector3f &x, float belief)
52
throw(std::domain_error);
53
54
float get_belief(const Eigen::Vector3f &x)
55
throw(std::runtime_error);
56
57
void clear_cell(const Eigen::Vector3f &x) throw();
58
59
void clear() throw();
60
61
virtual void get_renderables(PrepareRenderContext &context) const;
62
63
64
private:
65
class Batch
66
{
67
public:
68
Batch(PrepareRenderContext &context, size_t batch_size);
69
70
~Batch();
71
72
void add_cube(
73
double x_c, double y_c, double z_c,
74
double size,
75
double r, double g, double b);
76
77
private:
78
inline void add_quad(
79
Eigen::Vector3f * &vp,
80
const Eigen::Vector3f *vertices,
81
int i0, int i1, int i2, int i3)
82
{
83
*(vp++) = vertices[i0];
84
*(vp++) = vertices[i1];
85
*(vp++) = vertices[i2];
86
87
*(vp++) = vertices[i2];
88
*(vp++) = vertices[i3];
89
*(vp++) = vertices[i0];
90
}
91
92
void flush();
93
94
void allocate_buffers();
95
96
private:
97
PrepareRenderContext &m_context;
98
const size_t m_batch_size;
99
size_t m_used;
100
float *m_vertices, *m_colors, *m_normals;
101
};
102
103
class Node
104
{
105
public:
106
Node(Node *parent);
107
108
~Node();
109
110
inline bool is_leaf() const throw()
111
{
112
return m_children == 0;
113
}
114
115
uint8_t get_belief(
116
const Eigen::Vector3f &x,
117
const Eigen::Vector3f ¢er,
118
float node_size) throw();
119
120
void set_belief(
121
const Eigen::Vector3f &x,
122
uint8_t belief,
123
const float cell_size,
124
const Eigen::Vector3f ¢er,
125
float node_size) throw(std::domain_error);
126
127
inline Node *get_parent() throw()
128
{
129
return m_parent;
130
}
131
132
inline const Node *get_parent() const throw()
133
{
134
return m_parent;
135
}
136
137
/**
138
* \brief Get a child octants's position relative to its
139
* parent.
140
*
141
* \pre \f$ 0 \leq i < 4 \f$
142
*
143
* \param i The octant for which to get the offset
144
* from its parent node.
145
* \return A vector describing the offset of node i from its parent.
146
*
147
* \sa \c m_children (for octant indexing).
148
*/
149
inline Eigen::Vector3f octant_offset(
150
int i, float node_size) const throw()
151
{
152
return Eigen::Vector3f(
153
node_size/4*(2*(i&1)-1),
154
node_size/4*((i&2)-1),
155
node_size/4*(((i&4)>>1)-1) );
156
}
157
158
/**
159
* \brief Check if a coordinate is enclosed in the
160
* quad-tree node.
161
*
162
* \return \c true if the node encloses the given coordinate,
163
* \c false otherwise.
164
*/
165
bool encloses(
166
const Eigen::Vector3f &x,
167
const Eigen::Vector3f ¢er,
168
float node_size) const throw();
169
170
/**
171
* \brief Check if a coordinate is enclosed the node's
172
* <em>i</em>:th quadrant.
173
*
174
* \pre The coordinate is enclosed in the node, i.e.
175
* <tt>encloses(x) == true</tt>.
176
*
177
* \param i The quadrant to check against, in the range 0
178
* to 3.
179
* \return \c true if the coordinate is enclosed by the
180
* node's <em>i</em>:th quadrant, \c false
181
* otherwise.
182
*
183
* \remarks Note that this doesn't check if the coordinare
184
* is enclosed by a child node, or even demands that any
185
* child nodes exist. \sa encloses(float, float),
186
* m_children (for quadrant indexing).
187
*/
188
inline bool enclosed_in_octant(
189
const Eigen::Vector3f &x, int i,
190
const Eigen::Vector3f ¢er) const throw()
191
{
192
return ( ( i&1 ? x(0) >= center(0) : x(0) < center(0) ) &&
193
( i&2 ? x(1) >= center(1) : x(1) < center(1) ) &&
194
( i&4 ? x(2) >= center(2) : x(2) < center(2) ) );
195
}
196
197
void render_no_cull(
198
Batch &batch,
199
const Eigen::Vector3f ¢er,
200
float node_size,
201
const int order[]) throw();
202
203
void render(
204
Batch &batch,
205
const Camera &camera,
206
const Eigen::Vector3f ¢er,
207
float node_size,
208
const Frustum &frustum,
209
const int order[],
210
int plane_mask) throw();
211
212
private:
213
/**
214
* \brief Optimize the storage by merging neighbouring nodes
215
* if possible.
216
*
217
* \pre \a node is a leaf node.
218
*/
219
static void optimize(Node *node);
220
221
public:
222
Node *m_parent;
223
224
/**
225
* \brief Pointers to the child nodes of the node.
226
*
227
* \invariant m_children == 0 \f$\Leftrightarrow\f$ the
228
* node is a leaf.
229
*/
230
Node **m_children;
231
232
uint8_t m_belief;
233
};
234
235
private:
236
Node *grow_to_accomodate(const Eigen::Vector3f &x) throw();
237
238
void calculate_bvs() throw();
239
240
private:
241
242
243
/**
244
* \brief The root node of the quad-tree.
245
* \invariant Always non-null (except in the constructor).
246
* \remarks May change over time (if the tree is enlarged).
247
*/
248
Node *m_root_node;
249
250
float m_cell_size;
251
252
float m_root_cell_size;
253
254
Eigen::Vector3f m_root_cell_center;
255
256
size_t m_max_cache_size;
257
};
258
}
259
}
260
261
262
#endif // PEEKABOT_RENDERER_PRIMITIVES_OCCUPANCY_GRID_2D_HH_INCLUDED
Loggerhead is a web-based interface for Breezy
Version: 2.0.1