| 123 | /// @details UNTESTED!!!! |
| 124 | template <typename GridVisitor> |
| 125 | inline void traverseGridSegment16(const vec2f &start, const vec2f &end, |
| 126 | GridVisitor &visitor) { |
| 127 | const i16 FP_SHIFT = 8; |
| 128 | const i16 FP_ONE = 1 << FP_SHIFT; |
| 129 | // const i16 FP_MASK = FP_ONE - 1; |
| 130 | |
| 131 | // Convert to fixed-point (Q8.8), signed |
| 132 | i16 startX_fp = static_cast<i16>(start.x * FP_ONE); |
| 133 | i16 startY_fp = static_cast<i16>(start.y * FP_ONE); |
| 134 | i16 endX_fp = static_cast<i16>(end.x * FP_ONE); |
| 135 | i16 endY_fp = static_cast<i16>(end.y * FP_ONE); |
| 136 | |
| 137 | i16 x0 = startX_fp >> FP_SHIFT; |
| 138 | i16 y0 = startY_fp >> FP_SHIFT; |
| 139 | i16 x1 = endX_fp >> FP_SHIFT; |
| 140 | i16 y1 = endY_fp >> FP_SHIFT; |
| 141 | |
| 142 | i16 stepX = (x1 > x0) ? 1 : (x1 < x0) ? -1 : 0; |
| 143 | i16 stepY = (y1 > y0) ? 1 : (y1 < y0) ? -1 : 0; |
| 144 | |
| 145 | i16 deltaX_fp = endX_fp - startX_fp; |
| 146 | i16 deltaY_fp = endY_fp - startY_fp; |
| 147 | |
| 148 | // Use (max)() to prevent macro expansion by Arduino.h's max macro |
| 149 | u16 absDeltaX_fp = |
| 150 | (deltaX_fp != 0) ? static_cast<u16>( |
| 151 | fl::abs((i32(FP_ONE) << FP_SHIFT) / deltaX_fp)) |
| 152 | : (fl::numeric_limits<u16>::max)(); |
| 153 | u16 absDeltaY_fp = |
| 154 | (deltaY_fp != 0) ? static_cast<u16>( |
| 155 | fl::abs((i32(FP_ONE) << FP_SHIFT) / deltaY_fp)) |
| 156 | : (fl::numeric_limits<u16>::max)(); |
| 157 | |
| 158 | i16 nextX_fp = (stepX > 0) ? ((x0 + 1) << FP_SHIFT) : (x0 << FP_SHIFT); |
| 159 | i16 nextY_fp = (stepY > 0) ? ((y0 + 1) << FP_SHIFT) : (y0 << FP_SHIFT); |
| 160 | |
| 161 | // Use (max)() to prevent macro expansion by Arduino.h's max macro |
| 162 | u16 tMaxX_fp = |
| 163 | (deltaX_fp != 0) |
| 164 | ? static_cast<u16>( |
| 165 | fl::abs(i32(nextX_fp - startX_fp)) * absDeltaX_fp >> FP_SHIFT) |
| 166 | : (fl::numeric_limits<u16>::max)(); |
| 167 | u16 tMaxY_fp = |
| 168 | (deltaY_fp != 0) |
| 169 | ? static_cast<u16>( |
| 170 | fl::abs(i32(nextY_fp - startY_fp)) * absDeltaY_fp >> FP_SHIFT) |
| 171 | : (fl::numeric_limits<u16>::max)(); |
| 172 | |
| 173 | const u16 maxT_fp = FP_ONE; |
| 174 | |
| 175 | i16 currentX = x0; |
| 176 | i16 currentY = y0; |
| 177 | |
| 178 | while (true) { |
| 179 | visitor.visit(currentX, currentY); |
| 180 | |
| 181 | u16 t_fp = (tMaxX_fp < tMaxY_fp) ? tMaxX_fp : tMaxY_fp; |
| 182 | if (t_fp > maxT_fp) |
no test coverage detected