Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <cstddef>
00031 #include <limits>
00032
00033 #include <gtest/gtest.h>
00034
00035 #include <planner_cspace/blockmem_gridmap.h>
00036
00037 TEST(BlockmemGridmap, ResetClear)
00038 {
00039 BlockMemGridmap<float, 3, 3, 0x20> gm;
00040
00041 for (int s = 4; s <= 6; s += 2)
00042 {
00043 gm.reset(CyclicVecInt<3, 3>(s, s, s));
00044 gm.clear(0.0);
00045
00046 CyclicVecInt<3, 3> i;
00047 for (i[0] = 0; i[0] < s; ++i[0])
00048 {
00049 for (i[1] = 0; i[1] < s; ++i[1])
00050 {
00051 for (i[2] = 0; i[2] < s; ++i[2])
00052 {
00053 ASSERT_EQ(gm[i], 0.0);
00054 }
00055 }
00056 }
00057
00058 gm.clear(3.0);
00059 for (i[0] = 0; i[0] < s; ++i[0])
00060 {
00061 for (i[1] = 0; i[1] < s; ++i[1])
00062 {
00063 for (i[2] = 0; i[2] < s; ++i[2])
00064 {
00065 ASSERT_EQ(gm[i], 3.0);
00066 }
00067 }
00068 }
00069 }
00070 }
00071
00072 TEST(BlockmemGridmap, WriteRead)
00073 {
00074 BlockMemGridmap<float, 3, 3, 0x20> gm;
00075
00076 const int s = 4;
00077 gm.reset(CyclicVecInt<3, 3>(s, s, s));
00078 gm.clear(0.0);
00079
00080 CyclicVecInt<3, 3> i;
00081 for (i[0] = 0; i[0] < s; ++i[0])
00082 {
00083 for (i[1] = 0; i[1] < s; ++i[1])
00084 {
00085 for (i[2] = 0; i[2] < s; ++i[2])
00086 {
00087 gm[i] = i[2] * 100 + i[1] * 10 + i[0];
00088 }
00089 }
00090 }
00091
00092 for (i[0] = 0; i[0] < s; ++i[0])
00093 {
00094 for (i[1] = 0; i[1] < s; ++i[1])
00095 {
00096 for (i[2] = 0; i[2] < s; ++i[2])
00097 {
00098 ASSERT_EQ(gm[i], i[2] * 100 + i[1] * 10 + i[0]);
00099 }
00100 }
00101 }
00102 }
00103
00104 TEST(BlockmemGridmap, OuterBoundery)
00105 {
00106 BlockMemGridmap<float, 3, 2, 0x20> gm;
00107
00108 const int s = 0x30;
00109 gm.reset(CyclicVecInt<3, 2>(s, s, s));
00110 gm.clear(1.0);
00111
00112 CyclicVecInt<3, 2> i;
00113 const int outer = 0x10;
00114 for (i[0] = -outer; i[0] < s + outer; ++i[0])
00115 {
00116 for (i[1] = -outer; i[1] < s + outer; ++i[1])
00117 {
00118 for (i[2] = -outer; i[2] < s + outer; ++i[2])
00119 {
00120 if (i[0] >= 0 && i[1] >= 0 && i[2] >= 0 &&
00121 i[0] < s && i[1] < s && i[2] < s)
00122 {
00123 ASSERT_TRUE(gm.validate(i));
00124 }
00125 else
00126 {
00127 ASSERT_FALSE(gm.validate(i));
00128 }
00129
00130 gm[i] = 1.0;
00131 }
00132 }
00133 }
00134 }
00135
00136 int main(int argc, char** argv)
00137 {
00138 testing::InitGoogleTest(&argc, argv);
00139
00140 return RUN_ALL_TESTS();
00141 }