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

Function getBufferRegionsForSubmap

grid_map_core/src/GridMapMath.cpp:329–435  ·  view source on GitHub ↗

Source from the content-addressed store, hash-verified

327}
328
329bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
330 const Index& submapIndex,
331 const Size& submapBufferSize,
332 const Size& bufferSize,
333 const Index& bufferStartIndex)
334{
335 if ((getIndexFromBufferIndex(submapIndex, bufferSize, bufferStartIndex) + submapBufferSize > bufferSize).any()) return false;
336
337 submapBufferRegions.clear();
338
339 Index bottomRightIndex = submapIndex + submapBufferSize - Index::Ones();
340 wrapIndexToRange(bottomRightIndex, bufferSize);
341
342 BufferRegion::Quadrant quadrantOfTopLeft = getQuadrant(submapIndex, bufferStartIndex);
343 BufferRegion::Quadrant quadrantOfBottomRight = getQuadrant(bottomRightIndex, bufferStartIndex);
344
345 if (quadrantOfTopLeft == BufferRegion::Quadrant::TopLeft) {
346
347 if (quadrantOfBottomRight == BufferRegion::Quadrant::TopLeft) {
348 submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft));
349 return true;
350 }
351
352 if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
353 Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
354 submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
355
356 Index topRightIndex(submapIndex(0), 0);
357 Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1));
358 submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
359 return true;
360 }
361
362 if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
363 Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
364 submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
365
366 Index bottomLeftIndex(0, submapIndex(1));
367 Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1));
368 submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
369 return true;
370 }
371
372 if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
373 Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1));
374 submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
375
376 Index topRightIndex(submapIndex(0), 0);
377 Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1));
378 submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
379
380 Index bottomLeftIndex(0, submapIndex(1));
381 Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1));
382 submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
383
384 Index bottomRightIndex = Index::Zero();
385 Size bottomRightSize(bottomLeftSize(0), topRightSize(1));
386 submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));

Callers 3

TESTFunction · 0.85
getSubmapMethod · 0.85

Calls 5

getIndexFromBufferIndexFunction · 0.85
wrapIndexToRangeFunction · 0.85
getQuadrantFunction · 0.85
BufferRegionClass · 0.85
clearMethod · 0.80

Tested by 1

TESTFunction · 0.68