00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_core/GridMap.hpp"
00010 #include "grid_map_sdf/SignedDistanceField.hpp"
00011
00012 #include <gtest/gtest.h>
00013 #include <math.h>
00014
00015 using namespace std;
00016 using namespace grid_map;
00017
00018 TEST(SignedDistanceField, EmptyMap)
00019 {
00020 GridMap map({"layer"});
00021 map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0));
00022
00023 SignedDistanceField sdf;
00024 sdf.calculateSignedDistanceField(map, "layer", 1.0);
00025 Position3 position(0.0, 0.0, 0.0);
00026
00027 EXPECT_NO_THROW(sdf.getDistanceAt(position));
00028 EXPECT_NO_THROW(sdf.getInterpolatedDistanceAt(position));
00029 EXPECT_NO_THROW(sdf.getDistanceGradientAt(position));
00030 }
00031
00032 TEST(SignedDistanceField, GetDistanceFlat)
00033 {
00034 GridMap map({"layer"});
00035 map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
00036 map["layer"].setConstant(1.0);
00037 map.at("layer", Index(0,0)) = -1.0;
00038
00039 SignedDistanceField sdf;
00040 sdf.calculateSignedDistanceField(map, "layer", 2.5);
00041 Position pos;
00042 map.getPosition(Index(9, 9), pos);
00043 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
00044 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.1)), -0.9, 0.0001);
00045 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.2)), -0.8, 0.0001);
00046 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.3)), -0.7, 0.0001);
00047 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.4)), -0.6, 0.0001);
00048 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -0.5, 0.0001);
00049 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.6)), -0.4, 0.0001);
00050 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.7)), -0.3, 0.0001);
00051 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.8)), -0.2, 0.0001);
00052 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.9)), -0.1, 0.0001);
00053 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
00054 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
00055 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.2, 0.0001);
00056 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.3, 0.0001);
00057 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), 0.4, 0.0001);
00058 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.5, 0.0001);
00059 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), 0.6, 0.0001);
00060 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.7, 0.0001);
00061 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.8, 0.0001);
00062 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.9, 0.0001);
00063 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 1.0, 0.0001);
00064 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 1.1, 0.0001);
00065 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 1.2, 0.0001);
00066 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 1.3, 0.0001);
00067 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 1.4, 0.0001);
00068 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 1.5, 0.0001);
00069 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 2.5, 0.0001);
00070 }
00071
00072
00073 TEST(SignedDistanceField, GetDistance)
00074 {
00075 GridMap map({"layer"});
00076 map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
00077 map["layer"].setConstant(1.0);
00078
00079 map.at("layer", Index(3,4)) = 2.0;
00080 map.at("layer", Index(3,5)) = 2.0;
00081 map.at("layer", Index(3,6)) = 2.0;
00082 map.at("layer", Index(4,4)) = 2.0;
00083 map.at("layer", Index(4,5)) = 2.0;
00084 map.at("layer", Index(4,6)) = 2.0;
00085 map.at("layer", Index(5,4)) = 2.0;
00086 map.at("layer", Index(5,5)) = 2.0;
00087 map.at("layer", Index(5,6)) = 2.0;
00088 map.at("layer", Index(6,4)) = 2.0;
00089 map.at("layer", Index(6,5)) = 2.0;
00090 map.at("layer", Index(6,6)) = 2.0;
00091 map.at("layer", Index(7,4)) = 2.0;
00092 map.at("layer", Index(7,5)) = 2.0;
00093 map.at("layer", Index(7,6)) = 2.0;
00094
00095 SignedDistanceField sdf;
00096 sdf.calculateSignedDistanceField(map, "layer", 1.5);
00097 Position pos;
00098
00099 map.getPosition(Index(5, 5), pos);
00100 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
00101 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -1.0, 0.0001);
00102 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), -1.0, 0.0001);
00103 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), -0.1, 0.0001);
00104 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), -0.1, 0.0001);
00105 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), -0.1, 0.0001);
00106 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), -0.1, 0.0001);
00107 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), -0.1, 0.0001);
00108 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), -0.1, 0.0001);
00109 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), -0.1, 0.0001);
00110 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.1, 0.0001);
00111 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.1, 0.0001);
00112 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.0, 0.0001);
00113 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.1, 0.0001);
00114 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.2, 0.0001);
00115 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.3, 0.0001);
00116 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.4, 0.0001);
00117 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.5, 0.0001);
00118 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 1.5, 0.0001);
00119
00120 map.getPosition(Index(5, 2), pos);
00121 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), 0.0, 0.0001);
00122 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), 0.0, 0.0001);
00123 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
00124 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
00125 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.1, 0.0001);
00126 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.1, 0.0001);
00127 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), 0.1, 0.0001);
00128 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.1, 0.0001);
00129 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), 0.1, 0.0001);
00130 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.1, 0.0001);
00131 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.1, 0.0001);
00132 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.1, 0.0001);
00133 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.2, 0.0001);
00134 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.3, 0.0001);
00135 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.4, 0.0001);
00136 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.5, 0.0001);
00137 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.6, 0.0001);
00138 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.7, 0.0001);
00139 EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 1.7, 0.0001);
00140 }
00141
00142 TEST(SignedDistanceField, GetDistanceGradient)
00143 {
00144 GridMap map({"layer"});
00145 map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
00146 map["layer"].setConstant(1.0);
00147
00148 map.at("layer", Index(3,4)) = 2.0;
00149 map.at("layer", Index(3,5)) = 2.0;
00150 map.at("layer", Index(3,6)) = 2.0;
00151 map.at("layer", Index(4,4)) = 2.0;
00152 map.at("layer", Index(4,5)) = 2.0;
00153 map.at("layer", Index(4,6)) = 2.0;
00154 map.at("layer", Index(5,4)) = 2.0;
00155 map.at("layer", Index(5,5)) = 2.0;
00156 map.at("layer", Index(5,6)) = 2.0;
00157 map.at("layer", Index(6,4)) = 2.0;
00158 map.at("layer", Index(6,5)) = 2.0;
00159 map.at("layer", Index(6,6)) = 2.0;
00160 map.at("layer", Index(7,4)) = 2.0;
00161 map.at("layer", Index(7,5)) = 2.0;
00162 map.at("layer", Index(7,6)) = 2.0;
00163
00164 SignedDistanceField sdf;
00165 sdf.calculateSignedDistanceField(map, "layer", 1.5);
00166 Position pos;
00167 Vector3 gradient;
00168
00169 map.getPosition(Index(5, 6), pos);
00170 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 0.0));
00171 EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00172 EXPECT_NEAR(gradient.y(), -1, 0.0001);
00173 EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
00174 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
00175 EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00176 EXPECT_NEAR(gradient.y(), -1, 0.0001);
00177 EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
00178 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.0));
00179 EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00180 EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
00181 EXPECT_NEAR(gradient.z(), 1, 0.0001);
00182 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.2));
00183 EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00184 EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
00185 EXPECT_NEAR(gradient.z(), 1.0, 0.0001);
00186 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 10.0));
00187 EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00188 EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
00189 EXPECT_NEAR(gradient.z(), 1.0, 0.0001);
00190 map.getPosition(Index(2, 2), pos);
00191 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
00192 EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00193 EXPECT_NEAR(gradient.y(), 1, 0.0001);
00194 EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
00195 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.0));
00196 EXPECT_NEAR(gradient.x(), 0.207107, 0.0001);
00197 EXPECT_NEAR(gradient.y(), 1.5, 0.0001);
00198 EXPECT_NEAR(gradient.z(), 1, 0.0001);
00199 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.2));
00200 EXPECT_NEAR(gradient.x(), 0.207107, 0.0001);
00201 EXPECT_NEAR(gradient.y(), 1.5, 0.0001);
00202 EXPECT_NEAR(gradient.z(), 1, 0.0001);
00203 map.getPosition(Index(12, 22), pos);
00204 gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
00205 EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
00206 EXPECT_NEAR(gradient.y(), 1.0, 0.0001);
00207 EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
00208 }
00209
00210 TEST(SignedDistanceField, GetInterpolatedDistance)
00211 {
00212 GridMap map({"layer"});
00213 map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
00214 map["layer"].setConstant(1.0);
00215
00216 map.at("layer", Index(3,3)) = 2.0;
00217 map.at("layer", Index(3,4)) = 2.0;
00218 map.at("layer", Index(3,5)) = 2.0;
00219 map.at("layer", Index(3,6)) = 2.0;
00220 map.at("layer", Index(3,7)) = 2.0;
00221 map.at("layer", Index(4,3)) = 2.0;
00222 map.at("layer", Index(4,4)) = 2.0;
00223 map.at("layer", Index(4,5)) = 2.0;
00224 map.at("layer", Index(4,6)) = 2.0;
00225 map.at("layer", Index(4,7)) = 2.0;
00226 map.at("layer", Index(5,3)) = 2.0;
00227 map.at("layer", Index(5,4)) = 2.0;
00228 map.at("layer", Index(5,5)) = 2.0;
00229 map.at("layer", Index(5,6)) = 2.0;
00230 map.at("layer", Index(5,7)) = 2.0;
00231 map.at("layer", Index(6,3)) = 2.0;
00232 map.at("layer", Index(6,4)) = 2.0;
00233 map.at("layer", Index(6,5)) = 2.0;
00234 map.at("layer", Index(6,6)) = 2.0;
00235 map.at("layer", Index(6,7)) = 2.0;
00236 map.at("layer", Index(7,3)) = 2.0;
00237 map.at("layer", Index(7,4)) = 2.0;
00238 map.at("layer", Index(7,5)) = 2.0;
00239 map.at("layer", Index(7,6)) = 2.0;
00240 map.at("layer", Index(7,7)) = 2.0;
00241
00242 SignedDistanceField sdf;
00243 sdf.calculateSignedDistanceField(map, "layer", 1.5);
00244 Position pos;
00245
00246 map.getPosition(Index(5, 5), pos);
00247 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -5.05, 0.0001);
00248 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -3.05, 0.0001);
00249 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), -1.05, 0.0001);
00250 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), -0.25, 0.0001);
00251 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), -0.25, 0.0001);
00252 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), -0.25, 0.0001);
00253 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), -0.25, 0.0001);
00254 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), -0.25, 0.0001);
00255 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), -0.25, 0.0001);
00256 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), -0.25, 0.0001);
00257 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.25, 0.0001);
00258 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), -0.1, 0.0001);
00259 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.0, 0.0001);
00260 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.1, 0.0001);
00261 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.2, 0.0001);
00262 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.3, 0.0001);
00263 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.4, 0.0001);
00264 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.5, 0.0001);
00265 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 8, 0.0001);
00266
00267 map.getPosition(Index(5, 10), pos);
00268 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
00269 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -0.5, 0.0001);
00270 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
00271 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
00272 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.2, 0.0001);
00273 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.3, 0.0001);
00274 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.35, 0.0001);
00275 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.35, 0.0001);
00276 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.35, 0.0001);
00277 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.45, 0.0001);
00278 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.55, 0.0001);
00279 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.65, 0.0001);
00280 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.75, 0.0001);
00281 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.85, 0.0001);
00282 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.95, 0.0001);
00283 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 8.45, 0.0001);
00284
00285 map.getPosition(Index(5, 0), pos);
00286 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.25, 0.0001);
00287 map.getPosition(Index(5, 1), pos);
00288 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.175, 0.0001);
00289 map.getPosition(Index(5, 2), pos);
00290 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.025, 0.0001);
00291 map.getPosition(Index(5, 3), pos);
00292 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.2, 0.0001);
00293 map.getPosition(Index(5, 4), pos);
00294 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.225, 0.0001);
00295 map.getPosition(Index(5, 5), pos);
00296 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.25, 0.0001);
00297 map.getPosition(Index(5, 6), pos);
00298 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.175, 0.0001);
00299 map.getPosition(Index(5, 7), pos);
00300 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.025, 0.0001);
00301 map.getPosition(Index(5, 8), pos);
00302 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.15, 0.0001);
00303 map.getPosition(Index(5, 9), pos);
00304 EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.25, 0.0001);
00305 }