| 127 | } |
| 128 | |
| 129 | bool getIndexFromPosition(Index& index, |
| 130 | const Position& position, |
| 131 | const Length& mapLength, |
| 132 | const Position& mapPosition, |
| 133 | const double& resolution, |
| 134 | const Size& bufferSize, |
| 135 | const Index& bufferStartIndex) |
| 136 | { |
| 137 | Vector offset; |
| 138 | getVectorToOrigin(offset, mapLength); |
| 139 | Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix(); |
| 140 | index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex); |
| 141 | return checkIfPositionWithinMap(position, mapLength, mapPosition) && checkIfIndexInRange(index, bufferSize); |
| 142 | } |
| 143 | |
| 144 | bool checkIfPositionWithinMap(const Position& position, |
| 145 | const Length& mapLength, |