| 239 | } |
| 240 | |
| 241 | void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition) |
| 242 | { |
| 243 | Vector vectorToOrigin; |
| 244 | getVectorToOrigin(vectorToOrigin, mapLength); |
| 245 | Position positionShifted = position - mapPosition + vectorToOrigin; |
| 246 | |
| 247 | // We have to make sure to stay inside the map. |
| 248 | for (int i = 0; i < positionShifted.size(); i++) { |
| 249 | |
| 250 | double epsilon = 10.0 * numeric_limits<double>::epsilon(); // TODO Why is the factor 10 necessary. |
| 251 | if (std::fabs(position(i)) > 1.0) epsilon *= std::fabs(position(i)); |
| 252 | |
| 253 | if (positionShifted(i) <= 0) { |
| 254 | positionShifted(i) = epsilon; |
| 255 | continue; |
| 256 | } |
| 257 | if (positionShifted(i) >= mapLength(i)) { |
| 258 | positionShifted(i) = mapLength(i) - epsilon; |
| 259 | continue; |
| 260 | } |
| 261 | } |
| 262 | |
| 263 | position = positionShifted + mapPosition - vectorToOrigin; |
| 264 | } |
| 265 | |
| 266 | const Eigen::Matrix2i getBufferOrderToMapFrameAlignment() |
| 267 | { |