MCPcopy Create free account
hub / github.com/ANYbotics/grid_map / incrementIndexForSubmap

Function incrementIndexForSubmap

grid_map_core/src/GridMapMath.cpp:459–488  ·  view source on GitHub ↗

Source from the content-addressed store, hash-verified

457}
458
459bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex,
460 const Size& submapBufferSize, const Size& bufferSize,
461 const Index& bufferStartIndex)
462{
463 // Copy the data first, only copy it back if everything is within range.
464 Index tempIndex = index;
465 Index tempSubmapIndex = submapIndex;
466
467 // Increment submap index.
468 if (tempSubmapIndex[1] + 1 < submapBufferSize[1]) {
469 // Same row.
470 tempSubmapIndex[1]++;
471 } else {
472 // Next row.
473 tempSubmapIndex[0]++;
474 tempSubmapIndex[1] = 0;
475 }
476
477 // End of iterations reached.
478 if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) return false;
479
480 // Get corresponding index in map.
481 Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
482 tempIndex = getBufferIndexFromIndex(unwrappedSubmapTopLeftIndex + tempSubmapIndex, bufferSize, bufferStartIndex);
483
484 // Copy data back.
485 index = tempIndex;
486 submapIndex = tempSubmapIndex;
487 return true;
488}
489
490Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, const Index& bufferStartIndex)
491{

Callers 2

TESTFunction · 0.85
SubmapIterator.cppFile · 0.85

Calls 3

checkIfIndexInRangeFunction · 0.85
getIndexFromBufferIndexFunction · 0.85
getBufferIndexFromIndexFunction · 0.85

Tested by 1

TESTFunction · 0.68