30{
36
37 Point ray = rayEnd - rayStart;
38
39 double stepX = ray.x() >= 0 ? 1 : -1;
40 double stepY = ray.y() >= 0 ? 1 : -1;
41
42 double nextVoxelBoundaryX = (currentVoxel.first + stepX) * xdist;
43 double nextVoxelBoundaryY = (currentVoxel.second + stepY) * ydist;
44
45 if (stepX < 0) { nextVoxelBoundaryX += xdist; }
46 if (stepY < 0) { nextVoxelBoundaryY += ydist; }
47
48 double tMaxX = ray.x() != 0 ? (nextVoxelBoundaryX - rayStart.x()) / ray.x() : DBL_MAX;
49 double tMaxY = ray.y() != 0 ? (nextVoxelBoundaryY - rayStart.y()) / ray.y() : DBL_MAX;
50
51 double tDeltaX = ray.x() != 0 ? static_cast<double>(xdist) / ray.x() * stepX : DBL_MAX;
52 double tDeltaY = ray.y() != 0 ? static_cast<double>(ydist) / ray.y() * stepY : DBL_MAX;
53
54 res.push_back(currentVoxel);
55
56 double tx = tMaxX;
57 double ty = tMaxY;
58
59 while (lastVoxel != currentVoxel) {
60 if (lastVoxel.first == currentVoxel.first) {
61 for (
int64_t i = currentVoxel.second; i != lastVoxel.second; i += (
int64_t) stepY) {
62 currentVoxel.second += (
int64_t) stepY;
63 res.push_back(currentVoxel);
64 }
65 break;
66 }
67 if (lastVoxel.second == currentVoxel.second) {
68 for (
int64_t i = currentVoxel.first; i != lastVoxel.first; i += (
int64_t) stepX) {
69 currentVoxel.first += (
int64_t) stepX;
70 res.push_back(currentVoxel);
71 }
72 break;
73 }
74
75 if (tx < ty) {
76 currentVoxel.first += (
int64_t) stepX;
77 tx += tDeltaX;
78 } else {
79 currentVoxel.second += (
int64_t) stepY;
80 ty += tDeltaY;
81 }
82 res.push_back(currentVoxel);
83 if (res.size() >= 100000) {
84 assert(0);
85 }
86 }
87
88 return res;
89}
Point b
Definition Line.hpp:198
Point a
Definition Line.hpp:197
IndexPair point_map_grid_index(const Point &pt, int64_t xdist, int64_t ydist)
Definition ConflictChecker.cpp:20
std::pair< int64_t, int64_t > IndexPair
Definition ConflictChecker.cpp:14
std::vector< IndexPair > Grids
Definition ConflictChecker.cpp:15
__int64 int64_t
Definition unistd.h:76