MRPT  1.9.9
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-2019, 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/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::CMatrixFixed<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::CMatrixFixed<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::CMatrixFixed<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  auto& pt = out_params_pts.back();
165 
166  pt[0] = mean[0] + ccos * U(0, 0) + ssin * U(0, 1);
167  pt[1] = mean[1] + ccos * U(1, 0) + ssin * U(1, 1);
168  }
169 }
170 
172  const double x, const double y, const double z,
173  std::vector<mrpt::math::CMatrixFixed<float, 3, 1>>& pts,
176 {
177  pts.resize(pts.size() + 1);
178  mrpt::math::CMatrixFixed<float, 3, 1>& pt = pts.back();
179  pt[0] = mean[0] + x * M(0, 0) + y * M(0, 1) + z * M(0, 2);
180  pt[1] = mean[1] + x * M(1, 0) + y * M(1, 1) + z * M(1, 2);
181  pt[2] = mean[2] + x * M(2, 0) + y * M(2, 1) + z * M(2, 2);
182 }
183 
184 /*---------------------------------------------------------------
185  generalizedEllipsoidPoints: 3D
186  ---------------------------------------------------------------*/
187 template <>
191  std::vector<mrpt::math::CMatrixFixed<float, 3, 1>>& pts,
192  const uint32_t slices, const uint32_t stacks)
193 {
194  MRPT_START
195  ASSERT_ABOVEEQ_(slices, 3);
196  ASSERT_ABOVEEQ_(stacks, 3);
197  // sin/cos cache --------
198  // Slices: [0,pi]
199  std::vector<double> slice_cos(slices), slice_sin(slices);
200  for (uint32_t i = 0; i < slices; i++)
201  {
202  double angle = M_PI * i / double(slices - 1);
203  slice_sin[i] = sin(angle);
204  slice_cos[i] = cos(angle);
205  }
206  // Stacks: [0,2*pi]
207  std::vector<double> stack_sin(stacks), stack_cos(stacks);
208  for (uint32_t i = 0; i < stacks; i++)
209  {
210  double angle = 2 * M_PI * i / double(stacks);
211  stack_sin[i] = sin(angle);
212  stack_cos[i] = cos(angle);
213  }
214 
215  // Points in the ellipsoid:
216  // * "#slices" slices, with "#stacks" points each, but for the two ends
217  // * 1 point at each end slice
218  // #total points = stacks*(slices-2) + 2
219  pts.clear();
220  pts.reserve((slices - 2) * stacks + 2);
221 
222  for (uint32_t i = 0; i < slices; i++)
223  {
224  if (i == 0)
225  aux_add3DpointWithEigenVectors(1, 0, 0, pts, U, mean);
226  else if (i == (slices - 1))
227  aux_add3DpointWithEigenVectors(-1, 0, 0, pts, U, mean);
228  else
229  {
230  const double x = slice_cos[i];
231  const double R = slice_sin[i];
232 
233  for (uint32_t j = 0; j < stacks; j++)
234  {
235  const double y = R * stack_cos[j];
236  const double z = R * stack_sin[j];
238  }
239  }
240  }
241 
242  MRPT_END
243 }
244 } // namespace mrpt::opengl::detail
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble z
Definition: glext.h:3879
GLAPI void GLAPIENTRY glVertex3fv(const GLfloat *v)
GLAPI void GLAPIENTRY glEnable(GLenum cap)
void renderGeneralizedEllipsoidTemplate< 3 >(const std::vector< mrpt::math::CMatrixFixed< float, 3, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
STL namespace.
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:288
GLdouble s
Definition: glext.h:3682
#define GL_LIGHTING
Definition: glew.h:386
void generalizedEllipsoidPoints< 2 >(const mrpt::math::CMatrixFixed< double, 2, 2 > &U, const mrpt::math::CMatrixFixed< double, 2, 1 > &mean, std::vector< mrpt::math::CMatrixFixed< float, 2, 1 >> &out_params_pts, const uint32_t slices, const uint32_t stacks)
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
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
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:433
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
#define GL_LINE_LOOP
Definition: glew.h:275
#define GL_SRC_ALPHA
Definition: glew.h:287
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void generalizedEllipsoidPoints< 3 >(const mrpt::math::CMatrixFixed< double, 3, 3 > &U, const mrpt::math::CMatrixFixed< double, 3, 1 > &mean, std::vector< mrpt::math::CMatrixFixed< float, 3, 1 >> &out_params_pts, const uint32_t slices, const uint32_t stacks)
const float R
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:155
#define MRPT_END
Definition: exceptions.h:245
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
double mean(const CONTAINER &v)
Computes the mean value of a vector.
GLAPI void GLAPIENTRY glEnd(void)
GLenum GLint GLint y
Definition: glext.h:3542
#define GL_LINE_STRIP
Definition: glew.h:276
#define GL_LINES
Definition: glew.h:274
GLenum GLint x
Definition: glext.h:3542
GLAPI void GLAPIENTRY glVertex2f(GLfloat x, GLfloat y)
GLAPI void GLAPIENTRY glDisable(GLenum cap)
void renderGeneralizedEllipsoidTemplate< 2 >(const std::vector< mrpt::math::CMatrixFixed< float, 2, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
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)
#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: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019