MRPT  2.0.5
CGeneralizedEllipsoidTemplate.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 "opengl-precomp.h" // Precompiled header
11 
13 #include <mrpt/opengl/opengl_api.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::opengl;
17 using namespace mrpt::math;
18 using namespace std;
19 
20 template <>
22 {
23  const auto& pts = m_render_pts;
24 
27  vbd.clear();
28 
29  // Line loop:
30  const auto N = pts.size();
31  for (size_t i = 0; i < N; i++)
32  {
33  const auto ip = (i + 1) % N;
34  vbd.emplace_back(pts[i][0], pts[i][1], .0f);
35  vbd.emplace_back(pts[ip][0], pts[ip][1], .0f);
36  }
37 
38  // All lines, same color:
39  cbd.assign(vbd.size(), m_color);
40 }
41 
42 template <>
44 {
45  const auto& pts = m_render_pts;
46 
49  vbd.clear();
50 
51  const auto slices = m_numSegments, stacks = m_numSegments;
52 
53  // Points in the ellipsoid:
54  // * "#slices" slices, with "#stacks" points each, but for the two ends
55  // * 1 point at each end slice
56  // #total points = stacks*(slices-2) + 2
57  ASSERT_EQUAL_((slices - 2) * stacks + 2, pts.size());
58  const size_t idx_1st_slice = 1;
59 
60  // 1st slice: triangle fan (if it were solid)
61  // ----------------------------
62  for (size_t i = 0; i < stacks; i++)
63  {
64  const auto idx = idx_1st_slice + i;
65  vbd.emplace_back(pts[0]);
66  vbd.emplace_back(pts[idx]);
67  }
68 
69  // Middle slices: triangle strip (if it were solid)
70  // ----------------------------
71  for (size_t s = 0; s < slices - 3; s++)
72  {
73  size_t idx0 = idx_1st_slice + stacks * s;
74  size_t idx1 = idx0 + stacks;
75 
76  for (size_t i = 0; i < stacks; i++)
77  {
78  const size_t ii =
79  (i == (stacks - 1) ? 0 : i + 1); // next i with wrapping
80 
81  vbd.emplace_back(pts[idx0 + i]);
82  vbd.emplace_back(pts[idx1 + ii]);
83 
84  vbd.emplace_back(pts[idx1 + ii]);
85  vbd.emplace_back(pts[idx1 + i]);
86 
87  vbd.emplace_back(pts[idx1 + i]);
88  vbd.emplace_back(pts[idx0 + i]);
89 
90  vbd.emplace_back(pts[idx0 + i]);
91  vbd.emplace_back(pts[idx0 + ii]);
92 
93  vbd.emplace_back(pts[idx0 + ii]);
94  vbd.emplace_back(pts[idx1 + ii]);
95  }
96  }
97 
98  // Last slice: triangle fan (if it were solid)
99  // ----------------------------
100  const size_t idxN = pts.size() - 1;
101  const size_t idx_last_slice = idx_1st_slice + (slices - 3) * stacks;
102 
103  for (size_t i = 0; i < stacks; i++)
104  {
105  vbd.emplace_back(pts[idxN]);
106  vbd.emplace_back(pts[idx_last_slice + i]);
107  }
108 
109  // All lines, same color:
110  cbd.assign(vbd.size(), m_color);
111 }
112 
113 template <>
115 {
116  using P3f = mrpt::math::TPoint3Df;
117  const auto& pts = m_render_pts;
118 
119  // Render precomputed points in m_render_pts:
121  tris.clear();
122 
123  const auto N = pts.size();
124  for (size_t i = 0; i < N; i++)
125  {
126  const auto ip = (i + 1) % N;
127  tris.emplace_back(
128  P3f(0, 0, 0), P3f(pts[i][0], pts[i][1], .0f),
129  P3f(pts[ip][0], pts[ip][1], .0f));
130  }
131 
132  // All faces, all vertices, same color:
133  for (auto& t : tris) t.setColor(m_color);
134 }
135 
136 template <>
138 {
139  const auto& pts = m_render_pts;
140 
141  // Render precomputed points in m_render_pts:
143  tris.clear();
144 
145  const auto slices = m_numSegments, stacks = m_numSegments;
146 
147  // Points in the ellipsoid:
148  // * "#slices" slices, with "#stacks" points each, but for the two ends
149  // * 1 point at each end slice
150  // #total points = stacks*(slices-2) + 2
151  ASSERT_EQUAL_((slices - 2) * stacks + 2, pts.size());
152  const size_t idx_1st_slice = 1;
153 
154  // 1st slice: triangle fan (if it were solid)
155  // ----------------------------
156  for (size_t i = 0; i < stacks; i++)
157  {
158  const auto idx = idx_1st_slice + i;
159  const auto idxp = idx_1st_slice + ((i + 1) % stacks);
160 
161  tris.emplace_back(
162  // Points
163  pts[0], pts[idx], pts[idxp],
164  // Normals:
165  pts[0], pts[idx], pts[idxp]);
166  }
167 
168  // Middle slices: triangle strip (if it were solid)
169  // ----------------------------
170  for (size_t s = 0; s < slices - 3; s++)
171  {
172  size_t idx0 = idx_1st_slice + stacks * s;
173  size_t idx1 = idx0 + stacks;
174 
175  for (size_t i = 0; i < stacks; i++)
176  {
177  const size_t ii =
178  (i == (stacks - 1) ? 0 : i + 1); // next i with wrapping
179 
180  tris.emplace_back(
181  // Points
182  pts[idx0 + i], pts[idx0 + ii], pts[idx1 + i],
183  // Normals:
184  pts[idx0 + i], pts[idx0 + ii], pts[idx1 + i]);
185  tris.emplace_back(
186  // Points
187  pts[idx1 + ii], pts[idx1 + i], pts[idx0 + ii],
188  // Normals:
189  pts[idx1 + ii], pts[idx1 + i], pts[idx0 + ii]);
190  }
191  }
192 
193  // Last slice: triangle fan (if it were solid)
194  // ----------------------------
195  const size_t idxN = pts.size() - 1;
196  const size_t idx_last_slice = idx_1st_slice + (slices - 3) * stacks;
197 
198  for (size_t i = 0; i < stacks; i++)
199  {
200  const auto idx = idx_last_slice + i;
201  const auto idxp = idx_last_slice + ((i + 1) % stacks);
202 
203  tris.emplace_back(
204  // Points
205  pts[idx], pts[idxN], pts[idxp],
206  // Normals
207  pts[idx], pts[idxN], pts[idxp]);
208  }
209 
210  // All faces, all vertices, same color:
211  for (auto& t : tris) t.setColor(m_color);
212 }
213 
214 template <>
218 {
219  pts.clear();
220  pts.reserve(m_numSegments);
221  const double Aa = 2 * M_PI / m_numSegments;
222  for (double ang = 0; ang < 2 * M_PI; ang += Aa)
223  {
224  const double ccos = cos(ang);
225  const double ssin = sin(ang);
226 
227  pts.resize(pts.size() + 1);
228 
229  auto& pt = pts.back();
230 
231  pt[0] = d2f(m_mean[0] + ccos * U(0, 0) + ssin * U(0, 1));
232  pt[1] = d2f(m_mean[1] + ccos * U(1, 0) + ssin * U(1, 1));
233  }
234 }
235 
237  const double x, const double y, const double z,
238  std::vector<mrpt::math::CMatrixFixed<float, 3, 1>>& pts,
241 {
242  pts.resize(pts.size() + 1);
243  mrpt::math::CMatrixFixed<float, 3, 1>& pt = pts.back();
244  pt[0] = d2f(mean[0] + x * M(0, 0) + y * M(0, 1) + z * M(0, 2));
245  pt[1] = d2f(mean[1] + x * M(1, 0) + y * M(1, 1) + z * M(1, 2));
246  pt[2] = d2f(mean[2] + x * M(2, 0) + y * M(2, 1) + z * M(2, 2));
247 }
248 
249 template <>
253 {
254  MRPT_START
255  const auto slices = m_numSegments, stacks = m_numSegments;
256  ASSERT_ABOVEEQ_(slices, 3);
257  ASSERT_ABOVEEQ_(stacks, 3);
258  // sin/cos cache --------
259  // Slices: [0,pi]
260  std::vector<double> slice_cos(slices), slice_sin(slices);
261  for (uint32_t i = 0; i < slices; i++)
262  {
263  double angle = M_PI * i / double(slices - 1);
264  slice_sin[i] = sin(angle);
265  slice_cos[i] = cos(angle);
266  }
267  // Stacks: [0,2*pi]
268  std::vector<double> stack_sin(stacks), stack_cos(stacks);
269  for (uint32_t i = 0; i < stacks; i++)
270  {
271  double angle = 2 * M_PI * i / double(stacks);
272  stack_sin[i] = sin(angle);
273  stack_cos[i] = cos(angle);
274  }
275 
276  // Points in the ellipsoid:
277  // * "#slices" slices, with "#stacks" points each, but for the two ends
278  // * 1 point at each end slice
279  // #total points = stacks*(slices-2) + 2
280  pts.clear();
281  pts.reserve((slices - 2) * stacks + 2);
282 
283  for (uint32_t i = 0; i < slices; i++)
284  {
285  if (i == 0)
286  aux_add3DpointWithEigenVectors(1, 0, 0, pts, U, m_mean);
287  else if (i == (slices - 1))
288  aux_add3DpointWithEigenVectors(-1, 0, 0, pts, U, m_mean);
289  else
290  {
291  const double x = slice_cos[i];
292  const double R = slice_sin[i];
293 
294  for (uint32_t j = 0; j < stacks; j++)
295  {
296  const double y = R * stack_cos[j];
297  const double z = R * stack_sin[j];
298  aux_add3DpointWithEigenVectors(x, y, z, pts, U, m_mean);
299  }
300  }
301  }
302 
303  MRPT_END
304 }
void generatePoints(const cov_matrix_t &U, std::vector< array_parameter_t > &out_params_pts) const
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
void aux_add3DpointWithEigenVectors(const double x, const double y, const double z, std::vector< mrpt::math::CMatrixFixed< float, 3, 1 >> &pts, const mrpt::math::CMatrixFixed< double, 3, 3 > &M, const mrpt::math::CMatrixFixed< double, 3, 1 > &mean)
STL namespace.
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
float d2f(const double d)
shortcut for static_cast<float>(double)
This base provides a set of functions for maths stuff.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
A class that generalizes the concept of an ellipsoid to arbitrary parameterizations of uncertainty sh...
std::vector< mrpt::img::TColor > m_color_buffer_data
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::vector< mrpt::opengl::TTriangle > m_triangles
List of triangles.
const float R
#define MRPT_END
Definition: exceptions.h:245
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
double mean(const CONTAINER &v)
Computes the mean value of a vector.
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: ecc95703f Thu Jul 2 07:56:41 2020 +0200 at jue jul 2 08:00:14 CEST 2020