MRPT  1.9.9
CArrow.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 
12 #include <mrpt/math/CMatrixF.h>
13 #include <mrpt/math/geometry.h>
14 #include <mrpt/opengl/CArrow.h>
17 
18 #include <memory>
19 
20 #include "opengl_internals.h"
21 
22 using namespace mrpt;
23 using namespace mrpt::opengl;
24 
25 using namespace mrpt::math;
26 using namespace std;
27 
29 
30 void CArrow::render_dl() const
31 {
32 #if MRPT_HAS_OPENGL_GLUT
33 
34  GLUquadricObj* obj1 = gluNewQuadric();
35  GLUquadricObj* obj2 = gluNewQuadric();
36 
37  GLfloat mat[16];
38 
39  // Compute the direction vector, which will become the transformed z-axis:
40  float vx = m_x1 - m_x0;
41  float vy = m_y1 - m_y0;
42  float vz = m_z1 - m_z0;
43  if ((m_arrow_roll != -1.0f) || (m_arrow_pitch != -1.0f) ||
44  (m_arrow_yaw != -1.0f))
45  {
46  m_x0 = 0.0f;
47  m_x1 = 0.0f;
48  m_y0 = 0.0f;
49  m_y1 = 0.1f;
50  m_z0 = 0.0f;
51  m_z1 = 0.0f;
52 
53  float cr = cos(m_arrow_roll);
54  float sr = sin(m_arrow_roll);
55  float cp = cos(m_arrow_pitch);
56  float sp = sin(m_arrow_pitch);
57  float cy = cos(m_arrow_yaw);
58  float sy = sin(m_arrow_yaw);
59 
60  CMatrixFloat m(3, 3), xx(3, 1), out(1, 3);
61  m(0, 0) = cr * cp;
62  m(0, 1) = cr * sp * sy - sr * cy;
63  m(0, 2) = sr * sy + cr * sp * cy;
64  m(1, 0) = sr * cp;
65  m(1, 1) = sr * sp * sy + cr * cy;
66  m(1, 2) = sr * sp * cy - cr * sy;
67  m(2, 0) = -sp;
68  m(2, 1) = cp * sy;
69  m(2, 2) = cp * cy;
70  xx(0, 0) = 0.0f;
71  xx(1, 0) = 1.0f;
72  xx(2, 0) = 0.0f;
73 
74  out = m * xx;
75  vx = out(0, 0);
76  vy = out(1, 0);
77  vz = out(2, 0);
78  }
79 
80  // Normalize:
81  const float v_mod = sqrt(square(vx) + square(vy) + square(vz));
82  if (v_mod > 0)
83  {
84  vx /= v_mod;
85  vy /= v_mod;
86  vz /= v_mod;
87  }
88 
89  // A homogeneous transformation matrix, in this order:
90  //
91  // 0 4 8 12
92  // 1 5 9 13
93  // 2 6 10 14
94  // 3 7 11 15
95  //
96 
97  mat[3] = mat[7] = mat[11] = 0;
98  mat[15] = 1;
99  mat[12] = m_x0;
100  mat[13] = m_y0;
101  mat[14] = m_z0;
102 
103  // New Z-axis
104  mat[8] = vx;
105  mat[9] = vy;
106  mat[10] = vz;
107 
108  // New X-axis: Perp. to Z
109  if (vx != 0 || vy != 0)
110  {
111  mat[0] = -vy;
112  mat[1] = vx;
113  mat[2] = 0;
114  }
115  else
116  {
117  mat[0] = 0;
118  mat[1] = vz;
119  mat[2] = -vy;
120  }
121 
122  // New Y-axis: Perp. to both: the cross product:
123  // | i j k | | i j k |
124  // | x0 y0 z0| --> | 8 9 10|
125  // | x1 y1 z1| | 0 1 2 |
126  GLfloat* out_v3 = mat + 4;
128  mat + 8, // 1st vector
129  mat + 0, // 2nd vector
130  out_v3 // Output cross product
131  );
132 
133  glPushMatrix();
134 
135  glMultMatrixf(mat);
136  // Scale Z to the size of the cylinder:
137  glScalef(1.0f, 1.0f, v_mod * (1.0f - m_headRatio));
138  gluCylinder(obj1, m_smallRadius, m_smallRadius, 1, 10, 1);
139 
140  glPopMatrix();
141 
142  // Draw the head of the arrow: a cone (built from a cylinder)
143  //-------------------------------------------------------------
144  mat[12] = m_x0 + vx * v_mod * (1.0f - m_headRatio);
145  mat[13] = m_y0 + vy * v_mod * (1.0f - m_headRatio);
146  mat[14] = m_z0 + vz * v_mod * (1.0f - m_headRatio);
147 
148  glPushMatrix();
149 
150  glMultMatrixf(mat);
151  // Scale Z to the size of the cylinder:
152  glScalef(1.0f, 1.0f, v_mod * m_headRatio);
153 
154  gluCylinder(obj2, m_largeRadius, 0, 1, 10, 10);
155 
156  glPopMatrix();
157 
158  gluDeleteQuadric(obj1);
159  gluDeleteQuadric(obj2);
160 
161 #endif
162 }
163 
164 uint8_t CArrow::serializeGetVersion() const { return 1; }
166 {
167  writeToStreamRender(out);
168  out << m_x0 << m_y0 << m_z0;
169  out << m_x1 << m_y1 << m_z1;
170  out << m_headRatio << m_smallRadius << m_largeRadius;
171  out << m_arrow_roll << m_arrow_pitch << m_arrow_yaw;
172 }
173 
175 {
176  switch (version)
177  {
178  case 0:
179  {
180  readFromStreamRender(in);
181  in >> m_x0 >> m_y0 >> m_z0;
182  in >> m_x1 >> m_y1 >> m_z1;
183  in >> m_headRatio >> m_smallRadius >> m_largeRadius;
184  }
185  break;
186  case 1:
187  {
188  readFromStreamRender(in);
189  in >> m_x0 >> m_y0 >> m_z0;
190  in >> m_x1 >> m_y1 >> m_z1;
191  in >> m_headRatio >> m_smallRadius >> m_largeRadius;
192  in >> m_arrow_roll >> m_arrow_pitch >> m_arrow_yaw;
193  }
194  break;
195  default:
197  };
199 }
200 
202 {
204  out["x0"] = m_x0;
205  out["y0"] = m_y0;
206  out["z0"] = m_z0;
207  out["x1"] = m_x1;
208  out["y1"] = m_y1;
209  out["z1"] = m_z1;
210  out["headRatio"] = m_headRatio;
211  out["smallRadius"] = m_smallRadius;
212  out["largeRadius"] = m_largeRadius;
213 }
214 
216 {
217  uint8_t version;
219  switch (version)
220  {
221  case 1:
222  {
223  m_x0 = static_cast<float>(in["x0"]);
224  m_y0 = static_cast<float>(in["y0"]);
225  m_z0 = static_cast<float>(in["z0"]);
226  m_x1 = static_cast<float>(in["x1"]);
227  m_y1 = static_cast<float>(in["y1"]);
228  m_z1 = static_cast<float>(in["z1"]);
229  m_headRatio = static_cast<float>(in["headRatio"]);
230  m_smallRadius = static_cast<float>(in["smallRadius"]);
231  m_largeRadius = static_cast<float>(in["largeRadius"]);
232  }
233  break;
234  default:
236  }
237 }
240 {
241  bb_min.x = std::min(m_x0, m_x1);
242  bb_min.y = std::min(m_y0, m_y1);
243  bb_min.z = std::min(m_z0, m_z1);
244 
245  bb_max.x = std::max(m_x0, m_x1);
246  bb_max.y = std::max(m_y0, m_y1);
247  bb_max.z = std::max(m_z0, m_z1);
248 
249  // Convert to coordinates of my parent:
250  m_pose.composePoint(bb_min, bb_min);
251  m_pose.composePoint(bb_max, bb_max);
252 }
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
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:238
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
GLAPI void GLAPIENTRY glPopMatrix(void)
STL namespace.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CArrow.cpp:164
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:804
float GLfloat
Definition: glew.h:218
Virtual base class for "schematic archives" (JSON, XML,...)
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:97
T square(const T x)
Inline function for the square of a number.
This base provides a set of functions for maths stuff.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CArrow.cpp:165
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CArrow.cpp:174
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
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:54
GLAPI void GLAPIENTRY glScalef(GLfloat x, GLfloat y, GLfloat z)
GLuint in
Definition: glext.h:7391
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
A 3D arrow.
Definition: CArrow.h:28
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
const auto bb_min
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
GLAPI void GLAPIENTRY glPushMatrix(void)
Lightweight 3D point.
Definition: TPoint3D.h:90
This template class provides the basic functionality for a general 2D any-size, resizable container o...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: abb8b1a1e Fri Oct 18 14:19:12 2019 +0200 at vie oct 18 14:20:13 CEST 2019