34 #include <gtest/gtest.h> 39 TEST(VectorNavGrid, info_equality)
62 EXPECT_EQ(info0, info0);
63 EXPECT_EQ(info0, info1);
64 EXPECT_NE(info0, width_info);
65 EXPECT_NE(info0, height_info);
66 EXPECT_NE(info0, res_info);
67 EXPECT_NE(info0, frame_info);
68 EXPECT_NE(info0, originx_info);
69 EXPECT_NE(info0, originy_info);
72 TEST(VectorNavGrid, basic_test)
79 EXPECT_EQ(grid(0, 0), -3);
81 EXPECT_EQ(grid(0, 0), -3);
82 EXPECT_EQ(grid(1, 1), 10);
85 TEST(VectorNavGrid, basic_index_test)
94 EXPECT_EQ(grid(index0), -3);
96 EXPECT_EQ(grid(index0), -3);
97 EXPECT_EQ(grid(index1), 10);
98 EXPECT_EQ(grid(0, 0), -3);
99 EXPECT_EQ(grid(1, 1), 10);
102 TEST(VectorNavGrid, easy_coordinates_test)
110 EXPECT_DOUBLE_EQ(wx, 0.5);
111 EXPECT_DOUBLE_EQ(wy, 0.5);
113 EXPECT_DOUBLE_EQ(wx, 1.5);
114 EXPECT_DOUBLE_EQ(wy, 2.5);
116 unsigned int umx, umy;
126 EXPECT_DOUBLE_EQ(dmx, wx);
127 EXPECT_DOUBLE_EQ(dmy, wy);
159 TEST(VectorNavGrid, hard_coordinates_test)
170 EXPECT_DOUBLE_EQ(wx, -0.15);
171 EXPECT_DOUBLE_EQ(wy, 0.25);
173 EXPECT_DOUBLE_EQ(wx, -0.05);
174 EXPECT_DOUBLE_EQ(wy, 0.45);
176 unsigned int umx, umy;
186 EXPECT_DOUBLE_EQ(dmx, 1.5);
187 EXPECT_DOUBLE_EQ(dmy, 2.5);
200 TEST(VectorNavGrid, speed_test)
205 const int EXTRA = 300;
211 unsigned int umx, umy;
215 for (
int x = -EXTRA; x < N + EXTRA; x++)
217 for (
int y = -EXTRA; y < N + EXTRA; y++)
220 if (x < 0 || y < 0 || x >= N || y >= N)
224 EXPECT_EQ(umx, std::min(std::max(0, x), N - 1));
225 EXPECT_EQ(umy, std::min(std::max(0, y), N - 1));
238 EXPECT_DOUBLE_EQ(dmx, x + 0.5);
239 EXPECT_DOUBLE_EQ(dmy, y + 0.5);
246 return static_cast<int>(100 * floor(x) + floor(y));
269 for (
unsigned int j = 0; j < info.
height; j++)
271 for (
unsigned int i = 0; i < info.
width; i++)
283 unsigned int x0,
unsigned int x1,
unsigned int y0,
unsigned int y1)
285 for (
unsigned int x = 0; x < grid.
getWidth(); x++)
287 for (
unsigned int y = 0; y < grid.
getHeight(); y++)
289 if (x >= x0 && x < x1 && y >= y0 && y < y1)
295 EXPECT_EQ(grid(x, y), -10);
305 unsigned int x0,
unsigned int x1,
unsigned int y0,
unsigned int y1)
308 for (
unsigned int x = 0; x < grid.
getWidth(); x++)
310 for (
unsigned int y = 0; y < grid.
getHeight(); y++)
312 if (x >= x0 && x < x1 && y >= y0 && y < y1)
319 EXPECT_EQ(grid(x, y), -10);
327 for (
unsigned int j = 0; j < grid.
getHeight(); j++)
329 for (
unsigned int i = 0; i < grid.
getWidth(); i++)
331 printf(
"%d ", grid(i, j));
338 TEST(VectorNavGrid, resizing_grid_with_set)
345 decreased_width_info.width = 5;
346 grid.
setInfo(decreased_width_info);
350 increased_width_info.width = 9;
351 grid.
setInfo(increased_width_info);
358 increased_height_info.height = 9;
359 grid.
setInfo(increased_height_info);
363 decreased_height_info.height = 4;
364 grid.
setInfo(decreased_height_info);
368 TEST(VectorNavGrid, resizing_grid_with_update)
375 decreased_width_info.width = 5;
380 increased_width_info.width = 9;
388 increased_height_info.height = 9;
393 decreased_height_info.height = 4;
398 TEST(VectorNavGrid, change_origin)
409 bump_up_info.origin_y = 2;
419 bump_down_info.origin_y = 0;
431 TEST(VectorNavGrid, combined_changes)
440 info1.origin_x = -5.0;
471 for (
unsigned int x0 = 0; x0 < N; ++x0)
473 for (
unsigned int y0 = 0; y0 < N; ++y0)
477 for (
unsigned int x1 = 0; x1 < N; ++x1)
479 for (
unsigned int y1 = 0; y1 < N; ++y1)
484 if (x0 == x1 && y0 == y1)
486 EXPECT_EQ(index0, index1);
487 EXPECT_TRUE(!(index0 < index1) && !(index1 < index0));
488 EXPECT_GE(index0, index1);
489 EXPECT_LE(index0, index1);
490 EXPECT_GE(index1, index0);
491 EXPECT_LE(index1, index0);
495 EXPECT_NE(index0, index1);
496 EXPECT_FALSE(!(index0 < index1) && !(index1 < index0));
497 if (x0 < x1 || (x0 == x1 && y0 < y1))
499 EXPECT_LT(index0, index1);
500 EXPECT_GT(index1, index0);
501 EXPECT_LE(index0, index1);
502 EXPECT_GE(index1, index0);
506 EXPECT_GT(index0, index1);
507 EXPECT_LT(index1, index0);
508 EXPECT_GE(index0, index1);
509 EXPECT_LE(index1, index0);
518 int main(
int argc,
char **argv)
520 testing::InitGoogleTest(&argc, argv);
521 return RUN_ALL_TESTS();
void debugGridValues(const nav_grid::VectorNavGrid< int > &grid)
double origin_x
The origin defines the coordinates of minimum corner of cell (0,0) in the grid.
void gridToWorld(const NavGridInfo &info, int mx, int my, double &wx, double &wy)
Convert from grid coordinates to world coordinates of the center of the cell.
void setDefaultValue(const T new_value)
Set the default value.
GenericIndex< unsigned int > Index
bool isWithinGrid(const NavGridInfo &info, double wx, double wy)
Check to see if the world coordinates are within the grid.
void checkSetGridValues(const nav_grid::VectorNavGrid< int > &grid, unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
int main(int argc, char **argv)
void initializeTestGrid(nav_grid::VectorNavGrid< int > &grid)
void setInfo(const NavGridInfo &new_info) override
Change the info while attempting to keep the values associated with the grid coordinates.
unsigned int getHeight() const
void setValue(const unsigned int x, const unsigned int y, const T &value) override
set the value of the grid at (x,y)
void updateInfo(const NavGridInfo &new_info) override
Update the info while keeping the data geometrically in tact.
unsigned int getWidth() const
NavGridInfo getInfo() const
void worldToGrid(const NavGridInfo &info, double wx, double wy, double &mx, double &my)
Convert from world coordinates to the precise (double) grid coordinates.
A simple pair of x/y coordinates.
int testGridValue(double x, double y)
TEST(VectorNavGrid, info_equality)
void checkUpdateGridValues(const nav_grid::VectorNavGrid< int > &grid, unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)
Convert from world coordinates to grid coordinates.