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::opengl::detail
26 {
27 template <>
29  const std::vector<mrpt::math::CMatrixFixedNumeric<float, 2, 1>>& pts,
30  const float lineWidth, const uint32_t slices, const uint32_t stacks)
31 {
32 #if MRPT_HAS_OPENGL_GLUT
37  glLineWidth(lineWidth);
39 
40  glDisable(GL_LIGHTING); // Disable lights when drawing lines
41 
43  const size_t N = pts.size();
44  for (size_t i = 0; i < N; i++) glVertex2f(pts[i][0], pts[i][1]);
45 
46  glEnd();
47 
50 #else
51  MRPT_UNUSED_PARAM(pts);
52  MRPT_UNUSED_PARAM(lineWidth);
53  MRPT_UNUSED_PARAM(slices);
54  MRPT_UNUSED_PARAM(stacks);
55 #endif
56 }
57 
58 /*---------------------------------------------------------------
59  Render: 3D implementation
60  ---------------------------------------------------------------*/
61 template <>
63  const std::vector<mrpt::math::CMatrixFixedNumeric<float, 3, 1>>& pts,
64  const float lineWidth, const uint32_t slices, const uint32_t stacks)
65 {
66 #if MRPT_HAS_OPENGL_GLUT
71  glLineWidth(lineWidth);
73  glDisable(GL_LIGHTING); // Disable lights when drawing lines
74 
75  // Points in the ellipsoid:
76  // * "#slices" slices, with "#stacks" points each, but for the two ends
77  // * 1 point at each end slice
78  // #total points = stacks*(slices-2) + 2
79  ASSERT_EQUAL_((slices - 2) * stacks + 2, pts.size());
80  const size_t idx_1st_slice = 1;
81 
82  // 1st slice: triangle fan (if it were solid)
83  // ----------------------------
85  for (size_t i = 0; i < stacks; i++)
86  {
87  glVertex3fv(&pts[0][0]);
88  glVertex3fv(&pts[idx_1st_slice + i][0]);
89  }
90  glEnd();
91 
92  // Middle slices: triangle strip (if it were solid)
93  // ----------------------------
94  for (size_t s = 0; s < slices - 3; s++)
95  {
96  size_t idx_this_slice = idx_1st_slice + stacks * s;
97  size_t idx_next_slice = idx_this_slice + stacks;
98 
99  for (size_t i = 0; i < stacks; i++)
100  {
101  const size_t ii =
102  (i == (stacks - 1) ? 0 : i + 1); // next i with wrapping
103 
105  glVertex3fv(&pts[idx_this_slice + i][0]);
106  glVertex3fv(&pts[idx_next_slice + ii][0]);
107  glVertex3fv(&pts[idx_next_slice + i][0]);
108  glVertex3fv(&pts[idx_this_slice + i][0]);
109  glVertex3fv(&pts[idx_this_slice + ii][0]);
110  glVertex3fv(&pts[idx_next_slice + ii][0]);
111  glEnd();
112  }
113  }
114 
115  // Last slice: triangle fan (if it were solid)
116  // ----------------------------
117  const size_t idx_last_pt = pts.size() - 1;
118  const size_t idx_last_slice = idx_1st_slice + (slices - 3) * stacks;
119  glBegin(GL_LINES);
120  for (size_t i = 0; i < stacks; i++)
121  {
122  glVertex3fv(&pts[idx_last_pt][0]);
123  glVertex3fv(&pts[idx_last_slice + i][0]);
124  }
125  glEnd();
126 
127  // glBegin( GL_POINTS );
128  // const size_t N = pts.size();
129  // for (size_t i=0;i<N;i++)
130  // glVertex3f( pts[i][0], pts[i][1], pts[i][2] );
131  // glEnd();
132 
135 #else
136  MRPT_UNUSED_PARAM(pts);
137  MRPT_UNUSED_PARAM(lineWidth);
138  MRPT_UNUSED_PARAM(slices);
139  MRPT_UNUSED_PARAM(stacks);
140 #endif
141 }
142 
143 /*---------------------------------------------------------------
144  generalizedEllipsoidPoints: 2D
145  ---------------------------------------------------------------*/
146 template <>
150  std::vector<mrpt::math::CMatrixFixedNumeric<float, 2, 1>>& out_params_pts,
151  const uint32_t numSegments, const uint32_t numSegments_unused)
152 {
153  MRPT_UNUSED_PARAM(numSegments_unused);
154  out_params_pts.clear();
155  out_params_pts.reserve(numSegments);
156  const double Aa = 2 * M_PI / numSegments;
157  for (double ang = 0; ang < 2 * M_PI; ang += Aa)
158  {
159  const double ccos = cos(ang);
160  const double ssin = sin(ang);
161 
162  out_params_pts.resize(out_params_pts.size() + 1);
163 
164  Eigen::Matrix<float, 2, 1>& pt = out_params_pts.back();
165 
166  pt[0] = mean[0] + ccos * U.get_unsafe(0, 0) + ssin * U.get_unsafe(0, 1);
167  pt[1] = mean[1] + ccos * U.get_unsafe(1, 0) + ssin * U.get_unsafe(1, 1);
168  }
169 }
170 
172  const double x, const double y, const double z,
176 {
177  pts.resize(pts.size() + 1);
179  pt[0] = mean[0] + x * M.get_unsafe(0, 0) + y * M.get_unsafe(0, 1) +
180  z * M.get_unsafe(0, 2);
181  pt[1] = mean[1] + x * M.get_unsafe(1, 0) + y * M.get_unsafe(1, 1) +
182  z * M.get_unsafe(1, 2);
183  pt[2] = mean[2] + x * M.get_unsafe(2, 0) + y * M.get_unsafe(2, 1) +
184  z * M.get_unsafe(2, 2);
185 }
186 
187 /*---------------------------------------------------------------
188  generalizedEllipsoidPoints: 3D
189  ---------------------------------------------------------------*/
190 template <>
194  std::vector<mrpt::math::CMatrixFixedNumeric<float, 3, 1>>& pts,
195  const uint32_t slices, const uint32_t stacks)
196 {
197  MRPT_START
198  ASSERT_ABOVEEQ_(slices, 3);
199  ASSERT_ABOVEEQ_(stacks, 3);
200  // sin/cos cache --------
201  // Slices: [0,pi]
202  std::vector<double> slice_cos(slices), slice_sin(slices);
203  for (uint32_t i = 0; i < slices; i++)
204  {
205  double angle = M_PI * i / double(slices - 1);
206  slice_sin[i] = sin(angle);
207  slice_cos[i] = cos(angle);
208  }
209  // Stacks: [0,2*pi]
210  std::vector<double> stack_sin(stacks), stack_cos(stacks);
211  for (uint32_t i = 0; i < stacks; i++)
212  {
213  double angle = 2 * M_PI * i / double(stacks);
214  stack_sin[i] = sin(angle);
215  stack_cos[i] = cos(angle);
216  }
217 
218  // Points in the ellipsoid:
219  // * "#slices" slices, with "#stacks" points each, but for the two ends
220  // * 1 point at each end slice
221  // #total points = stacks*(slices-2) + 2
222  pts.clear();
223  pts.reserve((slices - 2) * stacks + 2);
224 
225  for (uint32_t i = 0; i < slices; i++)
226  {
227  if (i == 0)
228  aux_add3DpointWithEigenVectors(1, 0, 0, pts, U, mean);
229  else if (i == (slices - 1))
230  aux_add3DpointWithEigenVectors(-1, 0, 0, pts, U, mean);
231  else
232  {
233  const double x = slice_cos[i];
234  const double R = slice_sin[i];
235 
236  for (uint32_t j = 0; j < stacks; j++)
237  {
238  const double y = R * stack_cos[j];
239  const double z = R * stack_sin[j];
241  }
242  }
243  }
244 
245  MRPT_END
246 }
247 } // end namespaces
248 
249 
#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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020