Main MRPT website > C++ reference for MRPT 1.5.6
CArrow.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 
12 
13 #include <mrpt/opengl/CArrow.h>
14 #include <mrpt/math/CMatrix.h>
15 #include <mrpt/math/geometry.h>
16 #include <mrpt/utils/CStream.h>
17 
18 #include "opengl_internals.h"
19 
20 
21 using namespace mrpt;
22 using namespace mrpt::opengl;
23 using namespace mrpt::utils;
24 using namespace mrpt::math;
25 using namespace std;
26 
27 
29 
30 /** Class factory */
31 CArrowPtr CArrow::Create(
32  float x0,
33  float y0,
34  float z0,
35  float x1,
36  float y1,
37  float z1,
38  float headRatio,
39  float smallRadius,
40  float largeRadius,
41  float arrow_roll,
42  float arrow_pitch,
43  float arrow_yaw
44  )
45 {
46  return CArrowPtr(new CArrow(x0,y0,z0, x1,y1,z1, headRatio, smallRadius, largeRadius, arrow_roll, arrow_pitch, arrow_yaw ));
47 }
48 /*---------------------------------------------------------------
49  render
50  ---------------------------------------------------------------*/
51 void CArrow::render_dl() const
52 {
53 #if MRPT_HAS_OPENGL_GLUT
54 
55  GLUquadricObj *obj1 = gluNewQuadric();
56  GLUquadricObj *obj2 = gluNewQuadric();
57 
58  GLfloat mat[16];
59 
60  // Compute the direction vector, which will become the transformed z-axis:
61  float vx = m_x1-m_x0;
62  float vy = m_y1-m_y0;
63  float vz = m_z1-m_z0;
64  if ((m_arrow_roll!=-1.0f)||(m_arrow_pitch!=-1.0f)||(m_arrow_yaw!=-1.0f))
65  {
66  m_x0 = 0.0f;
67  m_x1 = 0.0f;
68  m_y0 = 0.0f;
69  m_y1 = 0.1f;
70  m_z0 = 0.0f;
71  m_z1 = 0.0f;
72 
73  float cr = cos(m_arrow_roll);
74  float sr = sin(m_arrow_roll);
75  float cp = cos(m_arrow_pitch);
76  float sp = sin(m_arrow_pitch);
77  float cy = cos(m_arrow_yaw);
78  float sy = sin(m_arrow_yaw);
79 
80  CMatrixFloat m(3,3),xx(3,1),out(1,3);
81  m(0,0) = cr*cp; m(0,1) = cr*sp*sy - sr*cy; m(0,2) = sr*sy + cr*sp*cy;
82  m(1,0) = sr*cp; m(1,1) = sr*sp*sy + cr*cy; m(1,2) = sr*sp*cy - cr*sy;
83  m(2,0) = -sp; m(2,1) = cp*sy; m(2,2) = cp*cy;
84  xx(0,0) = 0.0f;
85  xx(1,0) = 1.0f;
86  xx(2,0) = 0.0f;
87 
88  out = m * xx;
89  vx = out(0,0);
90  vy = out(1,0);
91  vz = out(2,0);
92  }
93 
94  // Normalize:
95  const float v_mod = sqrt( square(vx)+square(vy)+square(vz) );
96  if (v_mod>0)
97  {
98  vx/=v_mod;
99  vy/=v_mod;
100  vz/=v_mod;
101  }
102 
103  // A homogeneous transformation matrix, in this order:
104  //
105  // 0 4 8 12
106  // 1 5 9 13
107  // 2 6 10 14
108  // 3 7 11 15
109  //
110 
111  mat[3] = mat[7] = mat[11] = 0;
112  mat[15] = 1;
113  mat[12] = m_x0; mat[13] = m_y0; mat[14] = m_z0;
114 
115  // New Z-axis
116  mat[8] = vx;
117  mat[9] = vy;
118  mat[10] = vz;
119 
120  // New X-axis: Perp. to Z
121  if (vx!=0 || vy!=0)
122  {
123  mat[0] = -vy;
124  mat[1] = vx;
125  mat[2] = 0;
126  }
127  else
128  {
129  mat[0] = 0;
130  mat[1] = vz;
131  mat[2] = -vy;
132  }
133 
134  // New Y-axis: Perp. to both: the cross product:
135  // | i j k | | i j k |
136  // | x0 y0 z0| --> | 8 9 10|
137  // | x1 y1 z1| | 0 1 2 |
138  GLfloat *out_v3 = mat+4;
140  mat+8, // 1st vector
141  mat+0, // 2nd vector
142  out_v3 // Output cross product
143  );
144 
145  glPushMatrix();
146 
147  glMultMatrixf( mat );
148  // Scale Z to the size of the cylinder:
149  glScalef(1.0f,1.0f,v_mod*(1.0f-m_headRatio));
150  gluCylinder( obj1, m_smallRadius, m_smallRadius, 1, 10, 1 );
151 
152  glPopMatrix();
153 
154  // Draw the head of the arrow: a cone (built from a cylinder)
155  //-------------------------------------------------------------
156  mat[12] = m_x0 + vx*v_mod*(1.0f-m_headRatio);
157  mat[13] = m_y0 + vy*v_mod*(1.0f-m_headRatio);
158  mat[14] = m_z0 + vz*v_mod*(1.0f-m_headRatio);
159 
160  glPushMatrix();
161 
162  glMultMatrixf( mat );
163  // Scale Z to the size of the cylinder:
164  glScalef(1.0f,1.0f,v_mod*m_headRatio);
165 
166  gluCylinder( obj2, m_largeRadius, 0, 1, 10, 10 );
167 
168  glPopMatrix();
169 
170  gluDeleteQuadric(obj1);
171  gluDeleteQuadric(obj2);
172 
173 #endif
174 }
175 
176 /*---------------------------------------------------------------
177  Implements the writing to a CStream capability of
178  CSerializable objects
179  ---------------------------------------------------------------*/
181 {
182  if (version)
183  *version = 1;
184  else
185  {
186  writeToStreamRender(out);
187  out << m_x0 << m_y0 << m_z0;
188  out << m_x1 << m_y1 << m_z1;
189  out << m_headRatio << m_smallRadius << m_largeRadius;
190  out << m_arrow_roll << m_arrow_pitch << m_arrow_yaw;
191 
192  }
193 }
194 
195 /*---------------------------------------------------------------
196  Implements the reading from a CStream capability of
197  CSerializable objects
198  ---------------------------------------------------------------*/
200 {
201  switch(version)
202  {
203  case 0:
204  {
205  readFromStreamRender(in);
206  in >> m_x0 >> m_y0 >> m_z0;
207  in >> m_x1 >> m_y1 >> m_z1;
208  in >> m_headRatio >> m_smallRadius >> m_largeRadius;
209  }
210  break;
211  case 1:
212  {
213  readFromStreamRender(in);
214  in >> m_x0 >> m_y0 >> m_z0;
215  in >> m_x1 >> m_y1 >> m_z1;
216  in >> m_headRatio >> m_smallRadius >> m_largeRadius;
217  in >> m_arrow_roll >> m_arrow_pitch >> m_arrow_yaw;
218  }
219  break;
220  default:
222 
223  };
225 }
226 
227 
229 {
230  bb_min.x = std::min( m_x0, m_x1 );
231  bb_min.y = std::min( m_y0, m_y1 );
232  bb_min.z = std::min( m_z0, m_z1 );
233 
234  bb_max.x = std::max( m_x0, m_x1 );
235  bb_max.y = std::max( m_y0, m_y1 );
236  bb_max.z = std::max( m_z0, m_z1 );
237 
238  // Convert to coordinates of my parent:
239  m_pose.composePoint(bb_min, bb_min);
240  m_pose.composePoint(bb_max, bb_max);
241 }
#define min(a, b)
GLAPI void GLAPIENTRY glMultMatrixf(const GLfloat *m)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const MRPT_OVERRIDE
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
Definition: CArrow.cpp:228
GLAPI void GLAPIENTRY glPopMatrix(void)
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CArrow.cpp:199
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
double z
X,Y,Z coordinates.
GLuint in
Definition: glew.h:7146
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
Definition: geometry.h:614
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
A renderizable object suitable for rendering with OpenGL's display lists.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CArrow.cpp:180
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
int version
Definition: mrpt_jpeglib.h:898
float GLfloat
Definition: glew.h:213
GLAPI void GLAPIENTRY glScalef(GLfloat x, GLfloat y, GLfloat z)
const GLdouble * m
Definition: glew.h:5094
A 3D arrow.
Definition: CArrow.h:34
GLAPI void GLAPIENTRY glPushMatrix(void)
Lightweight 3D point.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
void render_dl() const MRPT_OVERRIDE
Render.
Definition: CArrow.cpp:51



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