@@ -283,81 +283,8 @@ void DensityGrid::calculateDensities(const std::string &file_name)
283283 {
284284 continue ; // ray is outside of bounds
285285 }
286-
287- #define NEW_METHOD
288- #if defined NEW_METHOD
289286 bounded_ = colours[i].alpha > 0 ;
290287 walkGrid ((start - bounds_.min_bound_ ) / voxel_width_, (end - bounds_.min_bound_ ) / voxel_width_, *this );
291- #else
292- // now walk the voxels
293- Eigen::Vector3d dir = end - start;
294- const Eigen::Vector3d source = (start - bounds_.min_bound_ ) / voxel_width_;
295- const Eigen::Vector3d target = (end - bounds_.min_bound_ ) / voxel_width_;
296- const double length = dir.norm ();
297- for (int a = 0 ; a<3 ; a++)
298- {
299- if (dir[a] == 0.0 )
300- {
301- dir[a] = 1e-10 ; // prevent division by 0
302- }
303- }
304- const double eps = 1e-9 ; // to stay away from edge cases
305- const double maxDist = (target - source).norm ();
306-
307- // cached values to speed up the loop below
308- Eigen::Vector3i adds;
309- Eigen::Vector3d offsets;
310- for (int k = 0 ; k < 3 ; ++k)
311- {
312- if (dir[k] > 0.0 )
313- {
314- adds[k] = 1 ;
315- offsets[k] = 0.5 ;
316- }
317- else
318- {
319- adds[k] = -1 ;
320- offsets[k] = -0.5 ;
321- }
322- }
323-
324- Eigen::Vector3d p = source; // our moving variable as we walk over the grid
325- Eigen::Vector3i inds = p.cast <int >();
326-
327- double depth = 0 ;
328- // walk over the grid, one voxel at a time.
329- do
330- {
331- double ls[3 ] = { (round (p[0 ] + offsets[0 ]) - p[0 ]) / dir[0 ], (round (p[1 ] + offsets[1 ]) - p[1 ]) / dir[1 ],
332- (round (p[2 ] + offsets[2 ]) - p[2 ]) / dir[2 ] };
333- int axis = (ls[0 ] < ls[1 ] && ls[0 ] < ls[2 ]) ? 0 : (ls[1 ] < ls[2 ] ? 1 : 2 );
334- inds[axis] += adds[axis];
335- if (inds[axis] < 0 || inds[axis] >= voxel_dims_[axis])
336- {
337- break ;
338- }
339- double minL = ls[axis] * length;
340- depth += minL + eps;
341- p = source + dir * (depth / length);
342- int index = getIndex (inds);
343- if (depth > maxDist)
344- {
345- double length_in_voxel = minL + maxDist - depth;
346- if (colours[i].alpha > 0 )
347- {
348- voxels_[index].addHitRay (static_cast <float >(length_in_voxel * voxel_width_));
349- }
350- else
351- {
352- voxels_[index].addMissRay (static_cast <float >(length_in_voxel * voxel_width_));
353- }
354- }
355- else
356- {
357- voxels_[index].addMissRay (static_cast <float >(minL * voxel_width_));
358- }
359- } while (depth <= maxDist);
360- #endif
361288 }
362289 };
363290 Cloud::read (file_name, calculate);
0 commit comments