MRPT  1.9.9
CAtan2LookUpTable.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
13 #include <cmath>
14 #include <vector>
15 
16 using namespace mrpt::math;
17 
19 {
20  this->resize(-1.0, 1.0, -1.0, 1.0, 0.5);
21 }
23  double xmin, double xmax, double ymin, double ymax,
24  double resolution) noexcept
25 {
26  this->resize(xmin, xmax, ymin, ymax, resolution);
27 }
28 
30  double xmin, double xmax, double ymin, double ymax,
31  double resolution) noexcept
32 {
33  const double def = .0;
34  if (m_grid.getResolution() == resolution)
35  m_grid.resize(xmin, xmax, ymin, ymax, def, .0);
36  else
37  m_grid.setSize(xmin, xmax, ymin, ymax, resolution, &def);
38 
39  const size_t nx = m_grid.getSizeX(), ny = m_grid.getSizeY();
40 
41  std::vector<double> idx2x(nx), idx2y(ny);
42 
43  for (size_t ix = 0; ix < nx; ix++) idx2x[ix] = m_grid.idx2x(ix);
44  for (size_t iy = 0; iy < ny; iy++) idx2y[iy] = m_grid.idx2y(iy);
45 
46  for (size_t ix = 0; ix < nx; ix++)
47  {
48  const double x = idx2x[ix];
49  for (size_t iy = 0; iy < ny; iy++)
50  {
51  const double y = idx2y[iy];
52  double* cp = m_grid.cellByIndex(ix, iy);
53  if (cp) *cp = ::atan2(y, x);
54  }
55  }
56 }
57 
59  const std::map<double, double>& lst_resolutions2extensions) noexcept
60 {
61  resize(lst_resolutions2extensions);
62 }
64  const std::map<double, double>& lst_resolutions2extensions) noexcept
65 {
66  m_grids.clear();
67 
68  for (const auto& it : lst_resolutions2extensions)
69  {
70  const double res = it.first;
71  const double exten = it.second;
72 
73  mrpt::containers::CDynamicGrid<double>& dg = m_grids[exten] =
75  -exten, exten, -exten, exten, res);
76 
77  const size_t nx = dg.getSizeX(), ny = dg.getSizeY();
78  std::vector<double> idx2x(nx), idx2y(ny);
79 
80  for (size_t ix = 0; ix < nx; ix++) idx2x[ix] = dg.idx2x(ix);
81  for (size_t iy = 0; iy < ny; iy++) idx2y[iy] = dg.idx2y(iy);
82 
83  for (size_t ix = 0; ix < nx; ix++)
84  {
85  const double x = idx2x[ix];
86  for (size_t iy = 0; iy < ny; iy++)
87  {
88  const double y = idx2y[iy];
89  double* cp = dg.cellByIndex(ix, iy);
90  if (cp) *cp = ::atan2(y, x);
91  }
92  }
93  }
94 }
95 
97  double y, double x, double& out_atan2) const noexcept
98 {
99  for (const auto& it : m_grids)
100  {
101  const double exten = it.first;
102  const mrpt::containers::CDynamicGrid<double>& dg = it.second;
103 
104  if (std::abs(x) > exten || std::abs(y) > exten) continue;
105 
106  const double* cp = dg.cellByPos(x, y);
107  if (!cp) continue;
108  out_atan2 = *cp;
109  return true;
110  }
111  return false;
112 }
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:279
void resize(const std::map< double, double > &lst_resolutions2extensions) noexcept
See CAtan2LookUpTableMultiRes for a discussion of the parameters.
double idx2y(int cy) const
Definition: CDynamicGrid.h:283
This base provides a set of functions for maths stuff.
T * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or nullptr if it is out of the ...
Definition: CDynamicGrid.h:201
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:222
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:242
bool atan2(double y, double x, double &out_atan2) const noexcept
Returns the precomputed value for atan2(y,x).
void resize(double xmin, double xmax, double ymin, double ymax, double resolution) noexcept
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:244
images resize(NUM_IMGS)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020