| 210 | } |
| 211 | |
| 212 | bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition, |
| 213 | IndicesMatrix *indicesMatrix) |
| 214 | { |
| 215 | |
| 216 | Index closestPointId; |
| 217 | if (!getClosestPointIndices(gridMap, queriedPosition, &closestPointId)) { |
| 218 | return false; |
| 219 | } |
| 220 | |
| 221 | Position closestPoint; |
| 222 | if (!gridMap.getPosition(closestPointId, closestPoint)) { |
| 223 | return false; |
| 224 | } |
| 225 | |
| 226 | const int idx0 = closestPointId.x(); |
| 227 | const int idy0 = closestPointId.y(); |
| 228 | const double x0 = closestPoint.x(); |
| 229 | const double y0 = closestPoint.y(); |
| 230 | const double x = queriedPosition.x(); |
| 231 | const double y = queriedPosition.y(); |
| 232 | |
| 233 | if (x > x0) { //first or fourth quadrant |
| 234 | if (y > y0) { //first quadrant |
| 235 | indicesMatrix->topLeft_ = Index(idx0, idy0 - 1); |
| 236 | indicesMatrix->topRight_ = Index(idx0 - 1, idy0 - 1); |
| 237 | indicesMatrix->bottomLeft_ = Index(idx0, idy0); |
| 238 | indicesMatrix->bottomRight_ = Index(idx0 - 1, idy0); |
| 239 | } else { // fourth quadrant |
| 240 | indicesMatrix->topLeft_ = Index(idx0, idy0); |
| 241 | indicesMatrix->topRight_ = Index(idx0 - 1, idy0); |
| 242 | indicesMatrix->bottomLeft_ = Index(idx0, idy0 + 1); |
| 243 | indicesMatrix->bottomRight_ = Index(idx0 - 1, idy0 + 1); |
| 244 | } |
| 245 | } else { // second or third quadrant |
| 246 | if (y > y0) { //second quadrant |
| 247 | indicesMatrix->topLeft_ = Index(idx0 + 1, idy0 - 1); |
| 248 | indicesMatrix->topRight_ = Index(idx0, idy0 - 1); |
| 249 | indicesMatrix->bottomLeft_ = Index(idx0 + 1, idy0); |
| 250 | indicesMatrix->bottomRight_ = Index(idx0, idy0); |
| 251 | } else { // third quadrant |
| 252 | indicesMatrix->topLeft_ = Index(idx0 + 1, idy0); |
| 253 | indicesMatrix->topRight_ = Index(idx0, idy0); |
| 254 | indicesMatrix->bottomLeft_ = Index(idx0 + 1, idy0 + 1); |
| 255 | indicesMatrix->bottomRight_ = Index(idx0, idy0 + 1); |
| 256 | } |
| 257 | } |
| 258 | |
| 259 | bindIndicesToRange(gridMap, indicesMatrix); |
| 260 | |
| 261 | return true; |
| 262 | |
| 263 | } |
| 264 | |
| 265 | bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index) |
| 266 | { |
no test coverage detected