vcglib: coordinateframe.cpp Source 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 
00052 #include "coordinateframe.h"
00053 
00054 using namespace vcg;
00055 
00056 CoordinateFrame::CoordinateFrame(float s)
00057 :basecolor(Color4b::White),xcolor(Color4b::Red)
00058 ,ycolor(Color4b::Green),zcolor(Color4b::Blue),size(s),linewidth(2.0)
00059 ,font(),drawaxis(true),drawlabels(true),drawvalues(false)
00060 {
00061   font.setFamily("Helvetica");
00062 }
00063 
00064 void CoordinateFrame::Render(QGLWidget* glw)
00065 {
00066   assert( glw!= NULL);
00067   glPushAttrib(GL_ALL_ATTRIB_BITS);
00068   glDisable(GL_LIGHTING);
00069   glDisable(GL_TEXTURE_2D);
00070   glEnable(GL_BLEND);
00071   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00072   glEnable(GL_LINE_SMOOTH);
00073   glEnable(GL_POINT_SMOOTH);
00074   glLineWidth(linewidth);
00075   glPointSize(linewidth*1.5);
00076 
00077   Point3d o(0,0,0);
00078   Point3d a(size,0,0);
00079   Point3d b(0,size,0);
00080   Point3d c(0,0,size);
00081   // Get gl state values
00082   double mm[16],mp[16];
00083   GLint vp[4];
00084   glGetDoublev(GL_MODELVIEW_MATRIX,mm);
00085   glGetDoublev(GL_PROJECTION_MATRIX,mp);
00086   glGetIntegerv(GL_VIEWPORT,vp);
00087   float slope_a=calcSlope(-a,a,2*size,10,mm,mp,vp);
00088   float slope_b=calcSlope(-b,b,2*size,10,mm,mp,vp);
00089   float slope_c=calcSlope(-c,c,2*size,10,mm,mp,vp);
00090   float scalefactor = size*0.02f;
00091   if(drawaxis){
00092     glBegin(GL_LINES);
00093       glColor(xcolor);
00094       glVertex(-a); glVertex(a);
00095       glColor(ycolor);
00096       glVertex(-b); glVertex(b);
00097       glColor(zcolor);
00098       glVertex(-c); glVertex(c);
00099     glEnd();
00100     glColor(basecolor);
00101     // positive axes
00102     drawTickedLine(o,a,size,slope_a,linewidth);  // Draws x axis
00103     drawTickedLine(o,b,size,slope_b,linewidth);  // Draws y axis
00104     drawTickedLine(o,c,size,slope_c,linewidth);  // Draws z axis
00105     // negative axes
00106     drawTickedLine(o,-a,size,slope_a,linewidth);  // Draws x axis
00107     drawTickedLine(o,-b,size,slope_b,linewidth);  // Draws y axis
00108     drawTickedLine(o,-c,size,slope_c,linewidth);  // Draws z axis
00109     glPushMatrix();
00110       glTranslate(a);
00111       glScalef(scalefactor,scalefactor,scalefactor);
00112       Add_Ons::Cone(10,linewidth*1.5,linewidth*0.5,true);
00113     glPopMatrix();
00114     glPushMatrix();
00115       glTranslate(b);
00116       glRotatef(90,0,0,1);
00117       glScalef(scalefactor,scalefactor,scalefactor);
00118       Add_Ons::Cone(10,linewidth*1.5,linewidth*0.5,true);
00119     glPopMatrix();
00120     glPushMatrix();
00121       glTranslate(c);
00122       glRotatef(-90,0,1,0);
00123       glScalef(scalefactor,scalefactor,scalefactor);
00124       Add_Ons::Cone(10,linewidth*1.5,linewidth*0.5,true);
00125     glPopMatrix();
00126   }
00127   if(drawlabels){
00128         font.setBold(true);
00129     font.setPixelSize(12);
00130     float d=size+scalefactor*linewidth*1.5;
00131     glColor(xcolor);
00132     glw->renderText(d,0,0,QString("X"),font);
00133     glColor(ycolor);
00134     glw->renderText(0,d,0,QString("Y"),font);
00135     glColor(zcolor);
00136     glw->renderText(0,0,d,QString("Z"),font);
00137   }  
00138   if(drawvalues){
00139         font.setBold(false);    
00140     font.setPixelSize(8);
00141     float i;
00142     glColor(Color4b::LightGray);
00143     for(i=slope_a;i<size;i+=slope_a){
00144       glw->renderText( i,0,0,QString(" %1").arg(i,3,'f',1),font);
00145       glw->renderText(-i,0,0,QString("-%1").arg(i,3,'f',1),font);
00146     }
00147     for(i=slope_b;i<size;i+=slope_b){
00148       glw->renderText(0, i,0,QString(" %1").arg(i,3,'f',1),font);
00149       glw->renderText(0,-i,0,QString("-%1").arg(i,3,'f',1),font);
00150     }
00151     for(i=slope_c;i<size;i+=slope_c){
00152       glw->renderText(0,0, i,QString(" %1").arg(i,3,'f',1),font);
00153       glw->renderText(0,0,-i,QString("-%1").arg(i,3,'f',1),font);
00154     }
00155   }
00156   
00157   glPopAttrib();
00158   assert(!glGetError());  
00159 }
00160 
00161 void CoordinateFrame::drawTickedLine(const Point3d &a,const Point3d &b, float dim,float tickDist,float linewidth)
00162 {
00163   Point3d v(b-a);
00164   v = v /dim; // normalize without computing square roots and powers
00165 
00166   glBegin(GL_POINTS);
00167   float i;
00168   for(i=tickDist;i<dim;i+=tickDist)
00169     glVertex3f(a[0] + i*v[0],a[1] + i*v[1],a[2] + i*v[2]);
00170   glEnd();
00171 
00172   glPushAttrib(GL_POINT_BIT);
00173   glPointSize(linewidth*3);  
00174   glBegin(GL_POINTS);
00175        glVertex3f(a[0] + dim*v[0],a[1] + dim*v[1],a[2] + dim*v[2]);
00176   glEnd();
00177 
00178   glPopAttrib();
00179 }
00180 
00181 float CoordinateFrame::calcSlope(const Point3d &a,const Point3d &b,float dim,int spacing,double *mm,double *mp,GLint *vp)
00182 {
00183    Point3d p1,p2;
00184 
00185   gluProject(a[0],a[1],a[2],mm,mp,vp,&p1[0],&p1[1],&p1[2]);
00186   gluProject(b[0],b[1],b[2],mm,mp,vp,&p2[0],&p2[1],&p2[2]);
00187   p1[2]=p2[2]=0;
00188 
00189   float tickNum = spacing/Distance(p2,p1);// pxl spacing
00190   float slope = dim*tickNum;
00191   float nslope = math::Min(
00192           math::Min(niceRound(slope), 0.5f*niceRound(2.0f*slope)), 
00193                                       0.2f*niceRound(5.0f*slope));
00194   nslope = math::Max<float>(niceRound(dim*.001f),nslope); // prevent too small slope
00195   return nslope;
00196 }
00197 
00198 float CoordinateFrame::niceRound(float val)
00199 {
00200   return powf(10.f,ceil(log10(val)));
00201 }
00202 
00203 MovableCoordinateFrame::MovableCoordinateFrame(float size)
00204 :CoordinateFrame(size),position(0,0,0),rotation(0,Point3f(1,0,0))
00205 {
00206   // nothing here
00207 }
00208 
00209 void MovableCoordinateFrame::Render(QGLWidget* gla)
00210 {
00211   glPushMatrix();
00212   
00213   glTranslate(position);  
00214   Matrix44f mrot; 
00215   rotation.ToMatrix(mrot);
00216 
00217   glMultMatrix(Inverse(mrot));
00218   
00219   CoordinateFrame::Render(gla);
00220   
00221   glPopMatrix();
00222 }
00223 
00224 void MovableCoordinateFrame::GetTransform(Matrix44f & transform)
00225 {
00226   // build the matrix that moves points in world coordinates
00227 
00228   // clean transform
00229   transform.SetIdentity();
00230 
00231   // apply rotation
00232   Matrix44f rot;
00233   rotation.ToMatrix(rot);
00234 
00235   transform = Inverse(rot) * transform ;
00236   
00237   // apply translation
00238   Matrix44f pos;
00239   pos.SetTranslate(position);
00240   
00241   transform = pos * transform;
00242   
00243 }
00244 
00245 void MovableCoordinateFrame::Reset(bool reset_position,bool reset_alignment)
00246 {
00247   if(reset_position)
00248     position = Point3f(0,0,0);
00249   if(reset_alignment)
00250     rotation = Quaternionf(0,Point3f(1,0,0));
00251 }
00252 
00253 void MovableCoordinateFrame::SetPosition(const Point3f newpos)
00254 {
00255   position = newpos;
00256 }
00257 
00258 void MovableCoordinateFrame::SetRotation(const Quaternionf newrot)
00259 {
00260   rotation = newrot;
00261 }
00262 
00263 Point3f MovableCoordinateFrame::GetPosition()
00264 {
00265   return position;
00266 }
00267 
00268 Quaternionf MovableCoordinateFrame::GetRotation()
00269 {
00270   return rotation;
00271 }
00272 
00273 void MovableCoordinateFrame::Rot(float angle_deg,const Point3f axis)
00274 {
00275   Similarityf s;
00276         s.SetRotate(math::ToRad(angle_deg),(rotation).Rotate(axis));
00277   Move(s);
00278 }
00279 
00280 void MovableCoordinateFrame::AlignWith(const Point3f pri,const Point3f secondary,const char c1, const char c2)
00281 {
00282   const float EPSILON=1e-6f;
00283   Point3f primary=pri;
00284 
00285   if( primary.Norm() < EPSILON*size )
00286     return;
00287 
00288   primary.Normalize();
00289   Plane3f plane(0,primary); // projection plane for the second rotation
00290    
00291   Point3f x(1,0,0),y(0,1,0),z(0,0,1);
00292   Point3f first(0,0,0),second(0,0,0),third(0,0,0);
00293 
00294   if(c1=='X'){ first = x;
00295     if((c2=='Y')||(c2==' ')){ second = y; third = z; } 
00296     else if(c2=='Z'){ second = z; third = y; } 
00297     else assert (0);
00298   } else if(c1=='Y'){ first = y;
00299     if((c2=='Z')||(c2==' ')){ second = z; third = x; } 
00300     else if(c2=='X'){ second = x; third = z; }
00301     else assert (0);
00302   } else if(c1=='Z'){ first = z;
00303     if((c2=='X')||(c2==' ')){ second = x; third = y; }
00304     else if(c2=='Y'){ second = y; third = x; }
00305     else assert (0);   
00306   } else assert (0);
00307 
00308   Point3f old_first = Inverse(rotation).Rotate(first); // axis 1
00309   Point3f old_second_pro = plane.Projection(Inverse(rotation).Rotate(second)); // axis 2 projection
00310   Point3f old_third_pro = plane.Projection(Inverse(rotation).Rotate(third)); // axis 3 projection
00311 
00312   // align axis 1 to primary
00313   RotateToAlign(old_first,primary);
00314 
00315   Point3f secondary_pro = plane.Projection(secondary); // secondary's projection
00316   Point3f new_second_pro = plane.Projection(Inverse(rotation).Rotate(second)); // axis 2 projection after the first rotation
00317 
00318   if( secondary.Norm() > EPSILON*size && secondary_pro.Norm() > EPSILON ){ // secondary is not null nor parallel to primary
00319     // align axis 2 projection after the first rotation to secondary's projection
00320     secondary_pro.Normalize();
00321     RotateToAlign(new_second_pro,secondary_pro);
00322     return;
00323   }
00324 
00325   if ( old_second_pro.Norm() > EPSILON ) { // can realign axis 2
00326     // align axis 2 projection after the first rotation to old axis 2 projection
00327     old_second_pro.Normalize();
00328     RotateToAlign(new_second_pro,old_second_pro);
00329     return;
00330   }
00331 
00332   // realign axis 3
00333   Point3f new_third_pro = plane.Projection(Inverse(rotation).Rotate(third));// axis 3 projection after the first rotation
00334   assert(old_third_pro.Norm() > EPSILON ); // old axis 3 projection should not be null
00335   // align axis 3 projection after the first rotation to old axis 3 projection
00336   old_third_pro.Normalize();
00337   RotateToAlign(new_third_pro,old_third_pro);
00338 }
00339 
00340 void MovableCoordinateFrame::Move(const Similarityf track)
00341 {
00342   position = position + track.tra;
00343   rotation = rotation * Inverse(track.rot);
00344 }
00345 
00346 void MovableCoordinateFrame::RotateToAlign(const Point3f source, const Point3f dest)
00347 {
00348   const float EPSILON=1e-6f;
00349   // source and dest must be versors
00350   assert( math::Abs(source.Norm() - 1) < EPSILON);
00351   assert( math::Abs(dest.Norm() - 1) < EPSILON);
00352 
00353   Point3f axis = dest ^ source;
00354   float sinangle = axis.Norm();
00355   float cosangle = dest.dot(source);
00356   float angle = math::Atan2(sinangle,cosangle);  
00357 
00358   if( math::Abs(angle) < EPSILON )    
00359     return; // angle ~ 0, aborting
00360 
00361   if( math::Abs(math::Abs(angle)-M_PI) < EPSILON){
00362     // must find a axis to flip on
00363     Plane3f plane(0,source);
00364     axis=plane.Projection(Point3f(1,0,0)); // project a "random" point on source's normal plane
00365         if(axis.Norm() < EPSILON){ // source was ~ [1,0,0]...
00366           axis=plane.Projection(Point3f(0,1,0)); 
00367       assert(axis.Norm() > EPSILON); // this point must be good
00368         }
00369   }
00370   rotation = rotation * Quaternionf(angle,axis);
00371 }