Main MRPT website > C++ reference for MRPT 1.9.9
CGeneralizedEllipsoidTemplate.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-2018, 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 "opengl-precomp.h" // Precompiled header
11 
13 #include <mrpt/opengl/gl_utils.h>
14 #include "opengl_internals.h"
15 
16 using namespace mrpt;
17 using namespace mrpt::opengl;
18 
19 using namespace mrpt::math;
20 using namespace std;
21 
22 /*---------------------------------------------------------------
23  Render: 2D implementation
24  ---------------------------------------------------------------*/
25 namespace mrpt
26 {
27 namespace opengl
28 {
29 namespace detail
30 {
31 template <>
33  const std::vector<mrpt::math::CMatrixFixedNumeric<float, 2, 1>>& pts,
34  const float lineWidth, const uint32_t slices, const uint32_t stacks)
35 {
36 #if MRPT_HAS_OPENGL_GLUT
41  glLineWidth(lineWidth);
43 
44  glDisable(GL_LIGHTING); // Disable lights when drawing lines
45 
47  const size_t N = pts.size();
48  for (size_t i = 0; i < N; i++) glVertex2f(pts[i][0], pts[i][1]);
49 
50  glEnd();
51 
54 #else
55  MRPT_UNUSED_PARAM(pts);
56  MRPT_UNUSED_PARAM(lineWidth);
57  MRPT_UNUSED_PARAM(slices);
58  MRPT_UNUSED_PARAM(stacks);
59 #endif
60 }
61 
62 /*---------------------------------------------------------------
63  Render: 3D implementation
64  ---------------------------------------------------------------*/
65 template <>
67  const std::vector<mrpt::math::CMatrixFixedNumeric<float, 3, 1>>& pts,
68  const float lineWidth, const uint32_t slices, const uint32_t stacks)
69 {
70 #if MRPT_HAS_OPENGL_GLUT
75  glLineWidth(lineWidth);
77  glDisable(GL_LIGHTING); // Disable lights when drawing lines
78 
79  // Points in the ellipsoid:
80  // * "#slices" slices, with "#stacks" points each, but for the two ends
81  // * 1 point at each end slice
82  // #total points = stacks*(slices-2) + 2
83  ASSERT_EQUAL_((slices - 2) * stacks + 2, pts.size());
84  const size_t idx_1st_slice = 1;
85 
86  // 1st slice: triangle fan (if it were solid)
87  // ----------------------------
89  for (size_t i = 0; i < stacks; i++)
90  {
91  glVertex3fv(&pts[0][0]);
92  glVertex3fv(&pts[idx_1st_slice + i][0]);
93  }
94  glEnd();
95 
96  // Middle slices: triangle strip (if it were solid)
97  // ----------------------------
98  for (size_t s = 0; s < slices - 3; s++)
99  {
100  size_t idx_this_slice = idx_1st_slice + stacks * s;
101  size_t idx_next_slice = idx_this_slice + stacks;
102 
103  for (size_t i = 0; i < stacks; i++)
104  {
105  const size_t ii =
106  (i == (stacks - 1) ? 0 : i + 1); // next i with wrapping
107 
109  glVertex3fv(&pts[idx_this_slice + i][0]);
110  glVertex3fv(&pts[idx_next_slice + ii][0]);
111  glVertex3fv(&pts[idx_next_slice + i][0]);
112  glVertex3fv(&pts[idx_this_slice + i][0]);
113  glVertex3fv(&pts[idx_this_slice + ii][0]);
114  glVertex3fv(&pts[idx_next_slice + ii][0]);
115  glEnd();
116  }
117  }
118 
119  // Last slice: triangle fan (if it were solid)
120  // ----------------------------
121  const size_t idx_last_pt = pts.size() - 1;
122  const size_t idx_last_slice = idx_1st_slice + (slices - 3) * stacks;
123  glBegin(GL_LINES);
124  for (size_t i = 0; i < stacks; i++)
125  {
126  glVertex3fv(&pts[idx_last_pt][0]);
127  glVertex3fv(&pts[idx_last_slice + i][0]);
128  }
129  glEnd();
130 
131  // glBegin( GL_POINTS );
132  // const size_t N = pts.size();
133  // for (size_t i=0;i<N;i++)
134  // glVertex3f( pts[i][0], pts[i][1], pts[i][2] );
135  // glEnd();
136 
139 #else
140  MRPT_UNUSED_PARAM(pts);
141  MRPT_UNUSED_PARAM(lineWidth);
142  MRPT_UNUSED_PARAM(slices);
143  MRPT_UNUSED_PARAM(stacks);
144 #endif
145 }
146 
147 /*---------------------------------------------------------------
148  generalizedEllipsoidPoints: 2D
149  ---------------------------------------------------------------*/
150 template <>
154  std::vector<mrpt::math::CMatrixFixedNumeric<float, 2, 1>>& out_params_pts,
155  const uint32_t numSegments, const uint32_t numSegments_unused)
156 {
157  MRPT_UNUSED_PARAM(numSegments_unused);
158  out_params_pts.clear();
159  out_params_pts.reserve(numSegments);
160  const double Aa = 2 * M_PI / numSegments;
161  for (double ang = 0; ang < 2 * M_PI; ang += Aa)
162  {
163  const double ccos = cos(ang);
164  const double ssin = sin(ang);
165 
166  out_params_pts.resize(out_params_pts.size() + 1);
167 
168  Eigen::Matrix<float, 2, 1>& pt = out_params_pts.back();
169 
170  pt[0] = mean[0] + ccos * U.get_unsafe(0, 0) + ssin * U.get_unsafe(0, 1);
171  pt[1] = mean[1] + ccos * U.get_unsafe(1, 0) + ssin * U.get_unsafe(1, 1);
172  }
173 }
174 
176  const double x, const double y, const double z,
180 {
181  pts.resize(pts.size() + 1);
183  pt[0] = mean[0] + x * M.get_unsafe(0, 0) + y * M.get_unsafe(0, 1) +
184  z * M.get_unsafe(0, 2);
185  pt[1] = mean[1] + x * M.get_unsafe(1, 0) + y * M.get_unsafe(1, 1) +
186  z * M.get_unsafe(1, 2);
187  pt[2] = mean[2] + x * M.get_unsafe(2, 0) + y * M.get_unsafe(2, 1) +
188  z * M.get_unsafe(2, 2);
189 }
190 
191 /*---------------------------------------------------------------
192  generalizedEllipsoidPoints: 3D
193  ---------------------------------------------------------------*/
194 template <>
198  std::vector<mrpt::math::CMatrixFixedNumeric<float, 3, 1>>& pts,
199  const uint32_t slices, const uint32_t stacks)
200 {
201  MRPT_START
202  ASSERT_ABOVEEQ_(slices, 3);
203  ASSERT_ABOVEEQ_(stacks, 3);
204  // sin/cos cache --------
205  // Slices: [0,pi]
206  std::vector<double> slice_cos(slices), slice_sin(slices);
207  for (uint32_t i = 0; i < slices; i++)
208  {
209  double angle = M_PI * i / double(slices - 1);
210  slice_sin[i] = sin(angle);
211  slice_cos[i] = cos(angle);
212  }
213  // Stacks: [0,2*pi]
214  std::vector<double> stack_sin(stacks), stack_cos(stacks);
215  for (uint32_t i = 0; i < stacks; i++)
216  {
217  double angle = 2 * M_PI * i / double(stacks);
218  stack_sin[i] = sin(angle);
219  stack_cos[i] = cos(angle);
220  }
221 
222  // Points in the ellipsoid:
223  // * "#slices" slices, with "#stacks" points each, but for the two ends
224  // * 1 point at each end slice
225  // #total points = stacks*(slices-2) + 2
226  pts.clear();
227  pts.reserve((slices - 2) * stacks + 2);
228 
229  for (uint32_t i = 0; i < slices; i++)
230  {
231  if (i == 0)
232  aux_add3DpointWithEigenVectors(1, 0, 0, pts, U, mean);
233  else if (i == (slices - 1))
234  aux_add3DpointWithEigenVectors(-1, 0, 0, pts, U, mean);
235  else
236  {
237  const double x = slice_cos[i];
238  const double R = slice_sin[i];
239 
240  for (uint32_t j = 0; j < stacks; j++)
241  {
242  const double y = R * stack_cos[j];
243  const double z = R * stack_sin[j];
245  }
246  }
247  }
248 
249  MRPT_END
250 }
251 }
252 }
253 } // end namespaces
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
GLAPI void GLAPIENTRY glVertex3fv(const GLfloat *v)
GLAPI void GLAPIENTRY glEnable(GLenum cap)
STL namespace.
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:287
GLdouble s
Definition: glext.h:3676
#define GL_LIGHTING
Definition: glew.h:385
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
A numeric matrix of compile-time fixed size.
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:153
void aux_add3DpointWithEigenVectors(const double x, const double y, const double z, std::vector< mrpt::math::CMatrixFixedNumeric< float, 3, 1 >> &pts, const mrpt::math::CMatrixFixedNumeric< double, 3, 3 > &M, const mrpt::math::CMatrixFixedNumeric< double, 3, 1 > &mean)
GLAPI void GLAPIENTRY glBegin(GLenum mode)
void renderGeneralizedEllipsoidTemplate< 3 >(const std::vector< mrpt::math::CMatrixFixedNumeric< float, 3, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
#define GL_BLEND
Definition: glew.h:432
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
#define GL_LINE_LOOP
Definition: glew.h:274
#define GL_SRC_ALPHA
Definition: glew.h:286
void renderGeneralizedEllipsoidTemplate< 2 >(const std::vector< mrpt::math::CMatrixFixedNumeric< float, 2, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:143
#define MRPT_END
Definition: exceptions.h:266
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLAPI void GLAPIENTRY glEnd(void)
GLenum GLint GLint y
Definition: glext.h:3538
#define GL_LINE_STRIP
Definition: glew.h:275
#define GL_LINES
Definition: glew.h:273
void generalizedEllipsoidPoints< 2 >(const mrpt::math::CMatrixFixedNumeric< double, 2, 2 > &U, const mrpt::math::CMatrixFixedNumeric< double, 2, 1 > &mean, std::vector< mrpt::math::CMatrixFixedNumeric< float, 2, 1 >> &out_params_pts, const uint32_t slices, const uint32_t stacks)
void generalizedEllipsoidPoints< 3 >(const mrpt::math::CMatrixFixedNumeric< double, 3, 3 > &U, const mrpt::math::CMatrixFixedNumeric< double, 3, 1 > &mean, std::vector< mrpt::math::CMatrixFixedNumeric< float, 3, 1 >> &out_params_pts, const uint32_t slices, const uint32_t stacks)
GLenum GLint x
Definition: glext.h:3538
unsigned __int32 uint32_t
Definition: rptypes.h:47
GLAPI void GLAPIENTRY glVertex2f(GLfloat x, GLfloat y)
GLAPI void GLAPIENTRY glDisable(GLenum cap)
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019