30 void CGeneralizedCylinder::TQuadrilateral::calculateNormal()
38 normal[0] = az *
by - ay *
bz;
39 normal[1] = ax *
bz - az * bx;
40 normal[2] = ay * bx - ax *
by;
42 for (
size_t i = 0; i < 3; i++)
s += normal[i] * normal[i];
44 for (
size_t i = 0; i < 3; i++) normal[i] /=
s;
47 #if MRPT_HAS_OPENGL_GLUT 48 class FQuadrilateralRenderer
57 for (
int i = 0; i < 4; i++)
61 ~FQuadrilateralRenderer() {}
65 void CGeneralizedCylinder::getMeshIterators(
66 const vector<TQuadrilateral>& m,
78 m.size() / getNumberOfSections();
79 begin = m.begin() + qps * firstSection;
80 end = m.begin() + qps * lastSection;
84 void CGeneralizedCylinder::render_dl()
const 86 #if MRPT_HAS_OPENGL_GLUT 87 if (!meshUpToDate) updateMesh();
93 glColor4ub(m_color.R, m_color.G, m_color.B, m_color.A);
96 for_each(
begin,
end, FQuadrilateralRenderer(m_color));
105 vector<CGeneralizedCylinder::TQuadrilateral>& mesh)
108 for (
size_t i = 0; i <
R; i++)
109 for (
size_t j = 0; j < C; j++)
112 pointsMesh(i, j), pointsMesh(i, j + 1),
113 pointsMesh(i + 1, j + 1), pointsMesh(i + 1, j)));
130 if (!meshUpToDate || !polysUpToDate) updatePolys();
134 void CGeneralizedCylinder::updateMesh()
const 136 CRenderizableDisplayList::notifyChange();
138 size_t A = axis.size();
139 vector<TPoint3D> genX = generatrix;
140 if (closed && genX.size() > 2) genX.push_back(genX[0]);
141 size_t G = genX.size();
146 for (
size_t i = 0; i < A; i++)
147 for (
size_t j = 0; j < G; j++)
148 axis[i].composePoint(genX[j], pointsMesh.get_unsafe(i, j));
152 polysUpToDate =
false;
155 void CGeneralizedCylinder::writeToStream(
162 writeToStreamRender(out);
163 out << axis << generatrix;
175 readFromStreamRender(
in);
177 in >>
a >> generatrix;
178 generatePoses(
a, axis);
179 meshUpToDate =
false;
180 polysUpToDate =
false;
184 readFromStreamRender(
in);
186 in >> axis >> generatrix;
187 meshUpToDate =
false;
188 polysUpToDate =
false;
193 CRenderizableDisplayList::notifyChange();
201 for (
size_t i = 0; i < profile.size(); i++)
203 profile[i].x, profile[i].y, profile[i].z,
p[i].x,
p[i].y,
p[i].z);
204 vector<math::TPolygon3D> convexPolys;
206 convexPolys.push_back(
p);
207 poly = mrpt::make_aligned_shared<CPolyhedron>(convexPolys);
212 if (!meshUpToDate) updateMesh();
213 if (axis.size() < 2 || generatrix.size() < 3)
214 throw std::logic_error(
"Not enough points.");
215 size_t i = fullyVisible ? 0 : firstSection;
217 poly->setPose(this->m_pose);
218 poly->setColor(getColor());
223 if (!meshUpToDate) updateMesh();
224 if (axis.size() < 2 || generatrix.size() < 3)
225 throw std::logic_error(
"Not enough points.");
226 size_t i = (fullyVisible ? axis.size() : lastSection) - 1;
228 poly->setPose(this->m_pose);
229 poly->setColor(getColor());
232 void CGeneralizedCylinder::generateSetOfPolygons(
233 std::vector<TPolygon3D>&
res)
const 235 if (!meshUpToDate || !polysUpToDate) updatePolys();
236 size_t N = polys.size();
238 for (
size_t i = 0; i < N; i++)
res[i] = polys[i].poly;
241 void CGeneralizedCylinder::getClosedSection(
244 if (index1 > index2) swap(index1, index2);
245 if (index2 >= axis.size() - 1)
throw std::logic_error(
"Out of range");
247 if (!meshUpToDate) updateMesh();
248 pointsMesh.extractRows(index1, index2 + 1, ROIpoints);
254 vector<TPoint3D> vec;
258 vector<TPoint3D> vertices;
262 vector<vector<uint32_t>> faces;
263 faces.reserve(nr * nc + 2);
264 vector<uint32_t> tmp(4);
265 for (
size_t i = 0; i < nr; i++)
266 for (
size_t j = 0; j < nc; j++)
268 size_t base = (nc + 1) * i + j;
271 tmp[2] = base + nc + 2;
272 tmp[3] = base + nc + 1;
273 faces.push_back(tmp);
276 for (
size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 1);
277 faces.push_back(tmp);
278 for (
size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 2) - 1;
279 poly = mrpt::make_aligned_shared<CPolyhedron>(vertices, faces);
282 void CGeneralizedCylinder::removeVisibleSectionAtStart()
284 CRenderizableDisplayList::notifyChange();
287 if (!getNumberOfSections())
throw std::logic_error(
"No more sections");
288 fullyVisible =
false;
290 lastSection = getNumberOfSections();
292 else if (firstSection >= lastSection)
293 throw std::logic_error(
"No more sections");
297 void CGeneralizedCylinder::removeVisibleSectionAtEnd()
299 CRenderizableDisplayList::notifyChange();
302 if (!getNumberOfSections())
throw std::logic_error(
"No more sections");
303 fullyVisible =
false;
305 lastSection = getNumberOfSections() - 1;
307 else if (firstSection >= lastSection)
308 throw std::logic_error(
"No more sections");
313 void CGeneralizedCylinder::updatePolys()
const 315 CRenderizableDisplayList::notifyChange();
317 if (!meshUpToDate) updateMesh();
318 size_t N = mesh.size();
321 for (
size_t i = 0; i < N; i++)
323 for (
size_t j = 0; j < 4; j++) tmp[j] = mesh[i].
points[j];
326 polysUpToDate =
true;
329 void CGeneralizedCylinder::generatePoses(
330 const vector<TPoint3D>& pIn,
333 size_t N = pIn.size();
343 if ((it2 = it1 + 1) == pIn.end())
347 yaws.push_back(atan2(it2->y - it1->y, it2->x - it1->x));
350 yaws.push_back(*yaws.rbegin());
352 for (
size_t i = 0; i < N; i++)
355 pOut[i] =
CPose3D(
p.x,
p.y,
p.z, yaws[i], 0, 0);
359 bool CGeneralizedCylinder::getFirstSectionPose(
CPose3D&
p)
361 if (axis.size() <= 0)
return false;
366 bool CGeneralizedCylinder::getLastSectionPose(
CPose3D&
p)
368 if (axis.size() <= 0)
return false;
373 bool CGeneralizedCylinder::getFirstVisibleSectionPose(
CPose3D&
p)
375 if (fullyVisible)
return getFirstSectionPose(
p);
376 if (getVisibleSections() <= 0)
return false;
377 p = axis[firstSection];
381 bool CGeneralizedCylinder::getLastVisibleSectionPose(
CPose3D&
p)
383 if (fullyVisible)
return getLastSectionPose(
p);
384 if (getVisibleSections() <= 0)
return false;
385 p = axis[lastSection];
389 void CGeneralizedCylinder::getBoundingBox(
396 m_pose.composePoint(bb_min, bb_min);
397 m_pose.composePoint(bb_max, bb_max);
void extractCol(size_t nCol, std::vector< T > &out, int startingRow=0) const
Returns a given column to a vector (without modifying the matrix)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
GLAPI void GLAPIENTRY glEnable(GLenum cap)
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
EIGEN_STRONG_INLINE iterator begin()
std::shared_ptr< CPolyhedron > Ptr
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
GLAPI void GLAPIENTRY glVertex3d(GLdouble x, GLdouble y, GLdouble z)
const Scalar * const_iterator
#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.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
This base provides a set of functions for maths stuff.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
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...
void getAsVector(std::vector< T > &out) const
Returns a vector containing the matrix's values.
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
This template class provides the basic functionality for a general 2D any-size, resizable container o...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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 appendCol(const std::vector< T > &in)
Appends a new column to the matrix from a vector.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< 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...
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
The namespace for 3D scene representation and rendering.
size_t getColCount() const
Number of columns in the matrix.
GLAPI void GLAPIENTRY glEnd(void)
size_t getRowCount() const
Number of rows in the matrix.
Auxiliary struct holding any quadrilateral, represented by foour points.
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLubyte GLubyte GLubyte a
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
3D polygon, inheriting from std::vector<TPoint3D>
void createMesh(const CMatrixTemplate< TPoint3D > &pointsMesh, size_t R, size_t C, vector< CGeneralizedCylinder::TQuadrilateral > &mesh)