00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_core/GridMapMath.hpp"
00010
00011
00012 #include <Eigen/Core>
00013
00014
00015 #include <gtest/gtest.h>
00016
00017
00018 #include <cfloat>
00019
00020
00021 #include <vector>
00022
00023 using namespace std;
00024 using namespace grid_map;
00025
00026 TEST(PositionFromIndex, Simple)
00027 {
00028 Length mapLength(3.0, 2.0);
00029 Position mapPosition(-1.0, 2.0);
00030 double resolution = 1.0;
00031 Size bufferSize(3, 2);
00032 Position position;
00033
00034 EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize));
00035 EXPECT_DOUBLE_EQ(1.0 + mapPosition.x(), position.x());
00036 EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y());
00037
00038 EXPECT_TRUE(getPositionFromIndex(position, Index(1, 0), mapLength, mapPosition, resolution, bufferSize));
00039 EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
00040 EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y());
00041
00042 EXPECT_TRUE(getPositionFromIndex(position, Index(1, 1), mapLength, mapPosition, resolution, bufferSize));
00043 EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
00044 EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y());
00045
00046 EXPECT_TRUE(getPositionFromIndex(position, Index(2, 1), mapLength, mapPosition, resolution, bufferSize));
00047 EXPECT_DOUBLE_EQ(-1.0 + mapPosition.x(), position.x());
00048 EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y());
00049
00050 EXPECT_FALSE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize));
00051 }
00052
00053 TEST(PositionFromIndex, CircularBuffer)
00054 {
00055 Length mapLength(0.5, 0.4);
00056 Position mapPosition(-0.1, 13.4);
00057 double resolution = 0.1;
00058 Size bufferSize(5, 4);
00059 Index bufferStartIndex(3, 1);
00060 Position position;
00061
00062 EXPECT_TRUE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00063 EXPECT_DOUBLE_EQ(0.2 + mapPosition.x(), position.x());
00064 EXPECT_DOUBLE_EQ(0.15 + mapPosition.y(), position.y());
00065
00066 EXPECT_TRUE(getPositionFromIndex(position, Index(4, 2), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00067 EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x());
00068 EXPECT_DOUBLE_EQ(0.05 + mapPosition.y(), position.y());
00069
00070 EXPECT_TRUE(getPositionFromIndex(position, Index(2, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00071 EXPECT_DOUBLE_EQ(-0.2 + mapPosition.x(), position.x());
00072 EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y());
00073
00074 EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00075 EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
00076 EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y());
00077
00078 EXPECT_TRUE(getPositionFromIndex(position, Index(4, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00079 EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x());
00080 EXPECT_DOUBLE_EQ(-0.05 + mapPosition.y(), position.y());
00081
00082 EXPECT_FALSE(getPositionFromIndex(position, Index(5, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00083 }
00084
00085 TEST(IndexFromPosition, Simple)
00086 {
00087 Length mapLength(3.0, 2.0);
00088 Position mapPosition(-12.4, -7.1);
00089 double resolution = 1.0;
00090 Index bufferSize(3, 2);
00091 Index index;
00092
00093 EXPECT_TRUE(getIndexFromPosition(index, Position(1.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
00094 EXPECT_EQ(0, index(0));
00095 EXPECT_EQ(0, index(1));
00096
00097 EXPECT_TRUE(getIndexFromPosition(index, Position(-1.0, -0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
00098 EXPECT_EQ(2, index(0));
00099 EXPECT_EQ(1, index(1));
00100
00101 EXPECT_TRUE(getIndexFromPosition(index, Position(0.6, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
00102 EXPECT_EQ(0, index(0));
00103 EXPECT_EQ(0, index(1));
00104
00105 EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, -0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
00106 EXPECT_EQ(1, index(0));
00107 EXPECT_EQ(1, index(1));
00108
00109 EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
00110 EXPECT_EQ(1, index(0));
00111 EXPECT_EQ(0, index(1));
00112
00113 EXPECT_FALSE(getIndexFromPosition(index, Position(4.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
00114 }
00115
00116 TEST(IndexFromPosition, EdgeCases)
00117 {
00118 Length mapLength(3.0, 2.0);
00119 Position mapPosition(0.0, 0.0);
00120 double resolution = 1.0;
00121 Size bufferSize(3, 2);
00122 Index index;
00123
00124 EXPECT_TRUE(getIndexFromPosition(index, Position(0.0, DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
00125 EXPECT_EQ(1, index(0));
00126 EXPECT_EQ(0, index(1));
00127
00128 EXPECT_TRUE(getIndexFromPosition(index, Position(0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
00129 EXPECT_EQ(1, index(0));
00130 EXPECT_EQ(1, index(1));
00131
00132 EXPECT_TRUE(getIndexFromPosition(index, Position(-0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
00133 EXPECT_EQ(2, index(0));
00134 EXPECT_EQ(1, index(1));
00135
00136 EXPECT_FALSE(getIndexFromPosition(index, Position(-1.5, 1.0), mapLength, mapPosition, resolution, bufferSize));
00137 }
00138
00139 TEST(IndexFromPosition, CircularBuffer)
00140 {
00141 Length mapLength(0.5, 0.4);
00142 Position mapPosition(0.4, -0.9);
00143 double resolution = 0.1;
00144 Size bufferSize(5, 4);
00145 Index bufferStartIndex(3, 1);
00146 Index index;
00147
00148 EXPECT_TRUE(getIndexFromPosition(index, Position(0.2, 0.15) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00149 EXPECT_EQ(3, index(0));
00150 EXPECT_EQ(1, index(1));
00151
00152 EXPECT_TRUE(getIndexFromPosition(index, Position(0.03, -0.17) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00153 EXPECT_EQ(0, index(0));
00154 EXPECT_EQ(0, index(1));
00155 }
00156
00157 TEST(checkIfPositionWithinMap, Inside)
00158 {
00159 Length mapLength(50.0, 25.0);
00160 Position mapPosition(11.4, 0.0);
00161
00162 EXPECT_TRUE(checkIfPositionWithinMap(Position(0.0, 0.0) + mapPosition, mapLength, mapPosition));
00163 EXPECT_TRUE(checkIfPositionWithinMap(Position(5.0, 5.0) + mapPosition, mapLength, mapPosition));
00164 EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, 10.0) + mapPosition, mapLength, mapPosition));
00165 EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, -10.0) + mapPosition, mapLength, mapPosition));
00166 EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, 10.0) + mapPosition, mapLength, mapPosition));
00167 EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, -10.0) + mapPosition, mapLength, mapPosition));
00168 }
00169
00170 TEST(checkIfPositionWithinMap, Outside)
00171 {
00172 Length mapLength(10.0, 5.0);
00173 Position mapPosition(-3.0, 145.2);
00174
00175 EXPECT_FALSE(checkIfPositionWithinMap(Position(5.5, 0.0) + mapPosition, mapLength, mapPosition));
00176 EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 0.0) + mapPosition, mapLength, mapPosition));
00177 EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 3.0) + mapPosition, mapLength, mapPosition));
00178 EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, -3.0) + mapPosition, mapLength, mapPosition));
00179 EXPECT_FALSE(checkIfPositionWithinMap(Position(3.0, 3.0) + mapPosition, mapLength, mapPosition));
00180 }
00181
00182 TEST(checkIfPositionWithinMap, EdgeCases)
00183 {
00184 Length mapLength(2.0, 3.0);
00185 Position mapPosition(0.0, 0.0);
00186
00187 EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, -1.5), mapLength, mapPosition));
00188 EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition));
00189 EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.0), mapLength, mapPosition));
00190 EXPECT_TRUE(checkIfPositionWithinMap(Position((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, mapPosition));
00191 EXPECT_FALSE(checkIfPositionWithinMap(Position(0.5, -1.5 - (2.0 * DBL_EPSILON)), mapLength, mapPosition));
00192 EXPECT_TRUE(checkIfPositionWithinMap(Position(-0.5, (3.0 + DBL_EPSILON) / 2.0), mapLength, mapPosition));
00193 }
00194
00195 TEST(getIndexShiftFromPositionShift, All)
00196 {
00197 double resolution = 1.0;
00198 Index indexShift;
00199
00200 EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.0, 0.0), resolution));
00201 EXPECT_EQ(0, indexShift(0));
00202 EXPECT_EQ(0, indexShift(1));
00203
00204 EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.35, -0.45), resolution));
00205 EXPECT_EQ(0, indexShift(0));
00206 EXPECT_EQ(0, indexShift(1));
00207
00208 EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.55, -0.45), resolution));
00209 EXPECT_EQ(-1, indexShift(0));
00210 EXPECT_EQ(0, indexShift(1));
00211
00212 EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-1.3, -2.65), resolution));
00213 EXPECT_EQ(1, indexShift(0));
00214 EXPECT_EQ(3, indexShift(1));
00215
00216 EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-0.4, 0.09), 0.2));
00217 EXPECT_EQ(2, indexShift(0));
00218 EXPECT_EQ(0, indexShift(1));
00219 }
00220
00221 TEST(getPositionShiftFromIndexShift, All)
00222 {
00223 double resolution = 0.3;
00224 Vector positionShift;
00225
00226 EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(0, 0), resolution));
00227 EXPECT_DOUBLE_EQ(0.0, positionShift.x());
00228 EXPECT_DOUBLE_EQ(0.0, positionShift.y());
00229
00230 EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(1, -1), resolution));
00231 EXPECT_DOUBLE_EQ(-0.3, positionShift.x());
00232 EXPECT_DOUBLE_EQ(0.3, positionShift.y());
00233
00234 EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(2, 1), resolution));
00235 EXPECT_DOUBLE_EQ(-0.6, positionShift.x());
00236 EXPECT_DOUBLE_EQ(-0.3, positionShift.y());
00237 }
00238
00239 TEST(checkIfIndexInRange, All)
00240 {
00241 Size bufferSize(10, 15);
00242 EXPECT_TRUE(checkIfIndexInRange(Index(0, 0), bufferSize));
00243 EXPECT_TRUE(checkIfIndexInRange(Index(9, 14), bufferSize));
00244 EXPECT_FALSE(checkIfIndexInRange(Index(10, 5), bufferSize));
00245 EXPECT_FALSE(checkIfIndexInRange(Index(5, 300), bufferSize));
00246 EXPECT_FALSE(checkIfIndexInRange(Index(-1, 0), bufferSize));
00247 EXPECT_FALSE(checkIfIndexInRange(Index(0, -300), bufferSize));
00248 }
00249
00250 TEST(boundIndexToRange, All)
00251 {
00252 int index;
00253 int bufferSize = 10;
00254
00255 index = 0;
00256 boundIndexToRange(index, bufferSize);
00257 EXPECT_EQ(0, index);
00258
00259 index = 1;
00260 boundIndexToRange(index, bufferSize);
00261 EXPECT_EQ(1, index);
00262
00263 index = -1;
00264 boundIndexToRange(index, bufferSize);
00265 EXPECT_EQ(0, index);
00266
00267 index = 9;
00268 boundIndexToRange(index, bufferSize);
00269 EXPECT_EQ(9, index);
00270
00271 index = 10;
00272 boundIndexToRange(index, bufferSize);
00273 EXPECT_EQ(9, index);
00274
00275 index = 35;
00276 boundIndexToRange(index, bufferSize);
00277 EXPECT_EQ(9, index);
00278
00279 index = -19;
00280 boundIndexToRange(index, bufferSize);
00281 EXPECT_EQ(0, index);
00282 }
00283
00284 TEST(wrapIndexToRange, All)
00285 {
00286 int index;
00287 int bufferSize = 10;
00288
00289 index = 0;
00290 wrapIndexToRange(index, bufferSize);
00291 EXPECT_EQ(0, index);
00292
00293 index = 1;
00294 wrapIndexToRange(index, bufferSize);
00295 EXPECT_EQ(1, index);
00296
00297 index = -1;
00298 wrapIndexToRange(index, bufferSize);
00299 EXPECT_EQ(9, index);
00300
00301 index = 9;
00302 wrapIndexToRange(index, bufferSize);
00303 EXPECT_EQ(9, index);
00304
00305 index = 10;
00306 wrapIndexToRange(index, bufferSize);
00307 EXPECT_EQ(0, index);
00308
00309 index = 11;
00310 wrapIndexToRange(index, bufferSize);
00311 EXPECT_EQ(1, index);
00312
00313 index = 35;
00314 wrapIndexToRange(index, bufferSize);
00315 EXPECT_EQ(5, index);
00316
00317 index = -9;
00318 wrapIndexToRange(index, bufferSize);
00319 EXPECT_EQ(1, index);
00320
00321 index = -19;
00322 wrapIndexToRange(index, bufferSize);
00323 EXPECT_EQ(1, index);
00324 }
00325
00326 TEST(boundPositionToRange, Simple)
00327 {
00328 double epsilon = 11.0 * numeric_limits<double>::epsilon();
00329
00330 Length mapLength(30.0, 10.0);
00331 Position mapPosition(0.0, 0.0);
00332 Position position;
00333
00334 position << 0.0, 0.0;
00335 boundPositionToRange(position, mapLength, mapPosition);
00336 EXPECT_DOUBLE_EQ(0.0, position.x());
00337 EXPECT_DOUBLE_EQ(0.0, position.y());
00338
00339 position << 15.0, 5.0;
00340 boundPositionToRange(position, mapLength, mapPosition);
00341 EXPECT_NEAR(15.0, position.x(), 15.0 * epsilon);
00342 EXPECT_GE(15.0, position.x());
00343 EXPECT_NEAR(5.0, position.y(), 5.0 * epsilon);
00344 EXPECT_GE(5.0, position.y());
00345
00346 position << -15.0, -5.0;
00347 boundPositionToRange(position, mapLength, mapPosition);
00348 EXPECT_NEAR(-15.0, position.x(), 15.0 * epsilon);
00349 EXPECT_LE(-15.0, position.x());
00350 EXPECT_NEAR(-5.0, position.y(), 5.0 * epsilon);
00351 EXPECT_LE(-5.0, position.y());
00352
00353 position << 16.0, 6.0;
00354 boundPositionToRange(position, mapLength, mapPosition);
00355 EXPECT_NEAR(15.0, position.x(), 16.0 * epsilon);
00356 EXPECT_GE(15.0, position.x());
00357 EXPECT_NEAR(5.0, position.y(), 6.0 * epsilon);
00358 EXPECT_GE(5.0, position.y());
00359
00360 position << -16.0, -6.0;
00361 boundPositionToRange(position, mapLength, mapPosition);
00362 EXPECT_NEAR(-15.0, position.x(), 16.0 * epsilon);
00363 EXPECT_LE(-15.0, position.x());
00364 EXPECT_NEAR(-5.0, position.y(), 6.0 * epsilon);
00365 EXPECT_LE(-5.0, position.y());
00366
00367 position << 1e6, 1e6;
00368 boundPositionToRange(position, mapLength, mapPosition);
00369 EXPECT_NEAR(15.0, position.x(), 1e6 * epsilon);
00370 EXPECT_GE(15.0, position.x());
00371 EXPECT_NEAR(5.0, position.y(), 1e6 * epsilon);
00372 EXPECT_GE(5.0, position.y());
00373
00374 position << -1e6, -1e6;
00375 boundPositionToRange(position, mapLength, mapPosition);
00376 EXPECT_NEAR(-15.0, position.x(), 1e6 * epsilon);
00377 EXPECT_LE(-15.0, position.x());
00378 EXPECT_NEAR(-5.0, position.y(), 1e6 * epsilon);
00379 EXPECT_LE(-5.0, position.y());
00380 }
00381
00382 TEST(boundPositionToRange, Position)
00383 {
00384 double epsilon = 11.0 * numeric_limits<double>::epsilon();
00385
00386 Length mapLength(30.0, 10.0);
00387 Position mapPosition(1.0, 2.0);
00388 Position position;
00389
00390 position << 0.0, 0.0;
00391 boundPositionToRange(position, mapLength, mapPosition);
00392 EXPECT_DOUBLE_EQ(0.0, position.x());
00393 EXPECT_DOUBLE_EQ(0.0, position.y());
00394
00395 position << 16.0, 7.0;
00396 boundPositionToRange(position, mapLength, mapPosition);
00397 EXPECT_NEAR(16.0, position.x(), 16.0 * epsilon);
00398 EXPECT_GE(16.0, position.x());
00399 EXPECT_NEAR(7.0, position.y(), 7.0 * epsilon);
00400 EXPECT_GE(7.0, position.y());
00401
00402 position << -14.0, -3.0;
00403 boundPositionToRange(position, mapLength, mapPosition);
00404 EXPECT_NEAR(-14.0, position.x(), 14.0 * epsilon);
00405 EXPECT_LE(-14.0, position.x());
00406 EXPECT_NEAR(-3.0, position.y(), 3.0 * epsilon);
00407 EXPECT_LE(-3.0, position.y());
00408
00409 position << 17.0, 8.0;
00410 boundPositionToRange(position, mapLength, mapPosition);
00411 EXPECT_NEAR(16.0, position.x(), 17.0 * epsilon);
00412 EXPECT_GE(16.0, position.x());
00413 EXPECT_NEAR(7.0, position.y(), 8.0 * epsilon);
00414 EXPECT_GE(7.0, position.y());
00415
00416 position << -15.0, -4.0;
00417 boundPositionToRange(position, mapLength, mapPosition);
00418 EXPECT_NEAR(-14.0, position.x(), 15.0 * epsilon);
00419 EXPECT_LE(-14.0, position.x());
00420 EXPECT_NEAR(-3.0, position.y(), 4.0 * epsilon);
00421 EXPECT_LE(-3.0, position.y());
00422
00423 position << 1e6, 1e6;
00424 boundPositionToRange(position, mapLength, mapPosition);
00425 EXPECT_NEAR(16.0, position.x(), 1e6 * epsilon);
00426 EXPECT_GE(16.0, position.x());
00427 EXPECT_NEAR(7.0, position.y(), 1e6 * epsilon);
00428 EXPECT_GE(7.0, position.y());
00429
00430 position << -1e6, -1e6;
00431 boundPositionToRange(position, mapLength, mapPosition);
00432 EXPECT_NEAR(-14.0, position.x(), 1e6 * epsilon);
00433 EXPECT_LE(-14.0, position.x());
00434 EXPECT_NEAR(-3.0, position.y(), 1e6 * epsilon);
00435 EXPECT_LE(-3.0, position.y());
00436 }
00437
00438 TEST(getSubmapInformation, Simple)
00439 {
00440
00441 Length mapLength(5.0, 4.0);
00442 Position mapPosition(0.0, 0.0);
00443 double resolution = 1.0;
00444 Size bufferSize(5, 4);
00445
00446
00447 Position requestedSubmapPosition;
00448 Position requestedSubmapLength;
00449
00450
00451 Index submapTopLeftIndex;
00452 Index submapSize;
00453 Position submapPosition;
00454 Length submapLength;
00455 Index requestedIndexInSubmap;
00456
00457 requestedSubmapPosition << 0.0, 0.5;
00458 requestedSubmapLength << 0.9, 2.9;
00459 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00460 requestedSubmapPosition, requestedSubmapLength, mapLength, mapPosition, resolution, bufferSize));
00461 EXPECT_EQ(2, submapTopLeftIndex(0));
00462 EXPECT_EQ(0, submapTopLeftIndex(1));
00463 EXPECT_EQ(1, submapSize(0));
00464 EXPECT_EQ(3, submapSize(1));
00465 EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
00466 EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
00467 EXPECT_DOUBLE_EQ(1.0, submapLength(0));
00468 EXPECT_DOUBLE_EQ(3.0, submapLength(1));
00469 EXPECT_EQ(0, requestedIndexInSubmap(0));
00470 EXPECT_EQ(1, requestedIndexInSubmap(1));
00471 }
00472
00473 TEST(getSubmapInformation, Zero)
00474 {
00475
00476 Length mapLength(5.0, 4.0);
00477 Position mapPosition(0.0, 0.0);
00478 double resolution = 1.0;
00479 Size bufferSize(5, 4);
00480
00481
00482 Position requestedSubmapPosition;
00483 Length requestedSubmapLength;
00484
00485
00486 Index submapTopLeftIndex;
00487 Index submapSize;
00488 Position submapPosition;
00489 Length submapLength;
00490 Index requestedIndexInSubmap;
00491
00492 requestedSubmapPosition << -1.0, -0.5;
00493 requestedSubmapLength << 0.0, 0.0;
00494 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00495 requestedSubmapPosition, requestedSubmapLength,
00496 mapLength, mapPosition, resolution, bufferSize));
00497 EXPECT_EQ(3, submapTopLeftIndex(0));
00498 EXPECT_EQ(2, submapTopLeftIndex(1));
00499 EXPECT_EQ(1, submapSize(0));
00500 EXPECT_EQ(1, submapSize(1));
00501 EXPECT_DOUBLE_EQ(requestedSubmapPosition.x(), submapPosition.x());
00502 EXPECT_DOUBLE_EQ(requestedSubmapPosition.y(), submapPosition.y());
00503 EXPECT_DOUBLE_EQ(resolution, submapLength(0));
00504 EXPECT_DOUBLE_EQ(resolution, submapLength(1));
00505 EXPECT_EQ(0, requestedIndexInSubmap(0));
00506 EXPECT_EQ(0, requestedIndexInSubmap(1));
00507 }
00508
00509 TEST(getSubmapInformation, ExceedingBoundaries)
00510 {
00511
00512 Length mapLength(5.0, 4.0);
00513 Position mapPosition(0.0, 0.0);
00514 double resolution = 1.0;
00515 Size bufferSize(5, 4);
00516
00517
00518 Position requestedSubmapPosition;
00519 Length requestedSubmapLength;
00520
00521
00522 Index submapTopLeftIndex;
00523 Size submapSize;
00524 Position submapPosition;
00525 Length submapLength;
00526 Index requestedIndexInSubmap;
00527
00528 requestedSubmapPosition << 2.0, 1.5;
00529 requestedSubmapLength << 2.9, 2.9;
00530 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00531 requestedSubmapPosition, requestedSubmapLength,
00532 mapLength, mapPosition, resolution, bufferSize));
00533 EXPECT_EQ(0, submapTopLeftIndex(0));
00534 EXPECT_EQ(0, submapTopLeftIndex(1));
00535 EXPECT_EQ(2, submapSize(0));
00536 EXPECT_EQ(2, submapSize(1));
00537 EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
00538 EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
00539 EXPECT_DOUBLE_EQ(2.0, submapLength(0));
00540 EXPECT_DOUBLE_EQ(2.0, submapLength(1));
00541 EXPECT_EQ(0, requestedIndexInSubmap(0));
00542 EXPECT_EQ(0, requestedIndexInSubmap(1));
00543
00544 requestedSubmapPosition << 0.0, 0.0;
00545 requestedSubmapLength << 1e6, 1e6;
00546 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00547 requestedSubmapPosition, requestedSubmapLength,
00548 mapLength, mapPosition, resolution, bufferSize));
00549 EXPECT_EQ(0, submapTopLeftIndex(0));
00550 EXPECT_EQ(0, submapTopLeftIndex(1));
00551 EXPECT_EQ(bufferSize(0), submapSize(0));
00552 EXPECT_EQ(bufferSize(1), submapSize(1));
00553 EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
00554 EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
00555 EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
00556 EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
00557 EXPECT_EQ(2, requestedIndexInSubmap(0));
00558 EXPECT_LE(1, requestedIndexInSubmap(1));
00559 EXPECT_GE(2, requestedIndexInSubmap(1));
00560 }
00561
00562 TEST(getSubmapInformation, CircularBuffer)
00563 {
00564
00565 Length mapLength(5.0, 4.0);
00566 Position mapPosition(0.0, 0.0);
00567 double resolution = 1.0;
00568 Size bufferSize(5, 4);
00569 Index bufferStartIndex(2, 1);
00570
00571
00572 Position requestedSubmapPosition;
00573 Length requestedSubmapLength;
00574
00575
00576 Index submapTopLeftIndex;
00577 Size submapSize;
00578 Position submapPosition;
00579 Length submapLength;
00580 Index requestedIndexInSubmap;
00581
00582 requestedSubmapPosition << 0.0, 0.5;
00583 requestedSubmapLength << 0.9, 2.9;
00584 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00585 requestedSubmapPosition, requestedSubmapLength,
00586 mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00587 EXPECT_EQ(4, submapTopLeftIndex(0));
00588 EXPECT_EQ(1, submapTopLeftIndex(1));
00589 EXPECT_EQ(1, submapSize(0));
00590 EXPECT_EQ(3, submapSize(1));
00591 EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
00592 EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
00593 EXPECT_DOUBLE_EQ(1.0, submapLength(0));
00594 EXPECT_DOUBLE_EQ(3.0, submapLength(1));
00595 EXPECT_EQ(0, requestedIndexInSubmap(0));
00596 EXPECT_EQ(1, requestedIndexInSubmap(1));
00597
00598 requestedSubmapPosition << 2.0, 1.5;
00599 requestedSubmapLength << 2.9, 2.9;
00600 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00601 requestedSubmapPosition, requestedSubmapLength,
00602 mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00603 EXPECT_EQ(2, submapTopLeftIndex(0));
00604 EXPECT_EQ(1, submapTopLeftIndex(1));
00605 EXPECT_EQ(2, submapSize(0));
00606 EXPECT_EQ(2, submapSize(1));
00607 EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
00608 EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
00609 EXPECT_DOUBLE_EQ(2.0, submapLength(0));
00610 EXPECT_DOUBLE_EQ(2.0, submapLength(1));
00611 EXPECT_EQ(0, requestedIndexInSubmap(0));
00612 EXPECT_EQ(0, requestedIndexInSubmap(1));
00613
00614 requestedSubmapPosition << 0.0, 0.0;
00615 requestedSubmapLength << 1e6, 1e6;
00616 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00617 requestedSubmapPosition, requestedSubmapLength,
00618 mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00619 EXPECT_EQ(2, submapTopLeftIndex(0));
00620 EXPECT_EQ(1, submapTopLeftIndex(1));
00621 EXPECT_EQ(bufferSize(0), submapSize(0));
00622 EXPECT_EQ(bufferSize(1), submapSize(1));
00623 EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
00624 EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
00625 EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
00626 EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
00627 EXPECT_EQ(2, requestedIndexInSubmap(0));
00628 EXPECT_LE(1, requestedIndexInSubmap(1));
00629 EXPECT_GE(2, requestedIndexInSubmap(1));
00630 }
00631
00632 TEST(getSubmapInformation, Debug1)
00633 {
00634
00635 Length mapLength(4.98, 4.98);
00636 Position mapPosition(-4.98, -5.76);
00637 double resolution = 0.06;
00638 Size bufferSize(83, 83);
00639 Index bufferStartIndex(0, 13);
00640
00641
00642 Position requestedSubmapPosition(-7.44, -3.42);
00643 Length requestedSubmapLength(0.12, 0.12);
00644
00645
00646 Index submapTopLeftIndex;
00647 Size submapSize;
00648 Position submapPosition;
00649 Length submapLength;
00650 Index requestedIndexInSubmap;
00651
00652 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00653 requestedSubmapPosition, requestedSubmapLength,
00654 mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00655 EXPECT_EQ(2, submapSize(0));
00656 EXPECT_EQ(3, submapSize(1));
00657 EXPECT_DOUBLE_EQ(0.12, submapLength(0));
00658 EXPECT_DOUBLE_EQ(0.18, submapLength(1));
00659 }
00660
00661 TEST(getSubmapInformation, Debug2)
00662 {
00663
00664 Length mapLength(4.98, 4.98);
00665 Position mapPosition(2.46, -25.26);
00666 double resolution = 0.06;
00667 Size bufferSize(83, 83);
00668 Index bufferStartIndex(42, 6);
00669
00670
00671 Position requestedSubmapPosition(0.24, -26.82);
00672 Length requestedSubmapLength(0.624614, 0.462276);
00673
00674
00675 Index submapTopLeftIndex;
00676 Size submapSize;
00677 Position submapPosition;
00678 Length submapLength;
00679 Index requestedIndexInSubmap;
00680
00681 EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
00682 requestedSubmapPosition, requestedSubmapLength,
00683 mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
00684 EXPECT_LT(0, submapSize(0));
00685 EXPECT_LT(0, submapSize(1));
00686 EXPECT_LT(0.0, submapLength(0));
00687 EXPECT_LT(0.0, submapLength(1));
00688 }
00689
00690 TEST(getBufferRegionsForSubmap, Trivial)
00691 {
00692 Size bufferSize(5, 4);
00693 Index submapIndex(0, 0);
00694 Size submapSize(0, 0);
00695 std::vector<BufferRegion> regions;
00696
00697 EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
00698 EXPECT_EQ(1, regions.size());
00699 EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
00700 EXPECT_EQ(0, regions[0].getStartIndex()[0]);
00701 EXPECT_EQ(0, regions[0].getStartIndex()[1]);
00702 EXPECT_EQ(0, regions[0].getSize()[0]);
00703 EXPECT_EQ(0, regions[0].getSize()[1]);
00704
00705 submapSize << 0, 7;
00706 EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
00707
00708 submapSize << 6, 7;
00709 EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
00710 }
00711
00712 TEST(getBufferRegionsForSubmap, Simple)
00713 {
00714 Size bufferSize(5, 4);
00715 Index submapIndex(1, 2);
00716 Size submapSize(3, 2);
00717 std::vector<BufferRegion> regions;
00718
00719 EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
00720 EXPECT_EQ(1, regions.size());
00721 EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
00722 EXPECT_EQ(1, regions[0].getStartIndex()[0]);
00723 EXPECT_EQ(2, regions[0].getStartIndex()[1]);
00724 EXPECT_EQ(3, regions[0].getSize()[0]);
00725 EXPECT_EQ(2, regions[0].getSize()[1]);
00726 }
00727
00728 TEST(getBufferRegionsForSubmap, CircularBuffer)
00729 {
00730 Size bufferSize(5, 4);
00731 Index submapIndex;
00732 Size submapSize;
00733 Index bufferStartIndex(3, 1);
00734 std::vector<BufferRegion> regions;
00735
00736 submapIndex << 3, 1;
00737 submapSize << 2, 3;
00738 EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
00739 EXPECT_EQ(1, regions.size());
00740 EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
00741 EXPECT_EQ(3, regions[0].getStartIndex()[0]);
00742 EXPECT_EQ(1, regions[0].getStartIndex()[1]);
00743 EXPECT_EQ(2, regions[0].getSize()[0]);
00744 EXPECT_EQ(3, regions[0].getSize()[1]);
00745
00746 submapIndex << 4, 1;
00747 submapSize << 2, 3;
00748 EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
00749 EXPECT_EQ(2, regions.size());
00750 EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
00751 EXPECT_EQ(4, regions[0].getStartIndex()[0]);
00752 EXPECT_EQ(1, regions[0].getStartIndex()[1]);
00753 EXPECT_EQ(1, regions[0].getSize()[0]);
00754 EXPECT_EQ(3, regions[0].getSize()[1]);
00755 EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[1].getQuadrant());
00756 EXPECT_EQ(0, regions[1].getStartIndex()[0]);
00757 EXPECT_EQ(1, regions[1].getStartIndex()[1]);
00758 EXPECT_EQ(1, regions[1].getSize()[0]);
00759 EXPECT_EQ(3, regions[1].getSize()[1]);
00760
00761 submapIndex << 1, 0;
00762 submapSize << 2, 1;
00763 EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
00764 EXPECT_EQ(1, regions.size());
00765 EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[0].getQuadrant());
00766 EXPECT_EQ(1, regions[0].getStartIndex()[0]);
00767 EXPECT_EQ(0, regions[0].getStartIndex()[1]);
00768 EXPECT_EQ(2, regions[0].getSize()[0]);
00769 EXPECT_EQ(1, regions[0].getSize()[1]);
00770
00771 submapIndex << 3, 1;
00772 submapSize << 5, 4;
00773 EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));\
00774 EXPECT_EQ(4, regions.size());
00775 EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
00776 EXPECT_EQ(3, regions[0].getStartIndex()[0]);
00777 EXPECT_EQ(1, regions[0].getStartIndex()[1]);
00778 EXPECT_EQ(2, regions[0].getSize()[0]);
00779 EXPECT_EQ(3, regions[0].getSize()[1]);
00780 EXPECT_EQ(BufferRegion::Quadrant::TopRight, regions[1].getQuadrant());
00781 EXPECT_EQ(3, regions[1].getStartIndex()[0]);
00782 EXPECT_EQ(0, regions[1].getStartIndex()[1]);
00783 EXPECT_EQ(2, regions[1].getSize()[0]);
00784 EXPECT_EQ(1, regions[1].getSize()[1]);
00785 EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[2].getQuadrant());
00786 EXPECT_EQ(0, regions[2].getStartIndex()[0]);
00787 EXPECT_EQ(1, regions[2].getStartIndex()[1]);
00788 EXPECT_EQ(3, regions[2].getSize()[0]);
00789 EXPECT_EQ(3, regions[2].getSize()[1]);
00790 EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[3].getQuadrant());
00791 EXPECT_EQ(0, regions[3].getStartIndex()[0]);
00792 EXPECT_EQ(0, regions[3].getStartIndex()[1]);
00793 EXPECT_EQ(3, regions[3].getSize()[0]);
00794 EXPECT_EQ(1, regions[3].getSize()[1]);
00795 }
00796
00797 TEST(checkIncrementIndex, Simple)
00798 {
00799 Index index(0, 0);
00800 Size bufferSize(4, 3);
00801
00802 EXPECT_TRUE(incrementIndex(index, bufferSize));
00803 EXPECT_EQ(0, index[0]);
00804 EXPECT_EQ(1, index[1]);
00805
00806 EXPECT_TRUE(incrementIndex(index, bufferSize));
00807 EXPECT_EQ(0, index[0]);
00808 EXPECT_EQ(2, index[1]);
00809
00810 EXPECT_TRUE(incrementIndex(index, bufferSize));
00811 EXPECT_EQ(1, index[0]);
00812 EXPECT_EQ(0, index[1]);
00813
00814 EXPECT_TRUE(incrementIndex(index, bufferSize));
00815 EXPECT_EQ(1, index[0]);
00816 EXPECT_EQ(1, index[1]);
00817
00818 for (int i = 0; i < 6; i++) {
00819 EXPECT_TRUE(incrementIndex(index, bufferSize));
00820 }
00821 EXPECT_EQ(3, index[0]);
00822 EXPECT_EQ(1, index[1]);
00823
00824 EXPECT_TRUE(incrementIndex(index, bufferSize));
00825 EXPECT_EQ(3, index[0]);
00826 EXPECT_EQ(2, index[1]);
00827
00828 EXPECT_FALSE(incrementIndex(index, bufferSize));
00829 EXPECT_EQ(index[0], index[0]);
00830 EXPECT_EQ(index[1], index[1]);
00831 }
00832
00833 TEST(checkIncrementIndex, CircularBuffer)
00834 {
00835 Size bufferSize(4, 3);
00836 Index bufferStartIndex(2, 1);
00837 Index index(bufferStartIndex);
00838
00839 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00840 EXPECT_EQ(2, index[0]);
00841 EXPECT_EQ(2, index[1]);
00842
00843 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00844 EXPECT_EQ(2, index[0]);
00845 EXPECT_EQ(0, index[1]);
00846
00847 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00848 EXPECT_EQ(3, index[0]);
00849 EXPECT_EQ(1, index[1]);
00850
00851 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00852 EXPECT_EQ(3, index[0]);
00853 EXPECT_EQ(2, index[1]);
00854
00855 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00856 EXPECT_EQ(3, index[0]);
00857 EXPECT_EQ(0, index[1]);
00858
00859 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00860 EXPECT_EQ(0, index[0]);
00861 EXPECT_EQ(1, index[1]);
00862
00863 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00864 EXPECT_EQ(0, index[0]);
00865 EXPECT_EQ(2, index[1]);
00866
00867 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00868 EXPECT_EQ(0, index[0]);
00869 EXPECT_EQ(0, index[1]);
00870
00871 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00872 EXPECT_EQ(1, index[0]);
00873 EXPECT_EQ(1, index[1]);
00874
00875 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00876 EXPECT_EQ(1, index[0]);
00877 EXPECT_EQ(2, index[1]);
00878
00879 EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
00880 EXPECT_EQ(1, index[0]);
00881 EXPECT_EQ(0, index[1]);
00882
00883 EXPECT_FALSE(incrementIndex(index, bufferSize, bufferStartIndex));
00884 EXPECT_EQ(index[0], index[0]);
00885 EXPECT_EQ(index[1], index[1]);
00886 }
00887
00888 TEST(checkIncrementIndexForSubmap, Simple)
00889 {
00890 Index submapIndex(0, 0);
00891 Index index;
00892 Index submapTopLeftIndex(3, 1);
00893 Size submapBufferSize(2, 4);
00894 Size bufferSize(8, 5);
00895
00896 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
00897 EXPECT_EQ(0, submapIndex[0]);
00898 EXPECT_EQ(1, submapIndex[1]);
00899 EXPECT_EQ(3, index[0]);
00900 EXPECT_EQ(2, index[1]);
00901
00902 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
00903 EXPECT_EQ(0, submapIndex[0]);
00904 EXPECT_EQ(2, submapIndex[1]);
00905 EXPECT_EQ(3, index[0]);
00906 EXPECT_EQ(3, index[1]);
00907
00908 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
00909 EXPECT_EQ(0, submapIndex[0]);
00910 EXPECT_EQ(3, submapIndex[1]);
00911 EXPECT_EQ(3, index[0]);
00912 EXPECT_EQ(4, index[1]);
00913
00914 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
00915 EXPECT_EQ(1, submapIndex[0]);
00916 EXPECT_EQ(0, submapIndex[1]);
00917 EXPECT_EQ(4, index[0]);
00918 EXPECT_EQ(1, index[1]);
00919
00920 submapIndex << 1, 2;
00921 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
00922 EXPECT_EQ(1, submapIndex[0]);
00923 EXPECT_EQ(3, submapIndex[1]);
00924 EXPECT_EQ(4, index[0]);
00925 EXPECT_EQ(4, index[1]);
00926
00927 EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
00928
00929 submapIndex << 2, 0;
00930 EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
00931 }
00932
00933 TEST(checkIncrementIndexForSubmap, CircularBuffer)
00934 {
00935 Index submapIndex(0, 0);
00936 Index index;
00937 Index submapTopLeftIndex(6, 3);
00938 Size submapBufferSize(2, 4);
00939 Size bufferSize(8, 5);
00940 Index bufferStartIndex(3, 2);
00941
00942 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
00943 EXPECT_EQ(0, submapIndex[0]);
00944 EXPECT_EQ(1, submapIndex[1]);
00945 EXPECT_EQ(6, index[0]);
00946 EXPECT_EQ(4, index[1]);
00947
00948 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
00949 EXPECT_EQ(0, submapIndex[0]);
00950 EXPECT_EQ(2, submapIndex[1]);
00951 EXPECT_EQ(6, index[0]);
00952 EXPECT_EQ(0, index[1]);
00953
00954 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
00955 EXPECT_EQ(0, submapIndex[0]);
00956 EXPECT_EQ(3, submapIndex[1]);
00957 EXPECT_EQ(6, index[0]);
00958 EXPECT_EQ(1, index[1]);
00959
00960 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
00961 EXPECT_EQ(1, submapIndex[0]);
00962 EXPECT_EQ(0, submapIndex[1]);
00963 EXPECT_EQ(7, index[0]);
00964 EXPECT_EQ(3, index[1]);
00965
00966 submapIndex << 1, 2;
00967 EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
00968 EXPECT_EQ(1, submapIndex[0]);
00969 EXPECT_EQ(3, submapIndex[1]);
00970 EXPECT_EQ(7, index[0]);
00971 EXPECT_EQ(1, index[1]);
00972
00973 EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
00974
00975 submapIndex << 2, 0;
00976 EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
00977 }
00978
00979 TEST(getIndexFromLinearIndex, Simple)
00980 {
00981 EXPECT_TRUE((Index(0, 0) == getIndexFromLinearIndex(0, Size(8, 5), false)).all());
00982 EXPECT_TRUE((Index(1, 0) == getIndexFromLinearIndex(1, Size(8, 5), false)).all());
00983 EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(1, Size(8, 5), true)).all());
00984 EXPECT_TRUE((Index(2, 0) == getIndexFromLinearIndex(2, Size(8, 5), false)).all());
00985 EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(8, Size(8, 5), false)).all());
00986 EXPECT_TRUE((Index(7, 4) == getIndexFromLinearIndex(39, Size(8, 5), false)).all());
00987 }