shape_reconstruction: coordinateframe.cpp Source File

Go to the documentation of this file.
00001 /****************************************************************************
00002  * MeshLab                                                           o o     *
00003  * A versatile mesh processing toolbox                             o     o   *
00004  *                                                                _   O  _   *
00005  * Copyright(C) 2008                                                \/)\/    *
00006  * Visual Computing Lab                                            /\/|      *
00007  * ISTI - Italian National Research Council                           |      *
00008  *                                                                    \      *
00009  * All rights reserved.                                                      *
00010  *                                                                           *
00011  * This program is free software; you can redistribute it and/or modify      *
00012  * it under the terms of the GNU General Public License as published by      *
00013  * the Free Software Foundation; either version 2 of the License, or         *
00014  * (at your option) any later version.                                       *
00015  *                                                                           *
00016  * This program is distributed in the hope that it will be useful,           *
00017  * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
00018  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
00019  * GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
00020  * for more details.                                                         *
00021  *                                                                           *
00022  ****************************************************************************/
00023 /****************************************************************************
00024   History
00025 $Log: not supported by cvs2svn $
00026 Revision 1.8  2008/03/02 16:44:18  benedetti
00027 moved ActiveCoordinateFrame to its own files
00028 
00029 Revision 1.7  2008/02/26 18:22:42  benedetti
00030 corrected after quaternion/similarity/trackball changes
00031 
00032 Revision 1.6  2008/02/22 20:34:35  benedetti
00033 corrected typo
00034 
00035 Revision 1.5  2008/02/22 20:04:02  benedetti
00036 many user interface improvements, cleaned up a little
00037 
00038 Revision 1.4  2008/02/17 20:52:53  benedetti
00039 some generalization made
00040 
00041 Revision 1.3  2008/02/16 14:12:30  benedetti
00042 first version
00043 
00044 
00045 ****************************************************************************/
00046 
00047 #include <GL/glew.h>
00048 #include <wrap/gl/math.h>
00049 #include <wrap/gl/space.h>
00050 #include <wrap/gl/addons.h>
00051 #include <wrap/qt/gl_label.h>
00052 
00053 #include "coordinateframe.h"
00054 
00055 using namespace vcg;
00056 
00057 CoordinateFrame::CoordinateFrame(float s)
00058 :basecolor(Color4b::White),xcolor(Color4b::Red)
00059 ,ycolor(Color4b::Green),zcolor(Color4b::Blue),size(s),linewidth(2.0)
00060 ,font(),drawaxis(true),drawlabels(true),drawvalues(false)
00061 {
00062   font.setFamily("Helvetica");
00063 }
00064 
00065 void CoordinateFrame::Render(QGLWidget* glw,QPainter* p)
00066 {
00067   assert( glw!= NULL);
00068   glPushAttrib(GL_ALL_ATTRIB_BITS);
00069   glDisable(GL_LIGHTING);
00070   glDisable(GL_TEXTURE_2D);
00071   glEnable(GL_BLEND);
00072   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00073   glEnable(GL_LINE_SMOOTH);
00074   glEnable(GL_POINT_SMOOTH);
00075   glLineWidth(linewidth);
00076   glPointSize(linewidth*1.5);
00077   glLabel::Mode md;
00078   Point3d o(0,0,0);
00079   Point3d a(size,0,0);
00080   Point3d b(0,size,0);
00081   Point3d c(0,0,size);
00082   // Get gl state values
00083   double mm[16],mp[16];
00084   GLint vp[4];
00085   glGetDoublev(GL_MODELVIEW_MATRIX,mm);
00086   glGetDoublev(GL_PROJECTION_MATRIX,mp);
00087   glGetIntegerv(GL_VIEWPORT,vp);
00088   float slope_a=calcSlope(-a,a,2*size,10,mm,mp,vp);
00089   float slope_b=calcSlope(-b,b,2*size,10,mm,mp,vp);
00090   float slope_c=calcSlope(-c,c,2*size,10,mm,mp,vp);
00091   float scalefactor = size*0.02f;
00092   if(drawaxis){
00093     glBegin(GL_LINES);
00094       glColor(xcolor);
00095       glVertex(-a); glVertex(a);
00096       glColor(ycolor);
00097       glVertex(-b); glVertex(b);
00098       glColor(zcolor);
00099       glVertex(-c); glVertex(c);
00100     glEnd();
00101     glColor(basecolor);
00102     // positive axes
00103     drawTickedLine(o,a,size,slope_a,linewidth);  // Draws x axis
00104     drawTickedLine(o,b,size,slope_b,linewidth);  // Draws y axis
00105     drawTickedLine(o,c,size,slope_c,linewidth);  // Draws z axis
00106     // negative axes
00107     drawTickedLine(o,-a,size,slope_a,linewidth);  // Draws x axis
00108     drawTickedLine(o,-b,size,slope_b,linewidth);  // Draws y axis
00109     drawTickedLine(o,-c,size,slope_c,linewidth);  // Draws z axis
00110     glPushMatrix();
00111       glTranslate(a);
00112       glScalef(scalefactor,scalefactor,scalefactor);
00113       Add_Ons::Cone(10,linewidth*1.5,linewidth*0.5,true);
00114     glPopMatrix();
00115     glPushMatrix();
00116       glTranslate(b);
00117       glRotatef(90,0,0,1);
00118       glScalef(scalefactor,scalefactor,scalefactor);
00119       Add_Ons::Cone(10,linewidth*1.5,linewidth*0.5,true);
00120     glPopMatrix();
00121     glPushMatrix();
00122       glTranslate(c);
00123       glRotatef(-90,0,1,0);
00124       glScalef(scalefactor,scalefactor,scalefactor);
00125       Add_Ons::Cone(10,linewidth*1.5,linewidth*0.5,true);
00126     glPopMatrix();
00127   }
00128   if(drawlabels){
00129     md.qFont.setBold(true);
00130     md.qFont.setPixelSize(12);
00131     float d=size+scalefactor*linewidth*1.5;
00132     if (p) {
00133       vcg::glLabel::render(p,vcg::Point3f(d,0,0),QString("X"),md);
00134       vcg::glLabel::render(p,vcg::Point3f(0,d,0),QString("Y"),md);
00135       vcg::glLabel::render(p,vcg::Point3f(0,0,d),QString("Z"),md);
00136     }
00137   }
00138   if(drawvalues){
00139     md.qFont.setBold(false);
00140     md.qFont.setPixelSize(8);
00141     md.color=Color4b(Color4b::LightGray);
00142     float i;
00143     glColor(Color4b::LightGray);
00144     for(i=slope_a;i<size;i+=slope_a){
00145       vcg::glLabel::render(p,vcg::Point3f( i,0,0),QString(" %1").arg(i,3,'f',1),md);
00146       vcg::glLabel::render(p,vcg::Point3f(-i,0,0),QString(" %1").arg(i,3,'f',1),md);
00147     }
00148     for(i=slope_b;i<size;i+=slope_b){
00149       vcg::glLabel::render(p,vcg::Point3f(0, i,0),QString(" %1").arg(i,3,'f',1),md);
00150       vcg::glLabel::render(p,vcg::Point3f(0,-i,0),QString(" %1").arg(i,3,'f',1),md);
00151     }
00152     for(i=slope_c;i<size;i+=slope_c){
00153       vcg::glLabel::render(p,vcg::Point3f(0,0, i),QString(" %1").arg(i,3,'f',1),md);
00154       vcg::glLabel::render(p,vcg::Point3f(0,0,-i),QString(" %1").arg(i,3,'f',1),md);
00155     }
00156   }
00157   glGetError(); // Patch to buggy qt rendertext;
00158   glPopAttrib();
00159   assert(!glGetError());
00160 }
00161 
00162 void CoordinateFrame::drawTickedLine(const Point3d &a,const Point3d &b, float dim,float tickDist,float linewidth)
00163 {
00164   Point3d v(b-a);
00165   v = v /dim; // normalize without computing square roots and powers
00166 
00167   glBegin(GL_POINTS);
00168   float i;
00169   for(i=tickDist;i<dim;i+=tickDist)
00170     glVertex3f(a[0] + i*v[0],a[1] + i*v[1],a[2] + i*v[2]);
00171   glEnd();
00172 
00173   glPushAttrib(GL_POINT_BIT);
00174   glPointSize(linewidth*3);
00175   glBegin(GL_POINTS);
00176        glVertex3f(a[0] + dim*v[0],a[1] + dim*v[1],a[2] + dim*v[2]);
00177   glEnd();
00178 
00179   glPopAttrib();
00180 }
00181 
00182 float CoordinateFrame::calcSlope(const Point3d &a,const Point3d &b,float dim,int spacing,double *mm,double *mp,GLint *vp)
00183 {
00184    Point3d p1,p2;
00185 
00186   gluProject(a[0],a[1],a[2],mm,mp,vp,&p1[0],&p1[1],&p1[2]);
00187   gluProject(b[0],b[1],b[2],mm,mp,vp,&p2[0],&p2[1],&p2[2]);
00188   p1[2]=p2[2]=0;
00189 
00190   float tickNum = spacing/Distance(p2,p1);// pxl spacing
00191   float slope = dim*tickNum;
00192   float nslope = math::Min(niceRound(slope), 0.5f*niceRound(2.0f*slope), 0.2f*niceRound(5.0f*slope));
00193   nslope = std::max(niceRound(dim*.001f),nslope); // prevent too small slope
00194   return nslope;
00195 }
00196 
00197 float CoordinateFrame::niceRound(float val)
00198 {
00199   return powf(10.f,ceil(log10(val)));
00200 }
00201 
00202 MovableCoordinateFrame::MovableCoordinateFrame(float size)
00203 :CoordinateFrame(size),position(0,0,0),rotation(0,Point3f(1,0,0))
00204 {
00205   // nothing here
00206 }
00207 
00208 void MovableCoordinateFrame::Render(QGLWidget* gla, QPainter *p)
00209 {
00210   glPushMatrix();
00211 
00212   glTranslate(position);
00213   Matrix44f mrot;
00214   rotation.ToMatrix(mrot);
00215 
00216   glMultMatrix(Inverse(mrot));
00217 
00218   CoordinateFrame::Render(gla,p);
00219 
00220   glPopMatrix();
00221 }
00222 
00223 void MovableCoordinateFrame::GetTransform(Matrix44f & transform)
00224 {
00225   // build the matrix that moves points in world coordinates
00226 
00227   // clean transform
00228   transform.SetIdentity();
00229 
00230   // apply rotation
00231   Matrix44f rot;
00232   rotation.ToMatrix(rot);
00233 
00234   transform = Inverse(rot) * transform ;
00235 
00236   // apply translation
00237   Matrix44f pos;
00238   pos.SetTranslate(position);
00239 
00240   transform = pos * transform;
00241 
00242 }
00243 
00244 void MovableCoordinateFrame::Reset(bool reset_position,bool reset_alignment)
00245 {
00246   if(reset_position)
00247     position = Point3f(0,0,0);
00248   if(reset_alignment)
00249     rotation = Quaternionf(0,Point3f(1,0,0));
00250 }
00251 
00252 void MovableCoordinateFrame::SetPosition(const Point3f newpos)
00253 {
00254   position = newpos;
00255 }
00256 
00257 void MovableCoordinateFrame::SetRotation(const Quaternionf newrot)
00258 {
00259   rotation = newrot;
00260 }
00261 
00262 Point3f MovableCoordinateFrame::GetPosition()
00263 {
00264   return position;
00265 }
00266 
00267 Quaternionf MovableCoordinateFrame::GetRotation()
00268 {
00269   return rotation;
00270 }
00271 
00272 void MovableCoordinateFrame::Rot(float angle_deg,const Point3f axis)
00273 {
00274   Similarityf s;
00275     s.SetRotate(math::ToRad(angle_deg),(rotation).Rotate(axis));
00276   Move(s);
00277 }
00278 
00279 void MovableCoordinateFrame::AlignWith(const Point3f pri,const Point3f secondary,const char c1, const char c2)
00280 {
00281   const float EPSILON=1e-6f;
00282   Point3f primary=pri;
00283 
00284   if( primary.Norm() < EPSILON*size )
00285     return;
00286 
00287   primary.Normalize();
00288   Plane3f plane(0,primary); // projection plane for the second rotation
00289 
00290   Point3f x(1,0,0),y(0,1,0),z(0,0,1);
00291   Point3f first(0,0,0),second(0,0,0),third(0,0,0);
00292 
00293   if(c1=='X'){ first = x;
00294     if((c2=='Y')||(c2==' ')){ second = y; third = z; }
00295     else if(c2=='Z'){ second = z; third = y; }
00296     else assert (0);
00297   } else if(c1=='Y'){ first = y;
00298     if((c2=='Z')||(c2==' ')){ second = z; third = x; }
00299     else if(c2=='X'){ second = x; third = z; }
00300     else assert (0);
00301   } else if(c1=='Z'){ first = z;
00302     if((c2=='X')||(c2==' ')){ second = x; third = y; }
00303     else if(c2=='Y'){ second = y; third = x; }
00304     else assert (0);
00305   } else assert (0);
00306 
00307   Point3f old_first = Inverse(rotation).Rotate(first); // axis 1
00308   Point3f old_second_pro = plane.Projection(Inverse(rotation).Rotate(second)); // axis 2 projection
00309   Point3f old_third_pro = plane.Projection(Inverse(rotation).Rotate(third)); // axis 3 projection
00310 
00311   // align axis 1 to primary
00312   RotateToAlign(old_first,primary);
00313 
00314   Point3f secondary_pro = plane.Projection(secondary); // secondary's projection
00315   Point3f new_second_pro = plane.Projection(Inverse(rotation).Rotate(second)); // axis 2 projection after the first rotation
00316 
00317   if( secondary.Norm() > EPSILON*size && secondary_pro.Norm() > EPSILON ){ // secondary is not null nor parallel to primary
00318     // align axis 2 projection after the first rotation to secondary's projection
00319     secondary_pro.Normalize();
00320     RotateToAlign(new_second_pro,secondary_pro);
00321     return;
00322   }
00323 
00324   if ( old_second_pro.Norm() > EPSILON ) { // can realign axis 2
00325     // align axis 2 projection after the first rotation to old axis 2 projection
00326     old_second_pro.Normalize();
00327     RotateToAlign(new_second_pro,old_second_pro);
00328     return;
00329   }
00330 
00331   // realign axis 3
00332   Point3f new_third_pro = plane.Projection(Inverse(rotation).Rotate(third));// axis 3 projection after the first rotation
00333   assert(old_third_pro.Norm() > EPSILON ); // old axis 3 projection should not be null
00334   // align axis 3 projection after the first rotation to old axis 3 projection
00335   old_third_pro.Normalize();
00336   RotateToAlign(new_third_pro,old_third_pro);
00337 }
00338 
00339 void MovableCoordinateFrame::Move(const Similarityf track)
00340 {
00341   position = position + track.tra;
00342   rotation = rotation * Inverse(track.rot);
00343 }
00344 
00345 void MovableCoordinateFrame::RotateToAlign(const Point3f source, const Point3f dest)
00346 {
00347   const float EPSILON=1e-6f;
00348   // source and dest must be versors
00349   assert( math::Abs(source.Norm() - 1) < EPSILON);
00350   assert( math::Abs(dest.Norm() - 1) < EPSILON);
00351 
00352   Point3f axis = dest ^ source;
00353   float sinangle = axis.Norm();
00354   float cosangle = dest.dot(source);
00355   float angle = math::Atan2(sinangle,cosangle);
00356 
00357   if( math::Abs(angle) < EPSILON )
00358     return; // angle ~ 0, aborting
00359 
00360   if( math::Abs(math::Abs(angle)-M_PI) < EPSILON){
00361     // must find a axis to flip on
00362     Plane3f plane(0,source);
00363     axis=plane.Projection(Point3f(1,0,0)); // project a "random" point on source's normal plane
00364     if(axis.Norm() < EPSILON){ // source was ~ [1,0,0]...
00365       axis=plane.Projection(Point3f(0,1,0));
00366       assert(axis.Norm() > EPSILON); // this point must be good
00367     }
00368   }
00369   rotation = rotation * Quaternionf(angle,axis);
00370 }