MRPT  2.0.4
CDisk.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/opengl/CDisk.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::opengl;
17 using namespace std;
19 
21 
22 void CDisk::onUpdateBuffers_Triangles()
23 {
25 
27  tris.clear();
28 
29  // precomputed table:
30  ASSERT_ABOVE_(m_nSlices, 2);
31 
32  const float dAng = 2 * M_PIf / m_nSlices;
33  float a = 0;
34  // unit circle points: cos(ang),sin(ang)
35  std::vector<mrpt::math::TPoint2Df> circle(m_nSlices);
36  for (unsigned int i = 0; i < m_nSlices; i++, a += dAng)
37  {
38  circle[i].x = cos(a);
39  circle[i].y = sin(a);
40  }
41 
42  const float r0 = m_radiusIn, r1 = m_radiusOut;
43 
44  if (std::abs(r0) < 1e-6f)
45  {
46  // a filled disk:
47  for (unsigned int i = 0; i < m_nSlices; i++)
48  {
49  const auto ip = (i + 1) % m_nSlices;
50  tris.emplace_back(
51  TPoint3Df(r1 * circle[i].x, r1 * circle[i].y, .0f),
52  TPoint3Df(r1 * circle[ip].x, r1 * circle[ip].y, .0f),
53  TPoint3Df(.0f, .0f, .0f));
54  }
55  }
56  else
57  {
58  // A ring:
59  for (unsigned int i = 0; i < m_nSlices; i++)
60  {
61  const auto ip = (i + 1) % m_nSlices;
62  tris.emplace_back(
63  TPoint3Df(r1 * circle[i].x, r1 * circle[i].y, .0f),
64  TPoint3Df(r1 * circle[ip].x, r1 * circle[ip].y, .0f),
65  TPoint3Df(r0 * circle[i].x, r0 * circle[i].y, .0f));
66 
67  tris.emplace_back(
68  TPoint3Df(r1 * circle[ip].x, r1 * circle[ip].y, .0f),
69  TPoint3Df(r0 * circle[ip].x, r0 * circle[ip].y, .0f),
70  TPoint3Df(r0 * circle[i].x, r0 * circle[i].y, .0f));
71  }
72  }
73 
74  // All faces, same color:
75  for (auto& t : tris) t.setColor(m_color);
76 }
77 
78 uint8_t CDisk::serializeGetVersion() const { return 1; }
80 {
81  writeToStreamRender(out);
82  out << m_radiusIn << m_radiusOut;
83  out << m_nSlices;
84 }
85 
87 {
88  switch (version)
89  {
90  case 0:
91  case 1:
92  {
93  readFromStreamRender(in);
94  in >> m_radiusIn >> m_radiusOut;
95  in >> m_nSlices;
96  if (version < 1)
97  {
98  float dummy_loops;
99  in >> dummy_loops;
100  }
101  }
102  break;
103  default:
105  };
107 }
108 
109 bool CDisk::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
110 {
111  // The disk is contained initially in a plane which contains (0,0,0),
112  // (1,0,0) and (0,1,0)
113  // These points are converted into:
114  //(x,y,z)
115  //( cos(w)*cos(p)+x, sin(w)*cos(p)*y, -sin(p)+z )
116  //( -sin(w)*cos(r)+cos(w)*sin(p)*sin(r)+x,
117  // cos(w)*cos(r)+sin(w)*sin(p)*sin(r)+y, cos(p)*sin(r)*z )
118  CPose3D transf = this->m_pose - o;
119  double x = transf.x(), y = transf.y(), z = transf.z(), w = transf.yaw(),
120  p = transf.pitch(), r = transf.roll();
121  double coef = sin(w) * sin(r) + cos(w) * sin(p) * cos(r);
122  // coef is the first component of the normal to the transformed Z plane. So,
123  // the scalar product between
124  // this normal and (1,0,0) (which happens to be the beam's vector) equals
125  // coef. And if it's 0, then both
126  // are orthogonal, that is, the beam is parallel to the plane.
127  if (coef == 0) return false;
128  // The following expression yields the collision point between the plane and
129  // the beam (the y and z
130  // coordinates are zero).
131  dist = x + (y * (sin(p) * sin(w) * cos(r) - cos(w) * sin(r)) +
132  z * cos(p) * cos(r)) /
133  coef;
134  if (dist < 0) return false;
135  // Euclidean distance is invariant to rotations...
136  double d2 = (x - dist) * (x - dist) + y * y + z * z;
137  return d2 >= (m_radiusIn * m_radiusIn) && d2 <= (m_radiusOut * m_radiusOut);
138 
139  // IMPORTANT NOTICE: using geometric intersection between Z plane and
140  // CPose's line intersection is SLOWER than the used method.
141 }
142 
145 {
146  bb_min.x = -std::max(m_radiusIn, m_radiusOut);
147  bb_min.y = bb_min.x;
148  bb_min.z = 0;
149 
150  bb_max.x = std::max(m_radiusIn, m_radiusOut);
151  bb_max.y = bb_max.x;
152  bb_max.z = 0;
153 
154  // Convert to coordinates of my parent:
155  m_pose.composePoint(bb_min, bb_min);
156  m_pose.composePoint(bb_max, bb_max);
157 }
#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.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CDisk.cpp:78
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: CDisk.cpp:143
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
STL namespace.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CDisk.cpp:86
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
A planar disk in the XY plane.
Definition: CDisk.h:30
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CDisk.cpp:79
#define M_PIf
Definition: common.h:61
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
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.
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
const auto bb_min
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray tracing.
Definition: CDisk.cpp:109



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 02a75dd25 Thu Jun 4 01:49:55 2020 +0200 at jue jun 4 02:00:12 CEST 2020