Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fixing the issue #2781: raytraceLine with same start and end point no… #2784

Merged
merged 3 commits into from
Feb 10, 2022
Merged

Fixing the issue #2781: raytraceLine with same start and end point no… #2784

Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
26c7d21
Fixing the issue #2781: raytraceLine with same start and end point no…
janx8r Jan 25, 2022
570bf02
Some whitespace modifications to make the code pass release_test
janx8r Jan 25, 2022
be5d43e
Add testcase for raytraceLine the same point
AlexeyMerzlyakov Feb 9, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 16 additions & 6 deletions nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,12 +232,22 @@ class VoxelGrid
if ((unsigned int)(dist) < min_length) {
return;
}
double scale = std::min(1.0, max_length / dist);

// Updating starting point to the point at distance min_length from the initial point
double min_x0 = x0 + (x1 - x0) / dist * min_length;
double min_y0 = y0 + (y1 - y0) / dist * min_length;
double min_z0 = z0 + (z1 - z0) / dist * min_length;
double scale, min_x0, min_y0, min_z0;
if (dist > 0.0) {
scale = std::min(1.0, max_length / dist);

// Updating starting point to the point at distance min_length from the initial point
min_x0 = x0 + (x1 - x0) / dist * min_length;
min_y0 = y0 + (y1 - y0) / dist * min_length;
min_z0 = z0 + (z1 - z0) / dist * min_length;
} else {
// dist can be 0 if [x0, y0, z0]==[x1, y1, z1].
// In this case only this voxel should be processed.
scale = 1.0;
min_x0 = x0;
min_y0 = y0;
min_z0 = z0;
}

int dx = int(x1) - int(min_x0); // NOLINT
int dy = int(y1) - int(min_y0); // NOLINT
Expand Down