HOG2
Voxels.cpp
Go to the documentation of this file.
1 //
2 // Voxels.cpp
3 // hog2 glut
4 //
5 // Created by Nathan Sturtevant on 3/7/16.
6 // Copyright © 2016 University of Denver. All rights reserved.
7 //
8 
9 #include <stdio.h>
10 #include "Voxels.h"
11 #include <cinttypes>
12 
14 //struct octTreeLink {
15 // int layer : 3;
16 // int index : 23;
17 // int subid : 6; // only valid at layer 0
18 //};
19 //
20 //struct octTreeNode {
21 // octTreeLink neighbors[6];
22 // point3d origin;
23 // float level;
24 // octTreeLink parent;
25 // octTreeLink firstChild; // (index) of first of 8 children
26 //};
27 
28 // get successors:
29 // * returns neighbors at the current level or a higher level if none at current level
30 // * When we expand a node, we might just generate children as successors [PEA*]
31 
32 bool operator==(const voxelState &v1, const voxelState &v2)
33 {
34  return v1.x == v2.x && v1.y == v2.y && v1.z == v2.z;
35 }
36 
37 
38 Voxels::Voxels(const char *filename)
39 {
40  printf("OctreeIndex size: %lu\n", sizeof(OctreeIndex));
41  FILE *f = fopen(filename, "r");
42  //FILE *f = fopen("/Users/nathanst/Desktop/3dmaps/Simple_test3dnav.3dnav", "r");
43  if (f == 0)
44  {
45  printf("Error opening file\n");
46  exit(0);
47  }
48  point3d minPoint;
49  //voxelWorld w;
50  fread(&w.header, sizeof(w.header), 1, f);
51  printf("Header is 0x%X\n", w.header);
52  fread(&w.voxelSize, sizeof(w.voxelSize), 1, f);
53  printf("Voxel size is %f\n", w.voxelSize);
54  fread(&w.numVoxelsGrids, sizeof(w.numVoxelsGrids), 1, f);
55  printf("%" PRId64 " voxel grids to follow\n", w.numVoxelsGrids);
56  fread(w.minbounds, sizeof(w.minbounds[0]), 4, f);
57  fread(w.maxbounds, sizeof(w.maxbounds[0]), 4, f);
58  printf("Min bounds: ");
59  minPoint.x = w.minbounds[0];
60  minPoint.y = w.minbounds[1];
61  minPoint.z = w.minbounds[2];
62  int maxWidth = 0;
63  for (int x = 0; x < 4; x++)
64  {
65  printf("%f ", w.minbounds[x]);
66  maxWidth = std::max(maxWidth, int(w.maxbounds[x]-w.minbounds[x]));
67  }
68  maxWidth /= w.voxelSize;
69  printf("\n");
70  printf("Max bounds: ");
71  for (int x = 0; x < 4; x++)
72  {
73  printf("%f ", w.maxbounds[x]);
74  }
75  printf("\n");
76  printf("Max dimension width = %d\n", maxWidth);
77  w.morton = new uint64_t[w.numVoxelsGrids];
78  w.grid = new uint64_t[w.numVoxelsGrids];
79  for (int x = 0; x < w.numVoxelsGrids; x++)
80  {
81  assert(fread(&w.morton[x], sizeof(w.morton[x]), 1, f) == 1);
82  assert(fread(&w.grid[x], sizeof(w.grid[x]), 1, f) == 1);
83  printf("0x%llX\n", w.morton[x]);
84  point3d p = GetVoxelCoordinate(w.morton[x], w.voxelSize, w.minbounds);
85  printf("(%f, %f, %f)\n", p.x, p.y, p.z);
86  printf("0x%llX\n", w.grid[x]);
87  p -= minPoint;
88  p /= (int)w.voxelSize;
89  AddVoxelCubeToOctree(w.grid[x], p);
90  for (int i = 0; i < 64; i++)
91  {
92  size_t a, b, c;
93  GetCoordsForIndex(i, a, b, c);
94  if ((w.grid[x]>>i)&1) // blocked
95  {
96  //printf("{%f, %f, %f} ", (p.x+a*w.voxelSize)/8, (p.y+b*w.voxelSize)/8, (p.z+c*w.voxelSize)/8);
97  }
98  }
99  printf("\n");
100  }
101 
102  printf("\n");
103 
104  fclose(f);
105 }
106 
107 Voxels::~Voxels()
108 {
109 
110 }
111 
112 void Voxels::Export(const char *filename)
113 {
114  FILE *f = fopen(filename, "w+");
115  if (f == 0)
116  return;
117  // number of voxels outside min bounded box
118  int buffer = 50;
119  int minx = (int)((w.maxbounds[0]-w.minbounds[0])/w.voxelSize);
120  int miny = (int)((w.maxbounds[1]-w.minbounds[1])/w.voxelSize);
121  int minz = (int)((w.maxbounds[2]-w.minbounds[2])/w.voxelSize);
122  int maxx = 0;
123  int maxy = 0;
124  int maxz = 0;
125 
126  for (int x = 0; x < w.numVoxelsGrids; x++)
127  {
128  point3d p = GetVoxelCoordinate(w.morton[x], w.voxelSize, w.minbounds);
129  for (int i = 0; i < 64; i++)
130  {
131  size_t a, b, c;
132  if ((w.grid[x]>>i)&1) // blocked
133  {
134  GetCoordsForIndex(i, a, b, c);
135  minx = std::min(minx, int(((p.x+a*w.voxelSize)-w.minbounds[0])/w.voxelSize));
136  miny = std::min(miny, int(((p.y+b*w.voxelSize)-w.minbounds[1])/w.voxelSize));
137  minz = std::min(minz, int(((p.z+c*w.voxelSize)-w.minbounds[2])/w.voxelSize));
138  maxx = std::max(maxx, int(((p.x+a*w.voxelSize)-w.minbounds[0])/w.voxelSize));
139  maxy = std::max(maxy, int(((p.y+b*w.voxelSize)-w.minbounds[1])/w.voxelSize));
140  maxz = std::max(maxz, int(((p.z+c*w.voxelSize)-w.minbounds[2])/w.voxelSize));
141  }
142  }
143  }
144 
145 
146  fprintf(f, "voxel %d %d %d\n", maxx-minx+1+2*buffer, maxy-miny+1+2*buffer, maxz-minz+1+2*buffer);
147 
148  for (size_t x = 0; x < w.numVoxelsGrids; x++)
149  {
150  point3d p = GetVoxelCoordinate(w.morton[x], w.voxelSize, w.minbounds);
151  for (int i = 0; i < 64; i++)
152  {
153  size_t a, b, c;
154  if ((w.grid[x]>>i)&1) // blocked
155  {
156  GetCoordsForIndex(i, a, b, c);
157  fprintf(f, "%d %d %d\n",
158  int(((p.x+a*w.voxelSize)-w.minbounds[0])/w.voxelSize)-minx+buffer,
159  int(((p.y+b*w.voxelSize)-w.minbounds[1])/w.voxelSize)-miny+buffer,
160  int(((p.z+c*w.voxelSize)-w.minbounds[2])/w.voxelSize)-minz+buffer);
161  }
162  }
163  }
164 
165 
166  fclose(f);
167 }
168 
169 
170 void Voxels::GetSuccessors(const voxelState &nodeID, std::vector<voxelState> &neighbors) const
171 {
172 
173 }
174 
175 void Voxels::GetActions(const voxelState &nodeID, std::vector<voxelAction> &actions) const
176 {
177 
178 }
179 
180 void Voxels::ApplyAction(voxelState &s, voxelAction a) const
181 {
182 
183 }
184 
185 bool Voxels::InvertAction(voxelAction &a) const
186 {
187  return false;
188 }
189 
190 
192 double Voxels::HCost(const voxelState &node1, const voxelState &node2) const
193 {
194  return 0;
195 }
196 
197 double Voxels::GCost(const voxelState &node1, const voxelState &node2) const
198 {
199  return 1;
200 }
201 
202 double Voxels::GCost(const voxelState &node, const voxelAction &act) const
203 {
204  return 1;
205 }
206 
207 bool Voxels::GoalTest(const voxelState &node, const voxelState &goal) const
208 {
209  return node == goal;
210 }
211 
212 
213 uint64_t Voxels::GetStateHash(const voxelState &node) const
214 {
215  return (((uint64_t)node.x)<<32)|(node.y<<16)|(node.z);
216 }
217 
218 void Voxels::GetStateFromHash(uint64_t parent, voxelState &s)
219 {
220  s.z = parent&0xFFFF;
221  s.y = (parent>>16)&0xFFFF;
222  s.x = (parent>>32)&0xFFFF;
223 }
224 
225 
226 uint64_t Voxels::GetActionHash(voxelAction act) const
227 {
228  return (int)act;
229 }
230 
231 
232 point3d Voxels::GetVoxelCoordinate(uint64_t morton, float voxelSize, const float minbounds[4]) const
233 {
234  point3d pt(DecodeMorton3X(morton), DecodeMorton3Y(morton), DecodeMorton3Z(morton));
235  pt.x*=voxelSize*4;
236  pt.x += minbounds[0];
237  pt.y*=voxelSize*4;
238  pt.y += minbounds[1];
239  pt.z*=voxelSize*4;
240  pt.z += minbounds[2];
241  //const size_t ix = static_cast<size_t>(floorf((coords.x - mBounds.MinX()) / (voxelSize * 4)));
242  return pt;
243 }
244 
245 void Voxels::AddVoxelCubeToOctree(uint64_t values, point3d p)
246 {
247 // int index = mLayer0VoxelGrids.size();
248  mLayer0VoxelGrids.push_back(values);
249 }
250 
251 void Voxels::OpenGLDraw() const
252 {
253  double xRange = max(w.maxbounds[0],-w.minbounds[0]);
254  double yRange = max(w.maxbounds[1],-w.minbounds[1]);
255  double zRange = max(w.maxbounds[2],-w.minbounds[2]);
256  double range = std::max(xRange, std::max(yRange, zRange));
257  for (size_t x = 0; x < w.numVoxelsGrids; x++)
258  {
259  point3d p = GetVoxelCoordinate(w.morton[x], w.voxelSize, w.minbounds);
260  bool drawFrame = false;
261  for (int i = 0; i < 64; i++)
262  {
263  size_t a, b, c;
264  if ((w.grid[x]>>i)&1) // blocked
265  {
266  GetCoordsForIndex(i, a, b, c);
267  drawFrame = true;
268  GLfloat rr, gg, bb;
269  rr = (1.+(p.x+a*w.voxelSize)/range)/2.0;
270  gg = (1.+(p.y+b*w.voxelSize)/range)/2.0;
271  bb = (1.+(p.z+c*w.voxelSize)/range)/2.0;
272  // 7?
273  rr = Colors::GetColor(rr, 0, 1, 7).r;
274  gg = Colors::GetColor(bb, 0, 1, 9).g*0.9;
275  bb = Colors::GetColor(bb, 0, 1, 9).b;
276  glColor3f(gg, rr, bb);
277  glEnable(GL_LIGHTING);
278  DrawBox((p.x+a*w.voxelSize+0.5*w.voxelSize)/range,
279  -(p.y+b*w.voxelSize+0.5*w.voxelSize)/range,
280  (p.z+c*w.voxelSize+0.5*w.voxelSize)/range,
281  1*(w.voxelSize/2.)/range);
282  }
283  }
284  //if (drawFrame)
285  if (0)
286  {
287  //DrawBoxFrame(p.x+w.voxelSize*2, p.y+w.voxelSize*2, p.z+w.voxelSize*2, w.voxelSize*2);
288  glDisable(GL_LIGHTING);
289  glColor4f(1, 1, 1, 1);
290  DrawBoxFrame((p.x+2*w.voxelSize)/range,
291  (p.y+2*w.voxelSize)/range,
292  (p.z+2*w.voxelSize)/range,
293  2*w.voxelSize/range);
294  }
295  }
296 
297 }
298 
299 void Voxels::OpenGLDraw(const voxelState&) const
300 {
301 
302 }
303 
304 void Voxels::OpenGLDraw(const voxelState&, const voxelState&, float) const
305 {
306 
307 }
308 
309 void Voxels::OpenGLDraw(const voxelState&, const voxelAction&) const
310 {
311 
312 }
313 
314 void Voxels::GLLabelState(const voxelState&, const char *) const
315 {
316 
317 }
318 
319 void Voxels::GLDrawLine(const voxelState &x, const voxelState &y) const
320 {
321 
322 }
323 
324 void Voxels::Draw(Graphics::Display &display) const
325 {
326 
327 }
328 
329 
330 // "Insert" two 0 bits after each of the 10 low bits of x
331 uint32_t Voxels::Part1By2(uint32_t x) const
332 {
333  x &= 0x000003ff; // x = ---- ---- ---- ---- ---- --98 7654 3210
334  x = (x ^ (x << 16)) & 0xff0000ff; // x = ---- --98 ---- ---- ---- ---- 7654 3210
335  x = (x ^ (x << 8)) & 0x0300f00f; // x = ---- --98 ---- ---- 7654 ---- ---- 3210
336  x = (x ^ (x << 4)) & 0x030c30c3; // x = ---- --98 ---- 76-- --54 ---- 32-- --10
337  x = (x ^ (x << 2)) & 0x09249249; // x = ---- 9--8 --7- -6-- 5--4 --3- -2-- 1--0
338  return x;
339 }
340 
341 uint32_t Voxels::EncodeMorton3(uint32_t x, uint32_t y, uint32_t z) const
342 {
343  return (Part1By2(z) << 2) + (Part1By2(y) << 1) + Part1By2(x);
344 }
345 
346 // Inverse of Part1By2 - "delete" all bits not at positions divisible by 3
347 uint32_t Voxels::Compact1By2(uint32_t x) const
348 {
349  x &= 0x09249249; // x = ---- 9--8 --7- -6-- 5--4 --3- -2-- 1--0
350  x = (x ^ (x >> 2)) & 0x030c30c3; // x = ---- --98 ---- 76-- --54 ---- 32-- --10
351  x = (x ^ (x >> 4)) & 0x0300f00f; // x = ---- --98 ---- ---- 7654 ---- ---- 3210
352  x = (x ^ (x >> 8)) & 0xff0000ff; // x = ---- --98 ---- ---- ---- ---- 7654 3210
353  x = (x ^ (x >> 16)) & 0x000003ff; // x = ---- ---- ---- ---- ---- --98 7654 3210
354  return x;
355 }
356 
357 uint32_t Voxels::DecodeMorton3X(uint32_t code) const
358 {
359  return Compact1By2(code >> 0);
360 }
361 
362 uint32_t Voxels::DecodeMorton3Y(uint32_t code) const
363 {
364  return Compact1By2(code >> 1);
365 }
366 
367 uint32_t Voxels::DecodeMorton3Z(uint32_t code) const
368 {
369  return Compact1By2(code >> 2);
370 }
371 
372 size_t Voxels::GetIndex(size_t x, size_t y, size_t z) const
373 {
374  return(x * 16 + y * 4 + z);
375 }
376 
377 void Voxels::GetCoordsForIndex(size_t i, size_t& x, size_t& y, size_t& z) const
378 {
379  x = i >> 4;
380  y = (i & 0x0F) >> 2;
381  z = (i & 0x03);
382 }
Colors::GetColor
rgbColor GetColor(float v, float vmin, float vmax, int type)
Given min/max values, get a color from a color schema.
Definition: Colors.cpp:24
rgbColor::b
float b
Definition: Colors.h:71
Voxels::HCost
double HCost(const voxelState &node1, const voxelState &node2) const
Heuristic value between two arbitrary nodes.
Definition: Voxels.cpp:192
min
double min(double a, double b)
Definition: FPUtil.h:35
Voxels::GetVoxelCoordinate
point3d GetVoxelCoordinate(uint64_t morton, float voxelSize, const float minbounds[4]) const
Definition: Voxels.cpp:232
Voxels::GetActionHash
uint64_t GetActionHash(voxelAction act) const
Definition: Voxels.cpp:226
Voxels::GetCoordsForIndex
void GetCoordsForIndex(size_t i, size_t &x, size_t &y, size_t &z) const
Definition: Voxels.cpp:377
operator==
bool operator==(const voxelState &v1, const voxelState &v2)
Definition: Voxels.cpp:32
Voxels::DecodeMorton3Y
uint32_t DecodeMorton3Y(uint32_t code) const
Definition: Voxels.cpp:362
Voxels::ApplyAction
void ApplyAction(voxelState &s, voxelAction a) const
Definition: Voxels.cpp:180
Voxels::mLayer0VoxelGrids
std::vector< uint64_t > mLayer0VoxelGrids
Definition: Voxels.h:103
Voxels::GetSuccessors
void GetSuccessors(const voxelState &nodeID, std::vector< voxelState > &neighbors) const
Definition: Voxels.cpp:170
voxelState
Definition: Voxels.h:22
Voxels.h
Voxels::DecodeMorton3Z
uint32_t DecodeMorton3Z(uint32_t code) const
Definition: Voxels.cpp:367
Voxels::EncodeMorton3
uint32_t EncodeMorton3(uint32_t x, uint32_t y, uint32_t z) const
Definition: Voxels.cpp:341
voxelAction
voxelAction
Definition: Voxels.h:30
rgbColor::g
float g
Definition: Colors.h:71
Voxels::Part1By2
uint32_t Part1By2(uint32_t x) const
Definition: Voxels.cpp:331
DrawBoxFrame
void DrawBoxFrame(GLfloat xx, GLfloat yy, GLfloat zz, GLfloat rad)
Definition: GLUtil.cpp:252
Voxels::voxelWorld::morton
uint64_t * morton
Definition: Voxels.h:99
voxelState::x
uint16_t x
Definition: Voxels.h:24
Voxels::GetStateHash
uint64_t GetStateHash(const voxelState &node) const
Definition: Voxels.cpp:213
Voxels::w
voxelWorld w
Definition: Voxels.h:117
Voxels::Voxels
Voxels(const char *filename)
Definition: Voxels.cpp:38
Voxels::voxelWorld::grid
uint64_t * grid
Definition: Voxels.h:100
voxelState::y
uint16_t y
Definition: Voxels.h:24
point3d
#define point3d
Definition: GLUtil.h:123
Graphics::Display
Definition: Graphics.h:146
Voxels::~Voxels
~Voxels()
Definition: Voxels.cpp:107
OctreeIndex
Definition: Voxels.h:16
Voxels::voxelWorld::numVoxelsGrids
uint64_t numVoxelsGrids
Definition: Voxels.h:96
Voxels::GetStateFromHash
void GetStateFromHash(uint64_t parent, voxelState &s)
Definition: Voxels.cpp:218
Voxels::GLDrawLine
void GLDrawLine(const voxelState &x, const voxelState &y) const
Definition: Voxels.cpp:319
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
voxelState::z
uint16_t z
Definition: Voxels.h:24
Voxels::GCost
double GCost(const voxelState &node1, const voxelState &node2) const
Definition: Voxels.cpp:197
rgbColor::r
float r
Definition: Colors.h:71
Voxels::voxelWorld::voxelSize
float voxelSize
Definition: Voxels.h:95
DrawBox
void DrawBox(GLfloat xx, GLfloat yy, GLfloat zz, GLfloat rad)
Definition: GLUtil.cpp:285
Voxels::GetActions
void GetActions(const voxelState &nodeID, std::vector< voxelAction > &actions) const
Definition: Voxels.cpp:175
Voxels::GLLabelState
void GLLabelState(const voxelState &, const char *) const
Definition: Voxels.cpp:314
Voxels::InvertAction
bool InvertAction(voxelAction &a) const
Definition: Voxels.cpp:185
Voxels::DecodeMorton3X
uint32_t DecodeMorton3X(uint32_t code) const
Definition: Voxels.cpp:357
Voxels::AddVoxelCubeToOctree
void AddVoxelCubeToOctree(uint64_t values, point3d p)
Definition: Voxels.cpp:245
Voxels::Draw
void Draw(Graphics::Display &display) const
Definition: Voxels.cpp:324
Voxels::voxelWorld::minbounds
float minbounds[4]
Definition: Voxels.h:97
Voxels::GetIndex
size_t GetIndex(size_t x, size_t y, size_t z) const
Definition: Voxels.cpp:372
Voxels::voxelWorld::maxbounds
float maxbounds[4]
Definition: Voxels.h:98
Voxels::voxelWorld::header
uint32_t header
Definition: Voxels.h:94
Voxels::GoalTest
bool GoalTest(const voxelState &node, const voxelState &goal) const
Definition: Voxels.cpp:207
Voxels::Export
void Export(const char *filename)
Definition: Voxels.cpp:112
Voxels::Compact1By2
uint32_t Compact1By2(uint32_t x) const
Definition: Voxels.cpp:347
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
Voxels::OpenGLDraw
void OpenGLDraw() const
Definition: Voxels.cpp:251
Generated by   doxygen 1.8.17