18 #include <Eigen/Dense> 31 void CGeneralizedCylinder::TQuadrilateral::calculateNormal()
39 normal[0] = az *
by - ay *
bz;
40 normal[1] = ax *
bz - az * bx;
41 normal[2] = ay * bx - ax *
by;
43 for (
double i : normal)
s += i * i;
45 for (
double& i : normal) i /=
s;
48 #if MRPT_HAS_OPENGL_GLUT 49 class FQuadrilateralRenderer
58 for (
const auto& point :
t.points)
62 ~FQuadrilateralRenderer() =
default;
66 void CGeneralizedCylinder::getMeshIterators(
67 const vector<TQuadrilateral>& m,
68 vector<TQuadrilateral>::const_iterator&
begin,
69 vector<TQuadrilateral>::const_iterator&
end)
const 78 ASSERT_(getNumberOfSections() > 0);
80 m.size() / getNumberOfSections();
81 begin = m.begin() + qps * firstSection;
82 end = m.begin() + qps * lastSection;
86 void CGeneralizedCylinder::render_dl()
const 88 #if MRPT_HAS_OPENGL_GLUT 89 if (!meshUpToDate) updateMesh();
95 glColor4ub(m_color.R, m_color.G, m_color.B, m_color.A);
96 vector<TQuadrilateral>::const_iterator
begin,
end;
98 for_each(
begin,
end, FQuadrilateralRenderer(m_color));
107 vector<CGeneralizedCylinder::TQuadrilateral>& mesh)
110 for (
size_t i = 0; i <
R; i++)
111 for (
size_t j = 0; j < C; j++)
113 pointsMesh(i, j), pointsMesh(i, j + 1),
114 pointsMesh(i + 1, j + 1), pointsMesh(i + 1, j));
119 if (!meshUpToDate || !polysUpToDate) updatePolys();
123 void CGeneralizedCylinder::updateMesh()
const 125 CRenderizableDisplayList::notifyChange();
127 size_t A = axis.size();
128 vector<TPoint3D> genX = generatrix;
129 if (closed && genX.size() > 2) genX.push_back(genX[0]);
130 size_t G = genX.size();
135 for (
size_t i = 0; i <
A; i++)
136 for (
size_t j = 0; j <
G; j++)
137 pointsMesh(i, j) = axis[i].composePoint(genX[j]);
141 polysUpToDate =
false;
144 uint8_t CGeneralizedCylinder::serializeGetVersion()
const {
return 1; }
147 writeToStreamRender(out);
148 out << axis << generatrix;
152 void CGeneralizedCylinder::serializeFrom(
159 readFromStreamRender(
in);
161 in >>
a >> generatrix;
162 generatePoses(
a, axis);
163 meshUpToDate =
false;
164 polysUpToDate =
false;
168 readFromStreamRender(
in);
170 in >> axis >> generatrix;
171 meshUpToDate =
false;
172 polysUpToDate =
false;
177 CRenderizableDisplayList::notifyChange();
185 for (
size_t i = 0; i < profile.size(); i++)
187 profile[i].x, profile[i].y, profile[i].z,
p[i].x,
p[i].y,
p[i].z);
188 vector<math::TPolygon3D> convexPolys;
190 convexPolys.push_back(
p);
191 poly = std::make_shared<CPolyhedron>(convexPolys);
196 if (!meshUpToDate) updateMesh();
197 if (axis.size() < 2 || generatrix.size() < 3)
198 throw std::logic_error(
"Not enough points.");
199 size_t i = fullyVisible ? 0 : firstSection;
201 poly->setPose(this->m_pose);
202 poly->setColor(getColor());
207 if (!meshUpToDate) updateMesh();
208 if (axis.size() < 2 || generatrix.size() < 3)
209 throw std::logic_error(
"Not enough points.");
210 size_t i = (fullyVisible ? axis.size() : lastSection) - 1;
212 poly->setPose(this->m_pose);
213 poly->setColor(getColor());
216 void CGeneralizedCylinder::generateSetOfPolygons(
217 std::vector<TPolygon3D>&
res)
const 219 if (!meshUpToDate || !polysUpToDate) updatePolys();
220 size_t N = polys.size();
222 for (
size_t i = 0; i < N; i++)
res[i] = polys[i].poly;
225 void CGeneralizedCylinder::getClosedSection(
228 if (index1 > index2) swap(index1, index2);
229 if (index2 >= axis.size() - 1)
throw std::logic_error(
"Out of range");
230 if (!meshUpToDate) updateMesh();
232 pointsMesh.asEigen().block(index1, 0, index2 + 1, pointsMesh.cols()));
240 vec.
asEigen() = ROIpoints.col(0);
241 ROIpoints.appendCol(vec);
243 vector<TPoint3D> vertices;
244 ROIpoints.asVector(vertices);
245 size_t nr = ROIpoints.rows() - 1;
246 size_t nc = ROIpoints.cols() - 1;
247 vector<vector<uint32_t>> faces;
248 faces.reserve(nr * nc + 2);
249 vector<uint32_t> tmp(4);
250 for (
size_t i = 0; i < nr; i++)
251 for (
size_t j = 0; j < nc; j++)
253 size_t base = (nc + 1) * i + j;
256 tmp[2] = base + nc + 2;
257 tmp[3] = base + nc + 1;
258 faces.push_back(tmp);
261 for (
size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 1);
262 faces.push_back(tmp);
263 for (
size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 2) - 1;
264 poly = CPolyhedron::Create(vertices, faces);
267 void CGeneralizedCylinder::removeVisibleSectionAtStart()
269 CRenderizableDisplayList::notifyChange();
272 if (!getNumberOfSections())
throw std::logic_error(
"No more sections");
273 fullyVisible =
false;
275 lastSection = getNumberOfSections();
277 else if (firstSection >= lastSection)
278 throw std::logic_error(
"No more sections");
282 void CGeneralizedCylinder::removeVisibleSectionAtEnd()
284 CRenderizableDisplayList::notifyChange();
287 if (!getNumberOfSections())
throw std::logic_error(
"No more sections");
288 fullyVisible =
false;
290 lastSection = getNumberOfSections() - 1;
292 else if (firstSection >= lastSection)
293 throw std::logic_error(
"No more sections");
298 void CGeneralizedCylinder::updatePolys()
const 300 CRenderizableDisplayList::notifyChange();
302 if (!meshUpToDate) updateMesh();
303 size_t N = mesh.size();
306 for (
size_t i = 0; i < N; i++)
308 for (
size_t j = 0; j < 4; j++) tmp[j] = mesh[i].
points[j];
311 polysUpToDate =
true;
314 void CGeneralizedCylinder::generatePoses(
315 const vector<TPoint3D>& pIn, std::vector<mrpt::poses::CPose3D>& pOut)
317 size_t N = pIn.size();
325 vector<TPoint3D>::const_iterator it1 = pIn.begin(), it2;
327 if ((it2 = it1 + 1) == pIn.end())
331 yaws.push_back(atan2(it2->y - it1->y, it2->x - it1->x));
334 yaws.push_back(*yaws.rbegin());
336 for (
size_t i = 0; i < N; i++)
339 pOut[i] =
CPose3D(
p.x,
p.y,
p.z, yaws[i], 0, 0);
343 bool CGeneralizedCylinder::getFirstSectionPose(
CPose3D&
p)
345 if (axis.size() <= 0)
return false;
350 bool CGeneralizedCylinder::getLastSectionPose(
CPose3D&
p)
352 if (axis.size() <= 0)
return false;
357 bool CGeneralizedCylinder::getFirstVisibleSectionPose(
CPose3D&
p)
359 if (fullyVisible)
return getFirstSectionPose(
p);
360 if (getVisibleSections() <= 0)
return false;
361 p = axis[firstSection];
365 bool CGeneralizedCylinder::getLastVisibleSectionPose(
CPose3D&
p)
367 if (fullyVisible)
return getLastSectionPose(
p);
368 if (getVisibleSections() <= 0)
return false;
369 p = axis[lastSection];
373 void CGeneralizedCylinder::getBoundingBox(
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr, 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...
GLAPI void GLAPIENTRY glEnable(GLenum cap)
Template for column vectors of dynamic size, compatible with Eigen.
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
GLAPI void GLAPIENTRY glVertex3d(GLdouble x, GLdouble y, GLdouble z)
#define GL_ONE_MINUS_SRC_ALPHA
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
GLsizei const GLfloat * points
A renderizable object suitable for rendering with OpenGL's display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
#define ASSERT_(f)
Defines an assertion mechanism.
This base provides a set of functions for maths stuff.
void createMesh(const CMatrixDynamic< TPoint3D_data > &pointsMesh, size_t R, size_t C, vector< CGeneralizedCylinder::TQuadrilateral > &mesh)
GLAPI void GLAPIENTRY glNormal3d(GLdouble nx, GLdouble ny, GLdouble nz)
void generatePolygon(CPolyhedron::Ptr &poly, const vector< TPoint3D > &profile, const CPose3D &pose)
GLAPI void GLAPIENTRY glBegin(GLenum mode)
IMPLEMENTS_SERIALIZABLE(CGeneralizedCylinder, CRenderizableDisplayList, mrpt::opengl) void CGeneralizedCylinder
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const_iterator begin() const
Virtual base class for "archives": classes abstracting I/O streams.
This object represents any figure obtained by extruding any profile along a given axis...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
The namespace for 3D scene representation and rendering.
GLAPI void GLAPIENTRY glEnd(void)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Auxiliary struct holding any quadrilateral, represented by foour points.
GLAPI void GLAPIENTRY glDisable(GLenum cap)
This template class provides the basic functionality for a general 2D any-size, resizable container o...
GLubyte GLubyte GLubyte a
3D polygon, inheriting from std::vector<TPoint3D>