Main MRPT website > C++ reference for MRPT 1.9.9
CAtan2LookUpTable.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
13 #include <vector>
14 #include <cmath>
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 
58 bool CAtan2LookUpTable::atan2(double y, double x, double& out_atan2) const
59  noexcept
60 {
61  const double* cp = m_grid.cellByPos(x, y);
62  if (!cp) return false;
63  out_atan2 = *cp;
64  return true;
65 }
66 
69  const std::map<double, double>& lst_resolutions2extensions) noexcept
70 {
71  resize(lst_resolutions2extensions);
72 }
74  const std::map<double, double>& lst_resolutions2extensions) noexcept
75 {
76  m_grids.clear();
77 
78  for (const auto& it : lst_resolutions2extensions)
79  {
80  const double res = it.first;
81  const double exten = it.second;
82 
83  mrpt::utils::CDynamicGrid<double>& dg = m_grids[exten] =
85  -exten, exten, -exten, exten, res);
86 
87  const size_t nx = dg.getSizeX(), ny = dg.getSizeY();
88  std::vector<double> idx2x(nx), idx2y(ny);
89 
90  for (size_t ix = 0; ix < nx; ix++) idx2x[ix] = dg.idx2x(ix);
91  for (size_t iy = 0; iy < ny; iy++) idx2y[iy] = dg.idx2y(iy);
92 
93  for (size_t ix = 0; ix < nx; ix++)
94  {
95  const double x = idx2x[ix];
96  for (size_t iy = 0; iy < ny; iy++)
97  {
98  const double y = idx2y[iy];
99  double* cp = dg.cellByIndex(ix, iy);
100  if (cp) *cp = ::atan2(y, x);
101  }
102  }
103  }
104 }
105 
107  double y, double x, double& out_atan2) const noexcept
108 {
109  for (const auto& it : m_grids)
110  {
111  const double exten = it.first;
112  const mrpt::utils::CDynamicGrid<double>& dg = it.second;
113 
114  if (std::abs(x) > exten || std::abs(y) > exten) continue;
115 
116  const double* cp = dg.cellByPos(x, y);
117  if (!cp) continue;
118  out_atan2 = *cp;
119  return true;
120  }
121  return false;
122 }
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:256
void resize(const std::map< double, double > &lst_resolutions2extensions) noexcept
See CAtan2LookUpTableMultiRes for a discussion of the parameters.
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:213
double idx2y(int cy) const
Definition: CDynamicGrid.h:295
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:254
bool atan2(double y, double x, double &out_atan2) const noexcept
Returns the precomputed value for atan2(y,x).
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:234
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:291
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
GLfloat GLfloat GLfloat GLfloat nx
Definition: glext.h:6296
GLenum GLint GLint y
Definition: glext.h:3538
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
GLfloat ny
Definition: glext.h:6292



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019