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 }
mrpt::math::createFromPoseX
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
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
mrpt::opengl::CRenderizable::notifyChange
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
Definition: CRenderizable.h:315
geometry.h
mrpt::math::TPoint3D_< double >
mrpt::math::TLine3D::pBase
TPoint3D pBase
Base point.
Definition: TLine3D.h:47
mrpt::serialization::CSchemeArchiveBase
Virtual base class for "schematic archives" (JSON, XML,...)
Definition: CSchemeArchiveBase.h:75
mrpt::opengl::CRenderizable
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
mrpt::opengl::CCylinder::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CCylinder.cpp:93
mrpt::opengl::CRenderizableShaderTriangles::m_triangles
std::vector< mrpt::opengl::TTriangle > m_triangles
List of triangles.
Definition: CRenderizableShaderTriangles.h:58
mrpt::opengl::CCylinder
A cylinder or cone whose base lies in the XY plane.
Definition: CCylinder.h:29
SCHEMA_DESERIALIZE_DATATYPE_VERSION
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
Definition: CSerializable.h:139
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::math::TPoint3Df
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::opengl::CCylinder::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CCylinder.cpp:123
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::opengl::CCylinder::getBoundingBox
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
bb_max
const auto bb_max
Definition: CPose3DPDFGrid_unittest.cpp:25
bb_min
const auto bb_min
Definition: CPose3DPDFGrid_unittest.cpp:23
mrpt::opengl::CCylinder::serializeFrom
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
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::math::getEpsilon
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
CSchemeArchiveBase.h
mrpt::d2f
float d2f(const double d)
shortcut for static_cast<float>(double)
Definition: core/include/mrpt/core/bits_math.h:189
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
TLine3D.h
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::math::TVector3Df
TPoint3Df TVector3Df
Definition: TPoint3D.h:274
opengl-precomp.h
mrpt::opengl::CCylinder::traceRay
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Simulation of ray-trace, given a pose.
Definition: CCylinder.cpp:192
solveEqn
bool solveEqn(double a, double b, double c, double &t)
Definition: CCylinder.cpp:155
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
CCylinder.h
mrpt::math::TLine3D
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
M_PIf
#define M_PIf
Definition: common.h:61
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::math::TLine3D::unitarize
void unitarize()
Unitarize director vector.
Definition: TLine3D.cpp:70
mrpt::math::TLine3D::director
TVector3D director
Director vector.
Definition: TLine3D.h:49
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
SCHEMA_SERIALIZE_DATATYPE_VERSION
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
Definition: CSerializable.h:131



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020