/*---------------------------------------------------------------
render
---------------------------------------------------------------*/
void CEllipsoid::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
MRPT_START
const size_t dim = m_cov.getColCount();
if(m_eigVal(0,0) != 0.0 && m_eigVal(1,1) != 0.0 && (dim==2 || m_eigVal(2,2) != 0.0) && m_quantiles!=0.0)
{
glEnable(GL_BLEND);
checkOpenGLError();
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
checkOpenGLError();
glLineWidth(m_lineWidth);
checkOpenGLError();
if (dim==2)
{
glDisable(GL_LIGHTING); // Disable lights when drawing lines
// ---------------------
// 2D ellipse
// ---------------------
/* Equivalent MATLAB code:
*
* q=1;
* [vec val]=eig(C);
* M=(q*val*vec)';
* R=M*[x;y];
* xx=R(1,:);yy=R(2,:);
* plot(xx,yy), axis equal;
*/
double ang;
unsigned int i;
// Compute the new vectors for the ellipsoid:
CMatrixDouble M;
M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint();
glBegin( GL_LINE_LOOP );
// Compute the points of the 2D ellipse:
for (i=0,ang=0;i<m_2D_segments;i++,ang+= (M_2PI/m_2D_segments))
{
double ccos = cos(ang);
double ssin = sin(ang);
const float x = ccos * M.get_unsafe(0,0) + ssin * M.get_unsafe(1,0);
const float y = ccos * M.get_unsafe(0,1) + ssin * M.get_unsafe(1,1);
glVertex2f( x,y );
} // end for points on ellipse
glEnd();
// 2D: Save bounding box:
const double max_radius = m_quantiles * std::max( m_eigVal(0,0), m_eigVal(1,1) );
m_bb_min = mrpt::math::TPoint3D(-max_radius,-max_radius, 0);
m_bb_max = mrpt::math::TPoint3D(max_radius,max_radius, 0);
// Convert to coordinates of my parent:
m_pose.composePoint(m_bb_min, m_bb_min);
m_pose.composePoint(m_bb_max, m_bb_max);
glEnable(GL_LIGHTING);
}
else
{
// ---------------------
// 3D ellipsoid
// ---------------------
GLfloat mat[16];
// A homogeneous transformation matrix, in this order:
//
// 0 4 8 12
// 1 5 9 13
// 2 6 10 14
// 3 7 11 15
//
mat[3] = mat[7] = mat[11] = 0;
mat[15] = 1;
mat[12] = mat[13] = mat[14] = 0;
mat[0] = m_eigVec(0,0); mat[1] = m_eigVec(1,0); mat[2] = m_eigVec(2,0); // New X-axis
mat[4] = m_eigVec(0,1); mat[5] = m_eigVec(1,1); mat[6] = m_eigVec(2,1); // New X-axis
mat[8] = m_eigVec(0,2); mat[9] = m_eigVec(1,2); mat[10] = m_eigVec(2,2); // New X-axis
GLUquadricObj *obj = gluNewQuadric();
checkOpenGLError();
if (!m_drawSolid3D) glDisable(GL_LIGHTING); // Disable lights when drawing lines
gluQuadricDrawStyle( obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
glPushMatrix();
glMultMatrixf( mat );
glScalef(m_eigVal(0,0)*m_quantiles,m_eigVal(1,1)*m_quantiles,m_eigVal(2,2)*m_quantiles);
gluSphere( obj, 1,m_3D_segments,m_3D_segments);
checkOpenGLError();
glPopMatrix();
gluDeleteQuadric(obj);
checkOpenGLError();
// 3D: Save bounding box:
const double max_radius = m_quantiles * std::max( m_eigVal(0,0), std::max(m_eigVal(1,1), m_eigVal(2,2) ) );
m_bb_min = mrpt::math::TPoint3D(-max_radius,-max_radius, 0);
m_bb_max = mrpt::math::TPoint3D(max_radius,max_radius, 0);
// Convert to coordinates of my parent:
m_pose.composePoint(m_bb_min, m_bb_min);
m_pose.composePoint(m_bb_max, m_bb_max);
}
glDisable(GL_BLEND);
glEnable(GL_LIGHTING);
}
MRPT_END_WITH_CLEAN_UP( \
cout << "Covariance matrix leading to error is:" << endl << m_cov << endl; \
);
/*---------------------------------------------------------------
render
---------------------------------------------------------------*/
void CEllipsoid::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
MRPT_START
const size_t dim = m_cov.getColCount();
if(m_eigVal(0,0) != 0.0 && m_eigVal(1,1) != 0.0 && (dim==2 || m_eigVal(2,2) != 0.0) && m_quantiles!=0.0)
{
glEnable(GL_BLEND);
checkOpenGLError();
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
checkOpenGLError();
glLineWidth(m_lineWidth);
checkOpenGLError();
if (dim==2)
{
// ---------------------
// 2D ellipse
// ---------------------
float x1=0,y1=0,x2=0,y2=0;
double ang;
unsigned int i;
// Compute the new vectors for the ellipsoid:
CMatrixDouble M;
M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint();
glBegin( GL_LINES );
// Compute the points of the 2D ellipse:
for (i=0,ang=0; i<m_2D_segments; i++,ang+= (M_2PI/(m_2D_segments-1)))
{
double ccos = cos(ang);
double ssin = sin(ang);
x2 = ccos * M.get_unsafe(0,0) + ssin * M.get_unsafe(1,0);
y2 = ccos * M.get_unsafe(0,1) + ssin * M.get_unsafe(1,1);
if (i>0)
{
glVertex2f( x1,y1 );
glVertex2f( x2,y2 );
}
x1 = x2;
y1 = y2;
} // end for points on ellipse
glEnd();
}
else
{
// ---------------------
// 3D ellipsoid
// ---------------------
GLfloat mat[16];
// A homogeneous transformation matrix, in this order:
//
// 0 4 8 12
// 1 5 9 13
// 2 6 10 14
// 3 7 11 15
//
mat[3] = mat[7] = mat[11] = 0;
mat[15] = 1;
mat[12] = mat[13] = mat[14] = 0;
mat[0] = m_eigVec(0,0);
mat[1] = m_eigVec(1,0);
mat[2] = m_eigVec(2,0); // New X-axis
mat[4] = m_eigVec(0,1);
mat[5] = m_eigVec(1,1);
mat[6] = m_eigVec(2,1); // New X-axis
mat[8] = m_eigVec(0,2);
mat[9] = m_eigVec(1,2);
mat[10] = m_eigVec(2,2); // New X-axis
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_COLOR_MATERIAL);
glShadeModel(GL_SMOOTH);
GLUquadricObj *obj = gluNewQuadric();
checkOpenGLError();
gluQuadricDrawStyle( obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
glPushMatrix();
glMultMatrixf( mat );
glScalef(m_eigVal(0,0)*m_quantiles,m_eigVal(1,1)*m_quantiles,m_eigVal(2,2)*m_quantiles);
gluSphere( obj, 1,m_3D_segments,m_3D_segments);
checkOpenGLError();
glPopMatrix();
gluDeleteQuadric(obj);
checkOpenGLError();
glDisable(GL_LIGHTING);
glDisable(GL_LIGHT0);
}
glLineWidth(1.0f);
glDisable(GL_BLEND);
}
MRPT_END_WITH_CLEAN_UP( \
cout << "Covariance matrix leading to error is:" << endl << m_cov << endl; \
);
