Main MRPT website > C++ reference for MRPT 1.5.6
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-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 "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 using namespace mrpt::utils;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 /*---------------------------------------------------------------
23  Render: 2D implementation
24  ---------------------------------------------------------------*/
25 namespace mrpt {
26 namespace opengl {
27 namespace detail {
28 template <>
30  const std::vector<mrpt::math::CMatrixFixedNumeric<float,2,1> > & pts,
31  const float lineWidth,
32  const uint32_t slices,
33  const uint32_t stacks)
34 {
35 #if MRPT_HAS_OPENGL_GLUT
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++)
45  glVertex2f( pts[i][0], pts[i][1] );
46 
47  glEnd();
48 
51 #else
53 #endif
54 }
55 
56 /*---------------------------------------------------------------
57  Render: 3D implementation
58  ---------------------------------------------------------------*/
59 template <>
61  const std::vector<mrpt::math::CMatrixFixedNumeric<float,3,1> > & pts,
62  const float lineWidth,
63  const uint32_t slices,
64  const uint32_t stacks
65  )
66 {
67 #if MRPT_HAS_OPENGL_GLUT
71  glDisable(GL_LIGHTING); // Disable lights when drawing lines
72 
73  // Points in the ellipsoid:
74  // * "#slices" slices, with "#stacks" points each, but for the two ends
75  // * 1 point at each end slice
76  // #total points = stacks*(slices-2) + 2
77  ASSERT_EQUAL_((slices-2)*stacks+2, pts.size())
78 
79  const size_t idx_1st_slice = 1;
80 
81  // 1st slice: triangle fan (if it were solid)
82  // ----------------------------
83  glBegin( GL_LINES );
84  for (size_t i=0;i<stacks;i++)
85  {
86  glVertex3fv(&pts[0][0]);
87  glVertex3fv(&pts[idx_1st_slice+i][0]);
88  }
89  glEnd();
90 
91  // Middle slices: triangle strip (if it were solid)
92  // ----------------------------
93  for (size_t s=0;s<slices-3;s++)
94  {
95  size_t idx_this_slice = idx_1st_slice + stacks*s;
96  size_t idx_next_slice = idx_this_slice + stacks;
97 
98  for (size_t i=0;i<stacks;i++)
99  {
100  const size_t ii = (i==(stacks-1) ? 0 : i+1); // next i with wrapping
101 
103  glVertex3fv(&pts[idx_this_slice+i][0]);
104  glVertex3fv(&pts[idx_next_slice+ii][0]);
105  glVertex3fv(&pts[idx_next_slice+i][0]);
106  glVertex3fv(&pts[idx_this_slice+i][0]);
107  glVertex3fv(&pts[idx_this_slice+ii][0]);
108  glVertex3fv(&pts[idx_next_slice+ii][0]);
109  glEnd();
110  }
111  }
112 
113  // Last slice: triangle fan (if it were solid)
114  // ----------------------------
115  const size_t idx_last_pt = pts.size()-1;
116  const size_t idx_last_slice = idx_1st_slice + (slices-3)*stacks;
117  glBegin( GL_LINES );
118  for (size_t i=0;i<stacks;i++)
119  {
120  glVertex3fv(&pts[idx_last_pt][0]);
121  glVertex3fv(&pts[idx_last_slice+i][0]);
122  }
123  glEnd();
124 
125  //glBegin( GL_POINTS );
126  //const size_t N = pts.size();
127  //for (size_t i=0;i<N;i++)
128  // glVertex3f( pts[i][0], pts[i][1], pts[i][2] );
129  //glEnd();
130 
133 #else
134  MRPT_UNUSED_PARAM(pts); MRPT_UNUSED_PARAM(lineWidth); MRPT_UNUSED_PARAM(slices); MRPT_UNUSED_PARAM(stacks);
135 #endif
136 }
137 
138 
139 /*---------------------------------------------------------------
140  generalizedEllipsoidPoints: 2D
141  ---------------------------------------------------------------*/
142 template <>
146  std::vector<mrpt::math::CMatrixFixedNumeric<float,2,1> > &out_params_pts,
147  const uint32_t numSegments,
148  const uint32_t numSegments_unused)
149 {
150  MRPT_UNUSED_PARAM(numSegments_unused);
151  out_params_pts.clear();
152  out_params_pts.reserve(numSegments);
153  const double Aa = 2*M_PI/numSegments;
154  for (double ang=0;ang<2*M_PI;ang+=Aa)
155  {
156  const double ccos = cos(ang);
157  const double ssin = sin(ang);
158 
159  out_params_pts.resize(out_params_pts.size()+1);
160 
161  Eigen::Matrix<float,2,1> &pt = out_params_pts.back();
162 
163  pt[0] = mean[0] + ccos * U.get_unsafe(0,0) + ssin * U.get_unsafe(0,1);
164  pt[1] = mean[1] + ccos * U.get_unsafe(1,0) + ssin * U.get_unsafe(1,1);
165  }
166 }
167 
168 
169 
170 inline
172  const double x,
173  const double y,
174  const double z,
175  std::vector<mrpt::math::CMatrixFixedNumeric<float,3,1> > & pts,
178 {
179  pts.resize(pts.size()+1);
181  pt[0] = mean[0] + x * M.get_unsafe(0,0) + y * M.get_unsafe(0,1) + z * M.get_unsafe(0,2);
182  pt[1] = mean[1] + x * M.get_unsafe(1,0) + y * M.get_unsafe(1,1) + z * M.get_unsafe(1,2);
183  pt[2] = mean[2] + x * M.get_unsafe(2,0) + y * M.get_unsafe(2,1) + z * M.get_unsafe(2,2);
184 }
185 
186 
187 /*---------------------------------------------------------------
188  generalizedEllipsoidPoints: 3D
189  ---------------------------------------------------------------*/
190 template <>
194  std::vector<mrpt::math::CMatrixFixedNumeric<float,3,1> > & pts,
195  const uint32_t slices,
196  const uint32_t stacks)
197 {
198  MRPT_START
199  ASSERT_ABOVEEQ_(slices,3)
200  ASSERT_ABOVEEQ_(stacks,3)
201 
202  // sin/cos cache --------
203  // Slices: [0,pi]
204  std::vector<double> slice_cos(slices),slice_sin(slices);
205  for (uint32_t i = 0; i < slices; i++)
206  {
207  double angle = M_PI * i / double(slices-1);
208  slice_sin[i] = sin(angle);
209  slice_cos[i] = cos(angle);
210  }
211  // Stacks: [0,2*pi]
212  std::vector<double> stack_sin(stacks),stack_cos(stacks);
213  for (uint32_t i = 0; i < stacks; i++)
214  {
215  double angle = 2*M_PI * i / double(stacks);
216  stack_sin[i] = sin(angle);
217  stack_cos[i] = cos(angle);
218  }
219 
220  // Points in the ellipsoid:
221  // * "#slices" slices, with "#stacks" points each, but for the two ends
222  // * 1 point at each end slice
223  // #total points = stacks*(slices-2) + 2
224  pts.clear();
225  pts.reserve((slices-2)*stacks+2);
226 
227  for (uint32_t i=0;i<slices;i++)
228  {
229  if (i==0)
230  aux_add3DpointWithEigenVectors(1,0,0, pts,U,mean);
231  else if (i==(slices-1))
232  aux_add3DpointWithEigenVectors(-1,0,0, pts,U,mean);
233  else
234  {
235  const double x = slice_cos[i];
236  const double R = slice_sin[i];
237 
238  for (uint32_t j=0;j<stacks;j++)
239  {
240  const double y = R*stack_cos[j];
241  const double z = R*stack_sin[j];
242  aux_add3DpointWithEigenVectors(x,y,z, pts,U,mean);
243  }
244  }
245  }
246 
247  MRPT_END
248 }
249 
250 
251 }}} // end namespaces
#define ASSERT_EQUAL_(__A, __B)
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
GLAPI void GLAPIENTRY glVertex3fv(const GLfloat *v)
GLAPI void GLAPIENTRY glEnable(GLenum cap)
#define M_PI
Definition: bits.h:78
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:283
#define GL_LIGHTING
Definition: glew.h:381
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
void renderGeneralizedEllipsoidTemplate< 2 >(const std::vector< mrpt::math::CMatrixFixedNumeric< float, 2, 1 > > &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
GLdouble s
Definition: glew.h:1295
A numeric matrix of compile-time fixed size.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
void OPENGL_IMPEXP 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)
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:428
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_LINE_LOOP
Definition: glew.h:270
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)
#define MRPT_START
#define GL_SRC_ALPHA
Definition: glew.h:282
GLdouble GLdouble z
Definition: glew.h:1464
#define ASSERT_ABOVEEQ_(__A, __B)
const float R
void OPENGL_IMPEXP checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:134
GLAPI void GLAPIENTRY glEnd(void)
void OPENGL_IMPEXP 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)
#define GL_LINE_STRIP
Definition: glew.h:271
GLdouble angle
Definition: glew.h:5105
#define GL_LINES
Definition: glew.h:269
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
unsigned __int32 uint32_t
Definition: rptypes.h:49
GLAPI void GLAPIENTRY glVertex2f(GLfloat x, GLfloat y)
GLAPI void GLAPIENTRY glDisable(GLenum cap)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018