void CParameterizedTrajectoryGenerator::renderPathAsSimpleLine(
const uint16_t k,
mrpt::opengl::CSetOfLines &gl_obj,
const float decimate_distance,
const float max_path_distance) const
{
const size_t nPointsInPath = getPointsCountInCPath_k(k);
// Decimate trajectories: we don't need centimeter resolution!
float last_added_dist = 0.0f;
for (size_t n=0;n<nPointsInPath;n++)
{
const float d = GetCPathPoint_d(k, n); // distance thru path "k" until timestep "n"
if (d<last_added_dist+decimate_distance && n!=0)
continue; // skip: decimation
last_added_dist = d;
const float x = GetCPathPoint_x(k, n);
const float y = GetCPathPoint_y(k, n);
if (gl_obj.empty())
gl_obj.appendLine(0,0,0, x,y,0);
else gl_obj.appendLineStrip(x,y,0);
// Draw the TP only until we reach the target of the "motion" segment:
if (max_path_distance!=0.0f && d>=max_path_distance) break;
}
}
void CParameterizedTrajectoryGenerator::renderPathAsSimpleLine(
const uint16_t k,
mrpt::opengl::CSetOfLines &gl_obj,
const float decimate_distance,
const float max_path_distance) const
{
const size_t nPointsInPath = getPathStepCount(k);
bool first=true;
// Decimate trajectories: we don't need centimeter resolution!
float last_added_dist = 0.0f;
for (size_t n=0;n<nPointsInPath;n++)
{
const float d = this->getPathDist(k, n); // distance thru path "k" until timestep "n"
if (d<last_added_dist+decimate_distance && n!=0)
continue; // skip: decimation
last_added_dist = d;
mrpt::math::TPose2D p;
this->getPathPose(k, n, p);
if (first) {
first=false;
gl_obj.appendLine(0,0,0, p.x, p.y,0);
}
else gl_obj.appendLineStrip(p.x, p.y,0);
// Draw the TP only until we reach the target of the "motion" segment:
if (max_path_distance!=0.0f && d>=max_path_distance) break;
}
}
void CPTG_RobotShape_Polygonal::add_robotShape_to_setOfLines(
mrpt::opengl::CSetOfLines& gl_shape,
const mrpt::poses::CPose2D& origin) const
{
const int N = m_robotShape.size();
if (N >= 2)
{
// Transform coordinates:
mrpt::math::CVectorDouble shap_x(N), shap_y(N), shap_z(N);
for (int i = 0; i < N; i++)
{
origin.composePoint(
m_robotShape[i].x, m_robotShape[i].y, 0, shap_x[i], shap_y[i],
shap_z[i]);
}
gl_shape.appendLine(
shap_x[0], shap_y[0], shap_z[0], shap_x[1], shap_y[1], shap_z[1]);
for (int i = 0; i <= shap_x.size(); i++)
{
const int idx = i % shap_x.size();
gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
}
}
}
void CPTG_RobotShape_Circular::add_robotShape_to_setOfLines(
mrpt::opengl::CSetOfLines &gl_shape,
const mrpt::poses::CPose2D &origin) const
{
const double R = m_robotRadius;
const int N = 21;
// Transform coordinates:
mrpt::math::CVectorDouble shap_x(N), shap_y(N),shap_z(N);
for (int i=0;i<N;i++) {
origin.composePoint(
R*cos(i*2*M_PI/(N-1)),R*sin(i*2*M_PI/(N-1)), 0,
shap_x[i], shap_y[i], shap_z[i]);
}
// Draw a "radius" to identify the "forward" orientation (phi=0)
gl_shape.appendLine( origin.x(), origin.y(), .0, shap_x[0],shap_y[0],shap_z[0] );
for (int i=1;i<=shap_x.size();i++) {
const int idx = i % shap_x.size();
gl_shape.appendLineStrip( shap_x[idx],shap_y[idx], shap_z[idx]);
}
}
// Aux function
void add_robotShape_to_setOfLines(
const CVectorFloat &shap_x_,
const CVectorFloat &shap_y_,
mrpt::opengl::CSetOfLines &gl_shape,
const mrpt::poses::CPose2D &origin = mrpt::poses::CPose2D () )
{
const int N = shap_x_.size();
if (N>=2 && N==shap_y_.size() )
{
// Transform coordinates:
CVectorDouble shap_x(N), shap_y(N),shap_z(N);
for (int i=0;i<N;i++) {
origin.composePoint(
shap_x_[i], shap_y_[i], 0,
shap_x[i], shap_y[i], shap_z[i]);
}
gl_shape.appendLine( shap_x[0], shap_y[0], shap_z[0], shap_x[1],shap_y[1],shap_z[1] );
for (int i=0;i<=shap_x.size();i++) {
const int idx = i % shap_x.size();
gl_shape.appendLineStrip( shap_x[idx],shap_y[idx], shap_z[idx]);
}
}
}
