MRPT  2.0.4
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-2020, 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/geometry.h>
13 #include <mrpt/opengl/CArrow.h>
16 
17 #include <memory>
18 
19 using namespace mrpt;
20 using namespace mrpt::opengl;
21 using namespace mrpt::math;
22 using namespace std;
23 
25 
26 void CArrow::onUpdateBuffers_Triangles()
27 {
28  using P3f = mrpt::math::TPoint3Df;
29  using V3f = mrpt::math::TVector3Df;
30 
32  tris.clear();
33 
34  // Compute the XYZ local frame of reference for the arrow:
35  // XY radial, Z from Point #0 -> point #1:
36  const P3f p0(m_x0, m_y0, m_z0), p1(m_x1, m_y1, m_z1);
37  auto p = p1 - p0;
38  const float P10_norm = p.norm();
39  ASSERT_ABOVE_(P10_norm, .0f);
40  // Unit vector:
41  p *= (1.0f / P10_norm);
42 
43  setLocalRepresentativePoint((p0 + p1) * 0.5f);
44 
45  // each column is a unit vector:
47  p, 2 /* provided vector is "z"*/);
48 
49  // Transformation:
50  const mrpt::poses::CPose3D T(
51  HM.blockCopy<3, 3>(0, 0), mrpt::math::TPoint3D(m_x0, m_y0, m_z0));
52 
53  // precomputed table:
54  ASSERT_ABOVE_(m_slices, 2);
55 
56  const float dAng = 2 * M_PIf / m_slices;
57  float a = 0;
58  // unit cc points: cos(ang),sin(ang)
59  std::vector<mrpt::math::TPoint2Df> cc(m_slices);
60  for (unsigned int i = 0; i < m_slices; i++, a += dAng)
61  {
62  cc[i].x = cos(a);
63  cc[i].y = sin(a);
64  }
65 
66  ASSERT_ABOVEEQ_(m_headRatio, .0f);
67  ASSERT_BELOWEQ_(m_headRatio, 1.0f);
68 
69  const float r0 = m_smallRadius, r1 = m_largeRadius,
70  h0 = P10_norm * (1.0f - m_headRatio), h1 = P10_norm;
71 
72  const float wall_tilt = 0;
73  const float coswt = std::cos(wall_tilt), sinwt = std::sin(wall_tilt);
74 
75  const float head_tilt = std::atan2(r1, P10_norm * m_headRatio);
76  const float cosht = std::cos(head_tilt), sinht = std::sin(head_tilt);
77 
78  // cylinder walls:
79  for (unsigned int i = 0; i < m_slices; i++)
80  {
81  const auto ip = (i + 1) % m_slices;
82 
83  tris.emplace_back(
84  // Points:
85  T.composePoint(P3f(r0 * cc[i].x, r0 * cc[i].y, .0f)),
86  T.composePoint(P3f(r0 * cc[ip].x, r0 * cc[ip].y, .0f)),
87  T.composePoint(P3f(r0 * cc[i].x, r0 * cc[i].y, h0)),
88  // Normals:
89  T.rotateVector(V3f(-coswt * cc[i].y, coswt * cc[i].x, sinwt)),
90  T.rotateVector(V3f(-coswt * cc[ip].y, coswt * cc[ip].x, sinwt)),
91  T.rotateVector(V3f(-coswt * cc[i].y, coswt * cc[i].x, sinwt)));
92 
93  tris.emplace_back(
94  // Points:
95  T.composePoint(P3f(r0 * cc[ip].x, r0 * cc[ip].y, .0f)),
96  T.composePoint(P3f(r0 * cc[ip].x, r0 * cc[ip].y, h0)),
97  T.composePoint(P3f(r0 * cc[i].x, r0 * cc[i].y, h0)),
98  // Normals:
99  T.rotateVector(V3f(-coswt * cc[ip].y, coswt * cc[ip].x, sinwt)),
100  T.rotateVector(V3f(-coswt * cc[ip].y, coswt * cc[ip].x, sinwt)),
101  T.rotateVector(V3f(-coswt * cc[i].y, coswt * cc[i].x, sinwt)));
102  }
103 
104  // top cone:
105  for (unsigned int i = 0; i < m_slices; i++)
106  {
107  const auto ip = (i + 1) % m_slices;
108  tris.emplace_back(
109  // Points:
110  T.composePoint(P3f(r1 * cc[i].x, r1 * cc[i].y, h0)),
111  T.composePoint(P3f(r1 * cc[ip].x, r1 * cc[ip].y, h0)),
112  T.composePoint(P3f(.0f, .0f, h1)),
113  // Normals:
114  T.rotateVector(V3f(-cosht * cc[i].y, cosht * cc[i].x, sinht)),
115  T.rotateVector(V3f(-cosht * cc[ip].y, cosht * cc[ip].x, sinht)),
116  T.rotateVector(V3f(-cosht * cc[i].y, cosht * cc[i].x, sinht)));
117  }
118 
119  // All faces, same color:
120  for (auto& t : tris) t.setColor(m_color);
121 }
122 
123 uint8_t CArrow::serializeGetVersion() const { return 2; }
125 {
126  writeToStreamRender(out);
127  out << m_x0 << m_y0 << m_z0;
128  out << m_x1 << m_y1 << m_z1;
129  out << m_headRatio << m_smallRadius << m_largeRadius;
130  out << m_slices;
131 }
132 
134 {
135  switch (version)
136  {
137  case 0:
138  case 1:
139  case 2:
140  {
141  readFromStreamRender(in);
142  in >> m_x0 >> m_y0 >> m_z0;
143  in >> m_x1 >> m_y1 >> m_z1;
144  in >> m_headRatio >> m_smallRadius >> m_largeRadius;
145  if (version == 1)
146  {
147  float arrow_roll, arrow_pitch, arrow_yaw;
148  in >> arrow_roll >> arrow_pitch >> arrow_yaw;
149  }
150  if (version >= 2) in >> m_slices;
151  }
152  break;
153  default:
155  };
157 }
158 
160 {
162  out["x0"] = m_x0;
163  out["y0"] = m_y0;
164  out["z0"] = m_z0;
165  out["x1"] = m_x1;
166  out["y1"] = m_y1;
167  out["z1"] = m_z1;
168  out["headRatio"] = m_headRatio;
169  out["smallRadius"] = m_smallRadius;
170  out["largeRadius"] = m_largeRadius;
171  out["slices"] = m_slices;
172 }
173 
175 {
176  uint8_t version;
178  switch (version)
179  {
180  case 1:
181  {
182  m_x0 = static_cast<float>(in["x0"]);
183  m_y0 = static_cast<float>(in["y0"]);
184  m_z0 = static_cast<float>(in["z0"]);
185  m_x1 = static_cast<float>(in["x1"]);
186  m_y1 = static_cast<float>(in["y1"]);
187  m_z1 = static_cast<float>(in["z1"]);
188  m_headRatio = static_cast<float>(in["headRatio"]);
189  m_smallRadius = static_cast<float>(in["smallRadius"]);
190  m_largeRadius = static_cast<float>(in["largeRadius"]);
191  m_slices = static_cast<unsigned int>(in["slices"]);
192  }
193  break;
194  default:
196  }
197 }
200 {
201  bb_min.x = std::min(m_x0, m_x1);
202  bb_min.y = std::min(m_y0, m_y1);
203  bb_min.z = std::min(m_z0, m_z1);
204 
205  bb_max.x = std::max(m_x0, m_x1);
206  bb_max.y = std::max(m_y0, m_y1);
207  bb_max.z = std::max(m_z0, m_z1);
208 
209  // Convert to coordinates of my parent:
210  m_pose.composePoint(bb_min, bb_min);
211  m_pose.composePoint(bb_max, bb_max);
212 }
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
Definition: MatrixBase.h:233
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
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:198
TPoint3Df TVector3Df
Definition: TPoint3D.h:274
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
STL namespace.
Renderizable generic renderer for objects using the triangles shader.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CArrow.cpp:123
mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D &local) const
Rotates a vector (i.e.
Definition: CPose3D.cpp:460
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
This base provides a set of functions for maths stuff.
#define M_PIf
Definition: common.h:61
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CArrow.cpp:124
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CArrow.cpp:133
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CMatrixDouble44 generateAxisBaseFromDirectionAndAxis(const mrpt::math::TVector3D &vec, uint8_t coord)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
Definition: geometry.cpp:1993
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
std::vector< mrpt::opengl::TTriangle > m_triangles
List of triangles.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
const auto bb_max
A 3D arrow.
Definition: CArrow.h:28
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
const auto bb_min
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7ea9b7e81 Mon May 25 11:43:10 2020 +0200 at lun may 25 11:45:15 CEST 2020