qglv_opengl: arrow.hpp Source File

Go to the documentation of this file.
00001 
00004 /*****************************************************************************
00005 ** Ifdefs
00006 *****************************************************************************/
00007 
00008 #ifndef qglv_opengl_ARROW_HPP_
00009 #define qglv_opengl_ARROW_HPP_
00010 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include <sophus/se3.hpp>
00016 
00017 #include "../gl/headers.hpp"
00018 #include "../gl/helpers.hpp"
00019 
00020 /*****************************************************************************
00021 ** Namespaces
00022 *****************************************************************************/
00023 
00024 namespace qglv {
00025 
00026 /*****************************************************************************
00027 ** Interfaces
00028 *****************************************************************************/
00029 
00039 template<typename T>
00040 void arrow( const Sophus::SE3Group<T> & pose, const float &length, const float &radius = 1.0 )
00041 {
00042   static const int subdivisions = 12;
00043   static GLUquadric* quadric = gluNewQuadric();
00044 
00045   glPushMatrix();
00046   const Eigen::Matrix<T,3,1> & center = pose.translation();
00047   qglv::translate(center);
00048   Eigen::Matrix<T,3,1> axis_angle = pose.so3().log();
00049   double angle = axis_angle.norm();
00050   if(angle != 0.)
00051   {
00052     glRotatef(angle*180.0/M_PI,  // radians to degrees
00053                axis_angle[0],
00054                axis_angle[1],
00055                axis_angle[2]);
00056   }
00057   // cylinder below points in the z direction, but we want it in the x direction, so rotate accordingly
00058   glRotatef(90.0f, 0.0, 1.0, 0.0);
00059   float used_radius = radius;
00060   if (radius < 0.0) {
00061     used_radius = 0.05 * length;
00062   }
00063   // DJS : I don't really understand how this code generates an arrow, it's a bit dodgy
00064   // and have had bugs with it when the length is small
00065   // this should never be >= 0.8 as it will cause coneRadiusCoef to become zero/negative
00066   const double head = std::min(2.5*(used_radius / length) + 0.1, 0.6);
00067   const double coneRadiusCoef = 4.0 - 5.0 * head;
00068   gluCylinder(quadric, used_radius, used_radius, length * (1.0 - head/coneRadiusCoef), subdivisions, 1);
00069   glTranslatef(0.0f, 0.0f, float(length * (1.0 - head)));
00070   gluCylinder(quadric, coneRadiusCoef * used_radius, 0.0, head * length, subdivisions, 1);
00071 //  glTranslatef(float(-length * (1.0 - head)), 0.0f, 0.0f);
00072   glPopMatrix();
00073 }
00074 
00075 /*****************************************************************************
00076 ** Trailers
00077 *****************************************************************************/
00078 
00079 } // namespace qglv
00080 
00081 #endif /* qglv_opengl_ARROW_HPP_ */