17 #if MRPT_HAS_OPENGL_GLUT
24 #include <OpenGL/gl.h>
31 #if MRPT_HAS_OPENGL_GLUT && defined(_WIN32)
34 #pragma comment(lib, "opengl32.lib")
35 #pragma comment(lib, "GlU32.lib")
37 #endif // MRPT_HAS_OPENGL_GLUT
59 triangles.emplace_back(std::move(t));
60 CRenderizable::notifyChange();
63 void CAngularObservationMesh::updateMesh()
const
65 CRenderizable::notifyChange();
67 size_t numRows = scanSet.size();
71 actualMesh.setSize(0, 0);
72 validityMatrix.setSize(0, 0);
76 if (pitchBounds.size() != numRows && pitchBounds.size() != 2)
return;
77 size_t numCols = scanSet[0].getScanSize();
78 actualMesh.setSize(numRows, numCols);
79 validityMatrix.setSize(numRows, numCols);
80 std::vector<double> pitchs(numRows);
81 if (pitchBounds.size() == 2)
83 double p1 = pitchBounds[0];
84 double p2 = pitchBounds[1];
85 for (
size_t i = 0; i < numRows; i++)
86 pitchs[i] = p1 + (p2 - p1) *
static_cast<double>(i) /
87 static_cast<double>(numRows - 1);
90 for (
size_t i = 0; i < numRows; i++) pitchs[i] = pitchBounds[i];
91 const bool rToL = scanSet[0].rightToLeft;
92 for (
size_t i = 0; i < numRows; i++)
94 const auto& ss_i = scanSet[i];
95 const double pitchIncr = scanSet[i].deltaPitch;
96 const double aperture = scanSet[i].aperture;
97 const CPose3D origin = scanSet[i].sensorPose;
99 for (
size_t j = 0; j < numCols; j++)
100 if ((validityMatrix(i, j) = ss_i.getScanRangeValidity(j)))
102 double pYaw = aperture * ((
static_cast<double>(j) /
103 static_cast<double>(numCols - 1)) -
108 CPose3D(0, 0, 0, rToL ? pYaw : -pYaw, pitchIncr)) +
109 CPoint3D(ss_i.getScanRange(j), 0, 0))
114 triangles.reserve(2 * (numRows - 1) * (numCols - 1));
115 for (
size_t k = 0; k < numRows - 1; k++)
117 for (
size_t j = 0; j < numCols - 1; j++)
119 int b1 = validityMatrix(k, j) ? 1 : 0;
120 int b2 = validityMatrix(k, j + 1) ? 1 : 0;
121 int b3 = validityMatrix(k + 1, j) ? 1 : 0;
122 int b4 = validityMatrix(k + 1, j + 1) ? 1 : 0;
123 switch (
b1 +
b2 +
b3 + b4)
132 actualMesh(k, j + 1), actualMesh(k + 1, j),
133 actualMesh(k + 1, j + 1));
136 actualMesh(k, j), actualMesh(k + 1, j),
137 actualMesh(k + 1, j + 1));
140 actualMesh(k, j), actualMesh(k, j + 1),
141 actualMesh(k + 1, j + 1));
144 actualMesh(k, j), actualMesh(k, j + 1),
145 actualMesh(k + 1, j));
149 actualMesh(k, j), actualMesh(k, j + 1),
150 actualMesh(k + 1, j));
152 actualMesh(k + 1, j + 1), actualMesh(k, j + 1),
153 actualMesh(k + 1, j));
164 case DefaultShaderID::TRIANGLES:
165 if (!m_Wireframe) CRenderizableShaderTriangles::render(rc);
167 case DefaultShaderID::WIREFRAME:
168 if (m_Wireframe) CRenderizableShaderWireFrame::render(rc);
172 void CAngularObservationMesh::renderUpdateBuffers()
const
174 CRenderizableShaderTriangles::renderUpdateBuffers();
175 CRenderizableShaderWireFrame::renderUpdateBuffers();
178 void CAngularObservationMesh::onUpdateBuffers_Wireframe()
180 auto& vbd = CRenderizableShaderWireFrame::m_vertex_buffer_data;
181 auto& cbd = CRenderizableShaderWireFrame::m_color_buffer_data;
185 for (
const auto& t : triangles)
188 for (
int k = 0; k <= 3; k++)
191 vbd.emplace_back(t.x(kk), t.y(kk), t.z(kk));
192 cbd.emplace_back(t.r(kk), t.g(kk), t.b(kk), t.a(kk));
197 void CAngularObservationMesh::onUpdateBuffers_Triangles()
199 auto& tris = CRenderizableShaderTriangles::m_triangles;
201 tris = this->triangles;
213 [[maybe_unused]]
double& dist)
const
219 bool CAngularObservationMesh::setScanSet(
220 const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
222 CRenderizable::notifyChange();
225 if (scans.size() > 0)
227 size_t setSize = scans[0].getScanSize();
228 bool rToL = scans[0].rightToLeft;
229 for (
auto it = scans.begin() + 1; it != scans.end(); ++it)
231 if (it->getScanSize() != setSize)
return false;
232 if (it->rightToLeft != rToL)
return false;
236 meshUpToDate =
false;
237 CRenderizable::notifyChange();
241 void CAngularObservationMesh::setPitchBounds(
242 const double initial,
const double final)
244 CRenderizable::notifyChange();
247 pitchBounds.push_back(initial);
248 pitchBounds.push_back(
final);
249 meshUpToDate =
false;
250 CRenderizable::notifyChange();
252 void CAngularObservationMesh::setPitchBounds(
const std::vector<double>& bounds)
254 CRenderizable::notifyChange();
256 pitchBounds = bounds;
257 meshUpToDate =
false;
258 CRenderizable::notifyChange();
260 void CAngularObservationMesh::getPitchBounds(
261 double& initial,
double&
final)
const
263 initial = pitchBounds.front();
264 final = pitchBounds.back();
266 void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds)
const
268 bounds = pitchBounds;
270 void CAngularObservationMesh::getScanSet(
271 std::vector<CObservation2DRangeScan>& scans)
const
276 void CAngularObservationMesh::generateSetOfTriangles(
279 if (!meshUpToDate) updateMesh();
280 res->insertTriangles(triangles.begin(), triangles.end());
293 void CAngularObservationMesh::generatePointCloud(
CPointsMap* out_map)
const
313 uint8_t CAngularObservationMesh::serializeGetVersion()
const {
return 0; }
314 void CAngularObservationMesh::serializeTo(
317 writeToStreamRender(
out);
319 out << pitchBounds << scanSet << m_Wireframe << mEnableTransparency;
322 void CAngularObservationMesh::serializeFrom(
328 readFromStreamRender(in);
329 in >> pitchBounds >> scanSet >> m_Wireframe >> mEnableTransparency;
334 meshUpToDate =
false;
335 CRenderizable::notifyChange();
338 void CAngularObservationMesh::TDoubleRange::values(
339 std::vector<double>& vals)
const
341 double value = initialValue();
342 double incr = increment();
343 size_t am = amount();
345 for (
size_t i = 0; i < am - 1; i++, value += incr) vals[i] = value;
346 vals[am - 1] = finalValue();
351 if (!meshUpToDate) updateMesh();
353 for (
int i = 0; i < validityMatrix.rows(); i++)
354 for (
int j = 0; j < validityMatrix.cols(); j++)
355 if (validityMatrix(i, j)) count++;
357 for (
int i = 0; i < actualMesh.rows(); i++)
358 for (
int j = 0; j < actualMesh.cols(); j++)
359 if (validityMatrix(i, j))
361 (scanSet[i].sensorPose).asTPose(), actualMesh(i, j));
372 : lins(l), pDist(p), pitchs()
374 pitchs.reserve(pi.size());
375 for (
auto it = pi.rbegin(); it != pi.rend(); ++it)
376 pitchs.push_back(*it);
388 ((
static_cast<double>(i) /
403 void CAngularObservationMesh::getUntracedRays(
407 scanSet.begin(), scanSet.end(),
414 for (
size_t i = 0; i < 3; i++) res[i] = t.
vertices[i].xyzrgba.pt;
418 void CAngularObservationMesh::generateSetOfTriangles(
419 std::vector<TPolygon3D>& res)
const
421 if (!meshUpToDate) updateMesh();
422 res.resize(triangles.size());
427 void CAngularObservationMesh::getBoundingBox(
430 if (!meshUpToDate) updateMesh();
433 std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
434 std::numeric_limits<double>::max());
436 -std::numeric_limits<double>::max(),
437 -std::numeric_limits<double>::max(),
438 -std::numeric_limits<double>::max());
440 for (
const auto& t : triangles)