MRPT  1.9.9
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-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/TLine3D.h>
13 #include <mrpt/math/geometry.h>
14 #include <mrpt/opengl/CCylinder.h>
17 #include "opengl_internals.h"
18 
19 using namespace mrpt;
20 using namespace mrpt::opengl;
21 using namespace mrpt::math;
22 
23 using namespace std;
24 
26 
27 /*---------------------------------------------------------------
28  render
29  ---------------------------------------------------------------*/
30 void CCylinder::render_dl() const
31 {
32 #if MRPT_HAS_OPENGL_GLUT
37  GLUquadricObj* obj = gluNewQuadric();
38 
39  // This is required to draw cylinders of negative height.
40  const float absHeight = std::abs(mHeight);
41  if (mHeight < 0)
42  {
43  glPushMatrix();
44  glTranslatef(0, 0, mHeight);
45  }
46 
47  gluCylinder(obj, mBaseRadius, mTopRadius, absHeight, mSlices, mStacks);
48 
49  if (mHeight < 0) glPopMatrix();
50 
51  if (mHasBottomBase) gluDisk(obj, 0, mBaseRadius, mSlices, 1);
52  if (mHasTopBase && mTopRadius > 0)
53  {
54  glPushMatrix();
55  glTranslatef(0, 0, mHeight);
56  gluDisk(obj, 0, mTopRadius, mSlices, 1);
57  glPopMatrix();
58  }
59  gluDeleteQuadric(obj);
61 
62 #endif
63 }
65 {
67  out["baseRadius"] = mBaseRadius;
68  out["topRadius"] = mTopRadius;
69  out["height"] = mHeight;
70  out["slices"] = mSlices;
71  out["stacks"] = mStacks;
72  out["hasBottomBase"] = mHasBottomBase;
73  out["hasTopBase"] = mHasTopBase;
74 }
76 {
77  uint8_t version;
79  switch (version)
80  {
81  case 1:
82  {
83  mBaseRadius = static_cast<float>(in["baseRadius"]);
84  mTopRadius = static_cast<float>(in["topRadius"]);
85  mHeight = static_cast<float>(in["height"]);
86  mSlices = static_cast<uint32_t>(in["slices"]);
87  mStacks = static_cast<uint32_t>(in["stacks"]);
88  mHasBottomBase = static_cast<bool>(in["hasBottomBase"]);
89  mHasTopBase = static_cast<bool>(in["hasTopBase"]);
90  }
91  break;
92  default:
94  }
95 }
96 uint8_t CCylinder::serializeGetVersion() const { return 0; }
98 {
99  writeToStreamRender(out);
100  // version 0
101  out << mBaseRadius << mTopRadius << mHeight << mSlices << mStacks
102  << mHasBottomBase << mHasTopBase;
103 }
105  mrpt::serialization::CArchive& in, uint8_t version)
106 {
107  switch (version)
108  {
109  case 0:
110  readFromStreamRender(in);
111  in >> mBaseRadius >> mTopRadius >> mHeight >> mSlices >> mStacks >>
112  mHasBottomBase >> mHasTopBase;
113  break;
114  default:
116  };
118 }
119 
120 bool solveEqn(double a, double b, double c, double& t)
121 { // Actually, the b from the quadratic equation is the DOUBLE of this. But
122  // this way, operations are simpler.
123  if (a < 0)
124  {
125  a = -a;
126  b = -b;
127  c = -c;
128  }
129  if (a >= mrpt::math::getEpsilon())
130  {
131  double delta = square(b) - a * c;
132  if (delta == 0)
133  return (t = -b / a) >= 0;
134  else if (delta >= 0)
135  {
136  delta = sqrt(delta);
137  if (-b - delta > 0)
138  {
139  t = (-b - delta) / a;
140  return true;
141  }
142  else if (-b + delta > 0)
143  {
144  t = (-b + delta) / a;
145  return true;
146  } // else return false; Both solutions are negative
147  } // else return false; Both solutions are complex
148  }
149  else if (abs(b) >= mrpt::math::getEpsilon())
150  {
151  t = -c / (b + b);
152  return t >= 0;
153  } // else return false; This actually isn't an equation
154  return false;
155 }
156 
157 bool CCylinder::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
158 {
159  TLine3D lin;
160  mrpt::math::createFromPoseX((o - this->m_pose).asTPose(), lin);
161  lin.unitarize(); // By adding this line, distance from any point of the
162  // line to its base is exactly equal to the "t".
163  if (abs(lin.director[2]) < getEpsilon())
164  {
165  if (!reachesHeight(lin.pBase.z)) return false;
166  float r;
167  return getRadius(static_cast<float>(lin.pBase.z), r)
168  ? solveEqn(
169  square(lin.director[0]) + square(lin.director[1]),
170  lin.director[0] * lin.pBase.x +
171  lin.director[1] * lin.pBase.y,
172  square(lin.pBase.x) + square(lin.pBase.y) - square(r),
173  dist)
174  : false;
175  }
176  bool fnd = false;
177  double nDist, tZ0;
178  if (mHasBottomBase && (tZ0 = -lin.pBase.z / lin.director[2]) > 0)
179  {
180  nDist = sqrt(
181  square(lin.pBase.x + tZ0 * lin.director[0]) +
182  square(lin.pBase.y + tZ0 * lin.director[1]));
183  if (nDist <= mBaseRadius)
184  {
185  fnd = true;
186  dist = tZ0;
187  }
188  }
189  if (mHasTopBase)
190  {
191  tZ0 = (mHeight - lin.pBase.z) / lin.director[2];
192  if (tZ0 > 0 && (!fnd || tZ0 < dist))
193  {
194  nDist = sqrt(
195  square(lin.pBase.x + tZ0 * lin.director[0]) +
196  square(lin.pBase.y + tZ0 * lin.director[1]));
197  if (nDist <= mTopRadius)
198  {
199  fnd = true;
200  dist = tZ0;
201  }
202  }
203  }
204  if (mBaseRadius == mTopRadius)
205  {
206  if (solveEqn(
207  square(lin.director[0]) + square(lin.director[1]),
208  lin.director[0] * lin.pBase.x + lin.director[1] * lin.pBase.y,
209  square(lin.pBase.x) + square(lin.pBase.y) - square(mBaseRadius),
210  nDist))
211  if ((!fnd || nDist < dist) &&
212  reachesHeight(lin.pBase.z + nDist * lin.director[2]))
213  {
214  dist = nDist;
215  fnd = true;
216  }
217  }
218  else
219  {
220  double slope = (mTopRadius - mBaseRadius) / mHeight;
221  if (solveEqn(
222  square(lin.director[0]) + square(lin.director[1]) -
223  square(lin.director[2] * slope),
224  lin.pBase.x * lin.director[0] + lin.pBase.y * lin.director[1] -
225  (mBaseRadius + slope * lin.pBase.z) * slope *
226  lin.director[2],
227  square(lin.pBase.x) + square(lin.pBase.y) -
228  square(mBaseRadius + slope * lin.pBase.z),
229  nDist))
230  if ((!fnd || nDist < dist) &&
231  reachesHeight(lin.pBase.z + nDist * lin.director[2]))
232  {
233  dist = nDist;
234  fnd = true;
235  }
236  }
237  return fnd;
238 }
239 
242 {
243  bb_min.x = -std::max(mBaseRadius, mTopRadius);
244  bb_min.y = bb_min.x;
245  bb_min.z = 0;
246 
247  bb_max.x = std::max(mBaseRadius, mTopRadius);
248  bb_max.y = bb_max.x;
249  bb_max.z = mHeight;
250 
251  // Convert to coordinates of my parent:
252  m_pose.composePoint(bb_min, bb_min);
253  m_pose.composePoint(bb_max, bb_max);
254 }
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
GLdouble GLdouble t
Definition: glext.h:3695
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
GLAPI void GLAPIENTRY glEnable(GLenum cap)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
GLAPI void GLAPIENTRY glPopMatrix(void)
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
STL namespace.
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:288
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CCylinder.cpp:75
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
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
This base provides a set of functions for maths stuff.
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
T square(const T x)
Inline function for the square of a number.
const GLubyte * c
Definition: glext.h:6406
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CCylinder.cpp:64
A cylinder or cone whose base lies in the XY plane.
Definition: CCylinder.h:29
GLubyte GLubyte b
Definition: glext.h:6372
#define GL_BLEND
Definition: glew.h:433
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:240
void unitarize()
Unitarize director vector.
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
GLAPI void GLAPIENTRY glTranslatef(GLfloat x, GLfloat y, GLfloat z)
#define GL_SRC_ALPHA
Definition: glew.h:287
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
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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:937
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray tracing.
Definition: CCylinder.cpp:157
bool solveEqn(double a, double b, double c, double &t)
Definition: CCylinder.cpp:120
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:155
GLuint in
Definition: glext.h:7391
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
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
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CCylinder.cpp:96
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019