44{
51
52 Point ray = rayEnd - rayStart;
53
54 double stepX = ray.x() >= 0 ? 1 : -1;
55 double stepY = ray.y() >= 0 ? 1 : -1;
56
57 double nextVoxelBoundaryX = (currentVoxel.first + stepX) * xdist;
58 double nextVoxelBoundaryY = (currentVoxel.second + stepY) * ydist;
59
60 if (stepX < 0) { nextVoxelBoundaryX += xdist; }
61 if (stepY < 0) { nextVoxelBoundaryY += ydist; }
62
63 double tMaxX = ray.x() != 0 ? (nextVoxelBoundaryX - rayStart.x()) / ray.x() : DBL_MAX;
64 double tMaxY = ray.y() != 0 ? (nextVoxelBoundaryY - rayStart.y()) / ray.y() : DBL_MAX;
65
66 double tDeltaX = ray.x() != 0 ? static_cast<double>(xdist) / ray.x() * stepX : DBL_MAX;
67 double tDeltaY = ray.y() != 0 ? static_cast<double>(ydist) / ray.y() * stepY : DBL_MAX;
68
69 res.push_back(currentVoxel);
70
71 double tx = tMaxX;
72 double ty = tMaxY;
73
74 while (lastVoxel != currentVoxel) {
75 if (lastVoxel.first == currentVoxel.first) {
76 for (int64_t i = currentVoxel.second; i != lastVoxel.second; i += (int64_t) stepY) {
77 currentVoxel.second += (int64_t) stepY;
78 res.push_back(currentVoxel);
79 }
80 break;
81 }
82 if (lastVoxel.second == currentVoxel.second) {
83 for (int64_t i = currentVoxel.first; i != lastVoxel.first; i += (int64_t) stepX) {
84 currentVoxel.first += (int64_t) stepX;
85 res.push_back(currentVoxel);
86 }
87 break;
88 }
89
90 if (tx < ty) {
91 currentVoxel.first += (int64_t) stepX;
92 tx += tDeltaX;
93 } else {
94 currentVoxel.second += (int64_t) stepY;
95 ty += tDeltaY;
96 }
97 res.push_back(currentVoxel);
98 if (res.size() >= 100000) {
99 assert(0);
100 }
101 }
102
103 return res;
104}
Point b
Definition Line.hpp:199
Point a
Definition Line.hpp:198
std::pair< int64_t, int64_t > IndexPair
Definition ConflictChecker.cpp:31
IndexPair point_map_grid_index(const Point &pt, int64_t xdist, int64_t ydist)
Definition ConflictChecker.cpp:34
std::vector< IndexPair > Grids
Definition ConflictChecker.cpp:32