Main MRPT website > C++ reference for MRPT 1.9.9
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-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 
12 #include <mrpt/opengl/CArrow.h>
13 #include <mrpt/math/CMatrix.h>
14 #include <mrpt/math/geometry.h>
16 
17 #include "opengl_internals.h"
18 
19 using namespace mrpt;
20 using namespace mrpt::opengl;
21 
22 using namespace mrpt::math;
23 using namespace std;
24 
26 
27 /** Class factory */
28 CArrow::Ptr CArrow::Create(
29  float x0, float y0, float z0, float x1, float y1, float z1, float headRatio,
30  float smallRadius, float largeRadius, float arrow_roll, float arrow_pitch,
31  float arrow_yaw)
32 {
33  return CArrow::Ptr(
34  new CArrow(
35  x0, y0, z0, x1, y1, z1, headRatio, smallRadius, largeRadius,
36  arrow_roll, arrow_pitch, arrow_yaw));
37 }
38 /*---------------------------------------------------------------
39  render
40  ---------------------------------------------------------------*/
41 void CArrow::render_dl() const
42 {
43 #if MRPT_HAS_OPENGL_GLUT
44 
45  GLUquadricObj* obj1 = gluNewQuadric();
46  GLUquadricObj* obj2 = gluNewQuadric();
47 
48  GLfloat mat[16];
49 
50  // Compute the direction vector, which will become the transformed z-axis:
51  float vx = m_x1 - m_x0;
52  float vy = m_y1 - m_y0;
53  float vz = m_z1 - m_z0;
54  if ((m_arrow_roll != -1.0f) || (m_arrow_pitch != -1.0f) ||
55  (m_arrow_yaw != -1.0f))
56  {
57  m_x0 = 0.0f;
58  m_x1 = 0.0f;
59  m_y0 = 0.0f;
60  m_y1 = 0.1f;
61  m_z0 = 0.0f;
62  m_z1 = 0.0f;
63 
64  float cr = cos(m_arrow_roll);
65  float sr = sin(m_arrow_roll);
66  float cp = cos(m_arrow_pitch);
67  float sp = sin(m_arrow_pitch);
68  float cy = cos(m_arrow_yaw);
69  float sy = sin(m_arrow_yaw);
70 
71  CMatrixFloat m(3, 3), xx(3, 1), out(1, 3);
72  m(0, 0) = cr * cp;
73  m(0, 1) = cr * sp * sy - sr * cy;
74  m(0, 2) = sr * sy + cr * sp * cy;
75  m(1, 0) = sr * cp;
76  m(1, 1) = sr * sp * sy + cr * cy;
77  m(1, 2) = sr * sp * cy - cr * sy;
78  m(2, 0) = -sp;
79  m(2, 1) = cp * sy;
80  m(2, 2) = cp * cy;
81  xx(0, 0) = 0.0f;
82  xx(1, 0) = 1.0f;
83  xx(2, 0) = 0.0f;
84 
85  out = m * xx;
86  vx = out(0, 0);
87  vy = out(1, 0);
88  vz = out(2, 0);
89  }
90 
91  // Normalize:
92  const float v_mod = sqrt(square(vx) + square(vy) + square(vz));
93  if (v_mod > 0)
94  {
95  vx /= v_mod;
96  vy /= v_mod;
97  vz /= v_mod;
98  }
99 
100  // A homogeneous transformation matrix, in this order:
101  //
102  // 0 4 8 12
103  // 1 5 9 13
104  // 2 6 10 14
105  // 3 7 11 15
106  //
107 
108  mat[3] = mat[7] = mat[11] = 0;
109  mat[15] = 1;
110  mat[12] = m_x0;
111  mat[13] = m_y0;
112  mat[14] = m_z0;
113 
114  // New Z-axis
115  mat[8] = vx;
116  mat[9] = vy;
117  mat[10] = vz;
118 
119  // New X-axis: Perp. to Z
120  if (vx != 0 || vy != 0)
121  {
122  mat[0] = -vy;
123  mat[1] = vx;
124  mat[2] = 0;
125  }
126  else
127  {
128  mat[0] = 0;
129  mat[1] = vz;
130  mat[2] = -vy;
131  }
132 
133  // New Y-axis: Perp. to both: the cross product:
134  // | i j k | | i j k |
135  // | x0 y0 z0| --> | 8 9 10|
136  // | x1 y1 z1| | 0 1 2 |
137  GLfloat* out_v3 = mat + 4;
139  mat + 8, // 1st vector
140  mat + 0, // 2nd vector
141  out_v3 // Output cross product
142  );
143 
144  glPushMatrix();
145 
146  glMultMatrixf(mat);
147  // Scale Z to the size of the cylinder:
148  glScalef(1.0f, 1.0f, v_mod * (1.0f - m_headRatio));
149  gluCylinder(obj1, m_smallRadius, m_smallRadius, 1, 10, 1);
150 
151  glPopMatrix();
152 
153  // Draw the head of the arrow: a cone (built from a cylinder)
154  //-------------------------------------------------------------
155  mat[12] = m_x0 + vx * v_mod * (1.0f - m_headRatio);
156  mat[13] = m_y0 + vy * v_mod * (1.0f - m_headRatio);
157  mat[14] = m_z0 + vz * v_mod * (1.0f - m_headRatio);
158 
159  glPushMatrix();
160 
161  glMultMatrixf(mat);
162  // Scale Z to the size of the cylinder:
163  glScalef(1.0f, 1.0f, v_mod * m_headRatio);
164 
165  gluCylinder(obj2, m_largeRadius, 0, 1, 10, 10);
166 
167  glPopMatrix();
168 
169  gluDeleteQuadric(obj1);
170  gluDeleteQuadric(obj2);
171 
172 #endif
173 }
174 
177 {
178  writeToStreamRender(out);
179  out << m_x0 << m_y0 << m_z0;
180  out << m_x1 << m_y1 << m_z1;
181  out << m_headRatio << m_smallRadius << m_largeRadius;
182  out << m_arrow_roll << m_arrow_pitch << m_arrow_yaw;
183 }
184 
186 {
187  switch (version)
188  {
189  case 0:
190  {
191  readFromStreamRender(in);
192  in >> m_x0 >> m_y0 >> m_z0;
193  in >> m_x1 >> m_y1 >> m_z1;
194  in >> m_headRatio >> m_smallRadius >> m_largeRadius;
195  }
196  break;
197  case 1:
198  {
199  readFromStreamRender(in);
200  in >> m_x0 >> m_y0 >> m_z0;
201  in >> m_x1 >> m_y1 >> m_z1;
202  in >> m_headRatio >> m_smallRadius >> m_largeRadius;
203  in >> m_arrow_roll >> m_arrow_pitch >> m_arrow_yaw;
204  }
205  break;
206  default:
208  };
210 }
211 
213  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
214 {
215  bb_min.x = std::min(m_x0, m_x1);
216  bb_min.y = std::min(m_y0, m_y1);
217  bb_min.z = std::min(m_z0, m_z1);
218 
219  bb_max.x = std::max(m_x0, m_x1);
220  bb_max.y = std::max(m_y0, m_y1);
221  bb_max.z = std::max(m_z0, m_z1);
222 
223  // Convert to coordinates of my parent:
224  m_pose.composePoint(bb_min, bb_min);
225  m_pose.composePoint(bb_max, bb_max);
226 }
#define min(a, b)
void render_dl() const override
Render.
Definition: CArrow.cpp:41
GLAPI void GLAPIENTRY glMultMatrixf(const GLfloat *m)
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const override
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
Definition: CArrow.cpp:212
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLAPI void GLAPIENTRY glPopMatrix(void)
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
STL namespace.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CArrow.cpp:175
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:803
float GLfloat
Definition: glew.h:217
unsigned char uint8_t
Definition: rptypes.h:41
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
This base provides a set of functions for maths stuff.
double x
X,Y,Z coordinates.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CArrow.cpp:176
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CArrow.cpp:185
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
GLAPI void GLAPIENTRY glScalef(GLfloat x, GLfloat y, GLfloat z)
GLuint in
Definition: glext.h:7274
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
A 3D arrow.
Definition: CArrow.h:31
GLAPI void GLAPIENTRY glPushMatrix(void)
Lightweight 3D point.
std::shared_ptr< CArrow > Ptr
Definition: CArrow.h:33



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