MRPT  2.0.4
CCylinder.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/TLine3D.h>
13 #include <mrpt/math/geometry.h>
14 #include <mrpt/opengl/CCylinder.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::opengl;
20 using namespace mrpt::math;
21 using namespace std;
22 
24 
25 void CCylinder::onUpdateBuffers_Triangles()
26 {
28  tris.clear();
29 
30  // precomputed table:
31  ASSERT_ABOVE_(m_slices, 2);
32 
33  const float dAng = 2 * M_PIf / m_slices;
34  float a = 0;
35  // unit circle points: cos(ang),sin(ang)
36  std::vector<mrpt::math::TPoint2Df> circle(m_slices);
37  for (unsigned int i = 0; i < m_slices; i++, a += dAng)
38  {
39  circle[i].x = cos(a);
40  circle[i].y = sin(a);
41  }
42 
43  const float r0 = m_baseRadius, r1 = m_topRadius;
44 
45  const float wall_tilt = std::atan2(r0 - r1, m_height);
46  const float coswt = std::cos(wall_tilt), sinwt = std::sin(wall_tilt);
47 
48  // cylinder walls:
49  for (unsigned int i = 0; i < m_slices; i++)
50  {
51  const auto ip = (i + 1) % m_slices;
52 
53  tris.emplace_back(
54  // Points:
55  TPoint3Df(r0 * circle[i].x, r0 * circle[i].y, .0f),
56  TPoint3Df(r0 * circle[ip].x, r0 * circle[ip].y, .0f),
57  TPoint3Df(r1 * circle[i].x, r1 * circle[i].y, m_height),
58  // Normals:
59  TVector3Df(-coswt * circle[i].y, coswt * circle[i].x, sinwt),
60  TVector3Df(-coswt * circle[ip].y, coswt * circle[ip].x, sinwt),
61  TVector3Df(-coswt * circle[i].y, coswt * circle[i].x, sinwt));
62 
63  tris.emplace_back(
64  // Points:
65  TPoint3Df(r0 * circle[ip].x, r0 * circle[ip].y, .0f),
66  TPoint3Df(r1 * circle[ip].x, r1 * circle[ip].y, m_height),
67  TPoint3Df(r1 * circle[i].x, r1 * circle[i].y, m_height),
68  // Normals:
69  TVector3Df(-coswt * circle[ip].y, coswt * circle[ip].x, sinwt),
70  TVector3Df(-coswt * circle[ip].y, coswt * circle[ip].x, sinwt),
71  TVector3Df(-coswt * circle[i].y, coswt * circle[i].x, sinwt));
72  }
73 
74  // bottom & top disks:
75  for (unsigned int i = 0; i < m_slices; i++)
76  {
77  const auto ip = (i + 1) % m_slices;
78  tris.emplace_back(
79  TPoint3Df(r0 * circle[i].x, r0 * circle[i].y, .0f),
80  TPoint3Df(r0 * circle[ip].x, r0 * circle[ip].y, .0f),
81  TPoint3Df(.0f, .0f, .0f));
82 
83  tris.emplace_back(
84  TPoint3Df(r1 * circle[i].x, r1 * circle[i].y, m_height),
85  TPoint3Df(r1 * circle[ip].x, r1 * circle[ip].y, m_height),
86  TPoint3Df(.0f, .0f, m_height));
87  }
88 
89  // All faces, same color:
90  for (auto& t : tris) t.setColor(m_color);
91 }
92 
94 {
96  out["baseRadius"] = m_baseRadius;
97  out["topRadius"] = m_topRadius;
98  out["height"] = m_height;
99  out["slices"] = m_slices;
100  out["hasBottomBase"] = m_hasBottomBase;
101  out["hasTopBase"] = m_hasTopBase;
102 }
104 {
105  uint8_t version;
107  switch (version)
108  {
109  case 1:
110  {
111  m_baseRadius = static_cast<float>(in["baseRadius"]);
112  m_topRadius = static_cast<float>(in["topRadius"]);
113  m_height = static_cast<float>(in["height"]);
114  m_slices = static_cast<uint32_t>(in["slices"]);
115  m_hasBottomBase = static_cast<bool>(in["hasBottomBase"]);
116  m_hasTopBase = static_cast<bool>(in["hasTopBase"]);
117  }
118  break;
119  default:
121  }
122 }
123 uint8_t CCylinder::serializeGetVersion() const { return 1; }
125 {
126  writeToStreamRender(out);
127  // version 0
128  out << m_baseRadius << m_topRadius << m_height << m_slices
129  << m_hasBottomBase << m_hasTopBase;
130 }
132  mrpt::serialization::CArchive& in, uint8_t version)
133 {
134  switch (version)
135  {
136  case 0:
137  case 1:
138  readFromStreamRender(in);
139  in >> m_baseRadius >> m_topRadius >> m_height >> m_slices;
140 
141  if (version < 1)
142  {
143  float old_mStacks;
144  in >> old_mStacks;
145  }
146 
147  in >> m_hasBottomBase >> m_hasTopBase;
148  break;
149  default:
151  };
153 }
154 
155 bool solveEqn(double a, double b, double c, double& t)
156 { // Actually, the b from the quadratic equation is the DOUBLE of this. But
157  // this way, operations are simpler.
158  if (a < 0)
159  {
160  a = -a;
161  b = -b;
162  c = -c;
163  }
164  if (a >= mrpt::math::getEpsilon())
165  {
166  double delta = square(b) - a * c;
167  if (delta == 0)
168  return (t = -b / a) >= 0;
169  else if (delta >= 0)
170  {
171  delta = sqrt(delta);
172  if (-b - delta > 0)
173  {
174  t = (-b - delta) / a;
175  return true;
176  }
177  else if (-b + delta > 0)
178  {
179  t = (-b + delta) / a;
180  return true;
181  } // else return false; Both solutions are negative
182  } // else return false; Both solutions are complex
183  }
184  else if (std::abs(b) >= mrpt::math::getEpsilon())
185  {
186  t = -c / (b + b);
187  return t >= 0;
188  } // else return false; This actually isn't an equation
189  return false;
190 }
191 
192 bool CCylinder::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
193 {
194  TLine3D lin;
195  mrpt::math::createFromPoseX((o - this->m_pose).asTPose(), lin);
196  lin.unitarize(); // By adding this line, distance from any point of the
197 
198  const float zz = d2f(lin.pBase.z);
199 
200  // line to its base is exactly equal to the "t".
201  if (std::abs(lin.director[2]) < getEpsilon())
202  {
203  if (!reachesHeight(zz)) return false;
204  float r;
205  return getRadius(zz, r)
206  ? solveEqn(
207  square(lin.director[0]) + square(lin.director[1]),
208  lin.director[0] * lin.pBase.x +
209  lin.director[1] * lin.pBase.y,
210  square(lin.pBase.x) + square(lin.pBase.y) - square(r),
211  dist)
212  : false;
213  }
214  bool fnd = false;
215  double nDist, tZ0;
216  if (m_hasBottomBase && (tZ0 = -lin.pBase.z / lin.director[2]) > 0)
217  {
218  nDist = sqrt(
219  square(lin.pBase.x + tZ0 * lin.director[0]) +
220  square(lin.pBase.y + tZ0 * lin.director[1]));
221  if (nDist <= m_baseRadius)
222  {
223  fnd = true;
224  dist = tZ0;
225  }
226  }
227  if (m_hasTopBase)
228  {
229  tZ0 = (m_height - lin.pBase.z) / lin.director[2];
230  if (tZ0 > 0 && (!fnd || tZ0 < dist))
231  {
232  nDist = sqrt(
233  square(lin.pBase.x + tZ0 * lin.director[0]) +
234  square(lin.pBase.y + tZ0 * lin.director[1]));
235  if (nDist <= m_topRadius)
236  {
237  fnd = true;
238  dist = tZ0;
239  }
240  }
241  }
242  if (m_baseRadius == m_topRadius)
243  {
244  if (solveEqn(
245  square(lin.director[0]) + square(lin.director[1]),
246  lin.director[0] * lin.pBase.x + lin.director[1] * lin.pBase.y,
247  square(lin.pBase.x) + square(lin.pBase.y) -
248  square(m_baseRadius),
249  nDist))
250  if ((!fnd || nDist < dist) &&
251  reachesHeight(lin.pBase.z + nDist * lin.director[2]))
252  {
253  dist = nDist;
254  fnd = true;
255  }
256  }
257  else
258  {
259  double slope = (m_topRadius - m_baseRadius) / m_height;
260  if (solveEqn(
261  square(lin.director[0]) + square(lin.director[1]) -
262  square(lin.director[2] * slope),
263  lin.pBase.x * lin.director[0] + lin.pBase.y * lin.director[1] -
264  (m_baseRadius + slope * lin.pBase.z) * slope *
265  lin.director[2],
266  square(lin.pBase.x) + square(lin.pBase.y) -
267  square(m_baseRadius + slope * lin.pBase.z),
268  nDist))
269  if ((!fnd || nDist < dist) &&
270  reachesHeight(lin.pBase.z + nDist * lin.director[2]))
271  {
272  dist = nDist;
273  fnd = true;
274  }
275  }
276  return fnd;
277 }
278 
281 {
282  bb_min.x = -std::max(m_baseRadius, m_topRadius);
283  bb_min.y = bb_min.x;
284  bb_min.z = 0;
285 
286  bb_max.x = std::max(m_baseRadius, m_topRadius);
287  bb_max.y = bb_max.x;
288  bb_max.z = m_height;
289 
290  // Convert to coordinates of my parent:
291  m_pose.composePoint(bb_min, bb_min);
292  m_pose.composePoint(bb_max, bb_max);
293 }
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.
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
STL namespace.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CCylinder.cpp:103
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
float d2f(const double d)
shortcut for static_cast<float>(double)
This base provides a set of functions for maths stuff.
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CCylinder.cpp:93
A cylinder or cone whose base lies in the XY plane.
Definition: CCylinder.h:29
#define M_PIf
Definition: common.h:61
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: CCylinder.cpp:279
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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
std::vector< mrpt::opengl::TTriangle > m_triangles
List of triangles.
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
void createFromPoseX(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
Definition: geometry.cpp:917
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void unitarize()
Unitarize director vector.
Definition: TLine3D.cpp:56
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Simulation of ray-trace, given a pose.
Definition: CCylinder.cpp:192
bool solveEqn(double a, double b, double c, double &t)
Definition: CCylinder.cpp:155
#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
const auto bb_min
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CCylinder.cpp:123
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020