GridMapMathTest.cpp
Go to the documentation of this file.
00001 /*
00002  * GridMapMathTest.cpp
00003  *
00004  *  Created on: Feb 10, 2014
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_core/GridMapMath.hpp"
00010 
00011 // Eigen
00012 #include <Eigen/Core>
00013 
00014 // gtest
00015 #include <gtest/gtest.h>
00016 
00017 // Limits
00018 #include <cfloat>
00019 
00020 // Vector
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   // Map
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   // Requested submap
00447   Position requestedSubmapPosition;
00448   Position requestedSubmapLength;
00449 
00450   // The returned submap indeces
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   // Map
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   // Requested submap
00482   Position requestedSubmapPosition;
00483   Length requestedSubmapLength;
00484 
00485   // The returned submap indeces
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   // Map
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   // Requested submap
00518   Position requestedSubmapPosition;
00519   Length requestedSubmapLength;
00520 
00521   // The returned submap indeces
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   // Map
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   // Requested submap
00572   Position requestedSubmapPosition;
00573   Length requestedSubmapLength;
00574 
00575   // The returned submap indeces
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   // Map
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   // Requested submap
00642   Position requestedSubmapPosition(-7.44, -3.42);
00643   Length requestedSubmapLength(0.12, 0.12);
00644 
00645   // The returned submap indeces
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   // Map
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   // Requested submap
00671   Position requestedSubmapPosition(0.24, -26.82);
00672   Length requestedSubmapLength(0.624614, 0.462276);
00673 
00674   // The returned submap indeces
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 }


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:13