PLearn 0.1
geometry.cc
Go to the documentation of this file.
00001 // -*- C++ -*-
00002 
00003 // geometry.cc
00004 //
00005 // Copyright (C) 2004 Pascal Lamblin 
00006 // 
00007 // Redistribution and use in source and binary forms, with or without
00008 // modification, are permitted provided that the following conditions are met:
00009 // 
00010 //  1. Redistributions of source code must retain the above copyright
00011 //     notice, this list of conditions and the following disclaimer.
00012 // 
00013 //  2. Redistributions in binary form must reproduce the above copyright
00014 //     notice, this list of conditions and the following disclaimer in the
00015 //     documentation and/or other materials provided with the distribution.
00016 // 
00017 //  3. The name of the authors may not be used to endorse or promote
00018 //     products derived from this software without specific prior written
00019 //     permission.
00020 // 
00021 // THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND ANY EXPRESS OR
00022 // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
00023 // OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN
00024 // NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00025 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
00026 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00027 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00028 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00029 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00030 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031 // 
00032 // This file is part of the PLearn library. For more information on the PLearn
00033 // library, go to the PLearn Web site at www.plearn.org
00034 
00035 /* *******************************************************      
00036    * $Id: geometry.cc,v 1.15 2005/12/29 11:52:28 lamblinp Exp $ 
00037    ******************************************************* */
00038 
00039 // Authors: Pascal Lamblin
00040 
00044 #include "geometry.h"
00045 
00046 namespace PLearn {
00047 using namespace std;
00048 
00049 Vec fixedAnglesFromRotation( const Mat& m )
00050 {
00051   Vec angle( 3 );
00052 
00053   angle[1] = atan2( -m(2,0), sqrt( m(0,0)*m(0,0) + m(1,0)*m(1,0) ) );
00054   angle[2] = atan2( m(1,0) / cos( angle[1] ), m(0,0) / cos( angle[1] ) );
00055   angle[0] = atan2( m(2,1) / cos( angle[1] ), m(2,2) / cos( angle[1] ) );
00056 
00057   if( angle[ 1 ] * 180 / Pi > 89.9 )
00058   {
00059     angle[ 1 ] = Pi / 2;
00060     angle[ 2 ] = 0;
00061     angle[ 0 ] = atan2( m( 0, 1 ), m( 1, 1 ) );
00062   }
00063   else if( angle[ 1 ] * 180.0 / Pi < -89.9 )
00064   {
00065     angle[ 1 ] = -Pi / 2;
00066     angle[ 2 ] = 0;
00067     angle[ 0 ] = -atan2( m( 0, 1 ), m( 1, 1 ) );
00068   }
00069 
00070   return( angle * ( real(180.0 / Pi) ) );
00071 }
00072 
00073 Mat rotationFromFixedAngles( real rx, real ry, real rz )
00074 {
00075   Mat rot( 3, 3 );
00076 
00077   rx *= DEG2RAD;
00078   ry *= DEG2RAD;
00079   rz *= DEG2RAD;
00080 
00081   rot( 0, 0 ) = cos( rz ) * cos( ry );
00082   rot( 1, 0 ) = sin( rz ) * cos( ry );
00083   rot( 2, 0 ) = -sin( ry );
00084 
00085   rot( 0, 1 ) = cos( rz ) * sin( ry ) * sin( rx ) - sin( rz ) * cos( rx );
00086   rot( 1, 1 ) = sin( rz ) * sin( ry ) * sin( rx ) + cos( rz ) * cos( rx );
00087   rot( 2, 1 ) = cos( ry ) * sin( rx );
00088 
00089   rot( 0, 2 ) = cos( rz ) * sin( ry ) * cos( rx ) + sin( rz ) * sin( rx );
00090   rot( 1, 2 ) = sin( rz ) * sin( ry ) * cos( rx ) - cos( rz ) * sin( rx );
00091   rot( 2, 2 ) = cos( ry ) * cos( rx );
00092 
00093   return rot;
00094 }
00095 
00096 
00097 Mat rotationFromAxisAngle( Vec& K, real th )
00098 {
00099   // j'ai pas cherché à comprendre
00100   Mat R( 3, 3 );
00101   real c = cos( th );
00102   real s = sin( th );
00103   real v = 1 - c;
00104 
00105   R( 0, 0 ) = K[0]*K[0]*v + c;
00106   R( 0, 1 ) = K[0]*K[1]*v - K[2]*s;
00107   R( 0, 2 ) = K[0]*K[2]*v + K[1]*s;
00108   R( 1, 0 ) = K[0]*K[1]*v + K[2]*s;
00109   R( 1, 1 ) = K[1]*K[1]*v + c;
00110   R( 1, 2 ) = K[1]*K[2]*v - K[0]*s;
00111   R( 2, 0 ) = K[0]*K[2]*v - K[1]*s;
00112   R( 2, 1 ) = K[1]*K[2]*v + K[0]*s;
00113   R( 2, 2 ) = K[2]*K[2]*v + c;
00114 
00115   return R;
00116 }
00117 
00118 
00119 Mat boundingBoxToVertices( const Mat& bbox )
00120 {
00121   /*
00122   Mat vertices( 8, 3 );
00123 
00124   ostringstream buf;
00125   buf << bbox(0,0) << " " << bbox(0,1) << " " << bbox(0,2) << " "
00126       << bbox(0,0) << " " << bbox(0,1) << " " << bbox(1,2) << " "
00127       << bbox(0,0) << " " << bbox(1,1) << " " << bbox(0,2) << " "
00128       << bbox(0,0) << " " << bbox(1,1) << " " << bbox(1,2) << " "
00129       << bbox(1,0) << " " << bbox(0,1) << " " << bbox(0,2) << " "
00130       << bbox(1,0) << " " << bbox(0,1) << " " << bbox(1,2) << " "
00131       << bbox(1,0) << " " << bbox(1,1) << " " << bbox(0,2) << " "
00132       << bbox(1,0) << " " << bbox(1,1) << " " << bbox(1,2) << " ";
00133 
00134   vertices << buf.str();
00135    */
00136 
00137   real buf_[24] = {
00138     bbox(0,0), bbox(0,1), bbox(0,2),
00139     bbox(0,0), bbox(0,1), bbox(1,2),
00140     bbox(0,0), bbox(1,1), bbox(0,2),
00141     bbox(0,0), bbox(1,1), bbox(1,2),
00142     bbox(1,0), bbox(0,1), bbox(0,2),
00143     bbox(1,0), bbox(0,1), bbox(1,2),
00144     bbox(1,0), bbox(1,1), bbox(0,2),
00145     bbox(1,0), bbox(1,1), bbox(1,2)
00146   };
00147 
00148   Mat vertices( 8, 3, buf_ );
00149 
00150   return vertices;
00151 }
00152 
00153 
00154 
00155 void transformPoints( const Mat& rot, const Vec& trans,
00156                       const Mat& points_in, Mat& points_out )
00157 {
00158   int n = points_in.length();
00159   points_out.resize( n, 3 );
00160   Mat tmp( n, 3 );
00161 
00162   productTranspose( tmp, points_in, rot );
00163   tmp += trans;
00164   points_out << tmp;
00165 }
00166 
00167 void transformMesh( const Mat& rot, const Vec& trans, SurfMesh& sm )
00168 {
00169   Mat input = sm->getVertexCoords();
00170   transformPoints( rot, trans, input, input );
00171   sm->setVertexCoords( input );
00172 
00173   input = sm->getVertexNorms();
00174   transformPoints( rot, Vec(3), input, input );
00175   sm->setVertexNorms( input );
00176 }
00177 
00178 
00179 
00180 void weightedTransformationFromMatchedPoints( const Mat& mp, const Mat& sp,
00181                                               const Vec& weights, Mat& rot,
00182                                               Vec& trans, real& error )
00183 {
00184   Vec cs( 3 );
00185   cs << weightedCentroid( sp, weights );
00186 
00187   Vec cm( 3 );
00188   cm << weightedCentroid( mp, weights );
00189 
00190   int n = mp.length();
00191   Mat origin_mp( n, 3 );
00192   Mat origin_sp( n, 3 );
00193 
00194   origin_mp = mp - cm;
00195   origin_sp = sp - cs;
00196 
00197   rot << weightedRotationFromMatchedPoints( origin_mp, origin_sp, 
00198                                             weights, error );
00199 
00200   Vec res( 3 );
00201   product( res, rot, cm );
00202 
00203   trans << ( cs - res );
00204 }
00205 
00206 Vec weightedCentroid( const Mat& pts, const Vec& weights )
00207 {
00208   Vec centroid( 3 );
00209   int n = pts.length();
00210   real w_tot = 0;
00211 
00212   for( int i=0 ; i<n ; i++ )
00213   {
00214     real w = weights[ i ];
00215     centroid += pts( i ) * w;
00216     w_tot += w;
00217   }
00218 
00219   if( w_tot == 0 )
00220   {
00221     centroid = Vec( 3 );
00222   }
00223   else
00224   {
00225     centroid /= w_tot;
00226   }
00227 
00228   return centroid;
00229 }
00230 
00231 
00232 Mat weightedRotationFromMatchedPoints( const Mat& mp, const Mat& sp, 
00233                                        const Vec& weights, real& error )
00234 {
00235   // Ouais, c'est absolument pas optimisé, je sais
00236 
00237   Mat M( 4, 4 );
00238   Mat A( 4, 4 );
00239   Mat rot( 3, 3 );
00240 
00241   int n = mp.length();
00242   Vec vm( 3 );
00243   Vec vs( 3 );
00244 
00245   for( int i=0 ; i<n ; i++ )
00246   {
00247     vm << mp( i );
00248     vs << sp( i );
00249 
00250     real weight = weights[ i ];
00251     M(0,1) = vm[2]+vs[2];
00252     M(0,2) = -vm[1]-vs[1];
00253     M(0,3) = vm[0]-vs[0];
00254 
00255     M(1,0) = -vm[2]-vs[2];
00256     M(1,2) = vm[0]+vs[0];
00257     M(1,3) = vm[1]-vs[1];
00258 
00259     M(2,0) = vm[1]+vs[1];
00260     M(2,1) = -vm[0]-vs[0];
00261     M(2,3) = vm[2]-vs[2];
00262 
00263     M(3,0) = -vm[0]+vs[0];
00264     M(3,1) = -vm[1]+vs[1];
00265     M(3,2) = -vm[2]+vs[2];
00266 
00267     // A = A + transpose(M) * M * weight
00268     transposeProductAcc( A, M, M * weight );
00269   }
00270 
00271   Vec ev( 4 );
00272   Mat e( 4, 4 );
00273   int n_rot;
00274 
00275   if( jacobi( A, ev, e, n_rot ) )
00276   {
00277     eigsrt( ev, e, 4 );
00278 
00279     error = ev[ 3 ];
00280     real theta = 2.0 * acos( e( 3, 3 ) );
00281 
00282     if( theta !=0 )
00283     {
00284       Vec v( 3 );
00285       v << e.subMat( 0, 3, 3, 1 );
00286       v /= real(sin( theta/2.0 ));
00287       rot << rotationFromAxisAngle( v, theta );
00288       return rot;
00289     }
00290   }
00291 
00292   // rot = Id3
00293   rot << diagonalmatrix( Vec( 3, 1 ) );
00294 
00295   return rot;
00296 
00297 }
00298 
00299 /*
00300  ******************************************************************************
00301  + FUNCTION: jacobi
00302  + AUTHOR:   Numerical Recipes in C
00303  + MODIFIED: Pascal Lamblin
00304  + DATE:     16-Aug-04
00305  + PURPOSE:  Finds eigenvectors and eigenvalues of a matrix.
00306  ******************************************************************************
00307  */
00308 int jacobi( Mat& a, Vec& d, Mat& v, int& nrot )
00309 {
00310   int n = a.length();
00311   if( n != a.width() || n != v.length() || n != v.width() || n != d.length() )
00312   {
00313     PLERROR( "in jacobi( a, d, v, nrot ):\n a.length(), a.width(), v.length(), v.width() and d.length() must be equal" );
00314   }
00315 
00316   // v = identity
00317   v = diagonalmatrix( Vec( n, 1 ) );
00318   nrot = 0;
00319 
00320   Vec b( n );
00321   Vec z( n );
00322 
00323   for( int ip=0 ; ip<n ; ip++ )
00324   {
00325     real val = a( ip,  ip );
00326     b[ ip ] = val;
00327     d[ ip ] = val;
00328   }
00329 
00330   for( int i=0 ; i<50 ; i++ )
00331   {
00332     real sm = 0;
00333     real tresh;
00334     for( int ip=0 ; ip<n-1 ; ip++ )
00335     {
00336       for( int iq=ip+1 ; iq<n ; iq++ )
00337       { sm += fabs( a( ip, iq ) ); }
00338     }
00339 
00340     if( sm == 0 )
00341     { return 1; }
00342 
00343     if( i < 3 )
00344     { tresh = 0.2*sm/(n*n); }
00345     else
00346     { tresh = 0; }
00347 
00348     for( int ip=0 ; ip<n-1 ; ip++ )
00349     {
00350       for( int iq=ip+1 ; iq<n ; iq++ )
00351       {
00352         real g=100.0 * fabs( a( ip, iq ) );
00353         if( i>3 && ( fabs( d[ip] ) + g == fabs( d[ip] ) )
00354             && ( fabs( d[iq] ) + g == fabs( d[iq] ) ) )
00355         { a( ip, iq ) = 0; }
00356         else if( fabs( a( ip, iq ) ) > tresh )
00357         {
00358           real h = d[iq] - d[ip];
00359           real t;
00360           if( fabs( h ) + g == fabs( h ) )
00361           { t = a( ip, iq ) / h; }
00362           else
00363           {
00364             real theta = 0.5 * h/( a( ip, iq ) );
00365             t = 1.0 / ( fabs( theta ) + sqrt( 1.0 + theta*theta ) );
00366             if( theta < 0.0 )
00367             { t = -t; }
00368           }
00369           real c = 1.0 / sqrt( 1 + t*t );
00370           real s = t*c;
00371           real tau = s / ( 1.0 + c );
00372           h = t*a( ip, iq );
00373           z[ip] -= h;
00374           z[iq] += h;
00375           d[ip] -= h;
00376           d[iq] += h;
00377           a( ip, iq ) = 0.0;
00378 
00379           for( int j=0 ; j<=ip-1 ; j++ )
00380           {
00381             rotate( a, j, ip, j, iq, s, tau );
00382           }
00383           for( int j=ip+1 ; j<=iq-1 ; j++ )
00384           {
00385             rotate( a, ip, j, j, iq , s, tau );
00386           }
00387           for( int j=iq+1 ; j<n ; j++ )
00388           {
00389             rotate( a, ip, j, iq, j, s, tau );
00390           }
00391           for( int j=0 ; j<n ; j++ )
00392           {
00393             rotate( v, j, ip, j, iq, s, tau );
00394           }
00395           ++nrot;
00396         }
00397       }
00398     }
00399     b += z;
00400     d << b;
00401     z = 0;
00402   }
00403   PLERROR( "jacobi: too many iterations" );
00404   return 0;
00405 }
00406 
00407 
00408 // auxiliary function for jacobi
00409 void rotate( Mat& a, int i, int j, int  k, int l,
00410              const real& s, const real& tau )
00411 {
00412   real g = a( i, j );
00413   real h = a( k, l );
00414   a( i, j ) = g - s*( h + g*tau );
00415   a( k, l ) = h + s*( g - h*tau );
00416 }
00417 
00418 
00419 /*
00420  ******************************************************************************
00421  + FUNCTION: eigsrt
00422  + AUTHOR:   Numerical Recipes in C
00423  + MODIFIED: Andrew E. Johnson (aej@ri.cmu.edu) 
00424  + DATE:     3-Nov-94
00425  + PURPOSE:  Sorts eigenvectors from jacobi based on eigenvalues.
00426  ******************************************************************************
00427  */
00428 
00429 void eigsrt( Vec& d, Mat& v, int n )
00430 {
00431   for( int i=0 ; i<n-1 ; i++ )
00432   {
00433     real p = d[ i ];
00434     int k = i;
00435 
00436     for( int j=i+1 ; j<n ; j++ )
00437     {
00438       if( fabs( d[j] ) >= fabs( p ) )
00439       {
00440         p = d[ j ];
00441         k = j;
00442       }
00443     }
00444 
00445     if( k!=i )
00446     {
00447       d[ k ] = d[ i ];
00448       d[ i ] = p;
00449       for( int j=0 ; j<n ; j++ )
00450       {
00451         p = v( j, i );
00452         v( j, i ) = v( j, k );
00453         v( j, k ) = p;
00454       }
00455     }
00456   }
00457 }
00458 
00459 
00460 real maxPointMotion( const Mat& old_points, const Mat& new_points )
00461 {
00462   real max_motion2 = 0;
00463   int n = old_points.length();
00464 
00465   if( new_points.length() != n )
00466   {
00467     PLERROR( "maxPointMotion: old_points and new_points Mat must have same length" );
00468   }
00469 
00470   for( int i=0 ; i<n ; i++)
00471   {
00472     real motion2 = powdistance( new_points( i ), old_points( i ), 2 );
00473     max_motion2 = max( max_motion2, motion2 );
00474   }
00475 
00476   return sqrt( max_motion2 );
00477 }
00478 
00479 
00480 
00481 
00482 /*
00483  ******************************************************************************
00484  + FUNCTION: CalcNormal
00485  + AUTHOR:   Andrew E. Johnson (aej@ri.cmu.edu)
00486  + DATE:     29-Jun-94
00487  + PURPOSE:  Given a node in the mesh it calculates the surface normal at that
00488  +           node using the nodes nearest neighbors.
00489  ******************************************************************************
00490  */
00491 
00492 real calcNormal( graph& mesh, const vertex_descriptor& vtx, Vec& norm )
00493 {
00494   set<vertex_descriptor> points;
00495 
00496   adjacency_iterator ai, ai_end;
00497   for( tie(ai,ai_end)=adjacent_vertices(vtx,mesh) ; ai!=ai_end ; ai++ )
00498   {
00499     adjacency_iterator bi, bi_end;
00500     for( tie(bi,bi_end)=adjacent_vertices(*ai,mesh) ; bi!=bi_end ; bi++ )
00501     {
00502       points.insert( *bi );
00503     }
00504   }
00505 
00506   /* determine the sums used to calculate the surface normal by fitting 
00507      a plane to the neighborhood of points */
00508 
00509   Vec sums( 10 );
00510   real d;
00511   real fit_error;
00512   findSumsFromPts( mesh, points, sums );
00513   calcPlaneParams( sums, norm, d, fit_error ); // fit the plane to get normal
00514 
00515   real n = sums[0];
00516 
00517   real error_sum = 0;
00518   for( tie(ai,ai_end)=adjacent_vertices(vtx,mesh) ; ai!=ai_end ; ai++ )
00519   {
00520     real dist = dot( norm, get(vertex_ppt,mesh,*ai)->coord ) + d;
00521     error_sum += dist*dist;
00522   }
00523 
00524   fit_error = sqrt( error_sum )/n;
00525 
00526   if( n <= 3 )
00527   { fit_error = INFINITY; }
00528 
00529   /* return the fit error on the plane */
00530   return fit_error;
00531 
00532 }
00533 
00534 Vec calcNormal( const Vec& v1, const Vec& v2, const Vec& v3,
00535                 const Vec& n1, const Vec& n2, const Vec& n3,  
00536                 const Vec& target)
00537 {
00538   // just use avg of the 3 normals for now (later use barycentric coords)
00539   Vec normal = n1 + n2 + n3;
00540   normalize( normal, 2 );
00541 
00542   return normal;
00543 }
00544 
00545 
00546 
00547 /*
00548  ******************************************************************************
00549  + FUNCTION: FindSumsFromPts
00550  + AUTHOR:   Andrew E. Johnson (aej@ri.cmu.edu)
00551  + DATE:     14-Jul-94
00552  + PURPOSE:  Calculates the sums needed to calculate the planar and quadric
00553  +           parameters of a region. It takes a set of integers that
00554  +           correspond to the array location of all of the meshpoints in the
00555  +           region and calulates the sums.
00556  ******************************************************************************
00557  */
00558 
00559 void findSumsFromPts( const graph& mesh, const set<vertex_descriptor>& points,
00560                       Vec& sums )
00561 {
00562   sums.resize( 10 );
00563 
00564   set<vertex_descriptor>::const_iterator it;
00565   for( it=points.begin() ; it!=points.end() ; it++ )
00566   {
00567     Vec p = get( vertex_ppt, mesh, *it )->coord;
00568     real x = p[ 0 ];
00569     real y = p[ 1 ];
00570     real z = p[ 2 ];
00571 
00572     sums[0]++;
00573     sums[1]+=x;
00574     sums[2]+=y;
00575     sums[3]+=z;
00576     sums[4]+=x*x;
00577     sums[5]+=y*y;
00578     sums[6]+=z*z;
00579     sums[7]+=x*y;
00580     sums[8]+=x*z;
00581     sums[9]+=y*z;
00582 
00583   }
00584 }
00585 
00586 /*
00587  ******************************************************************************
00588  + FUNCTION: CalcPlaneParams
00589  + AUTHOR:   Andrew E. Johnson (aej@ri.cmu.edu)
00590  + DATE:     6-Jul-94
00591  + PURPOSE:  Calculates the new plane parameters for a plane and a new point.
00592  ******************************************************************************
00593  */
00594 
00595 void calcPlaneParams(const Vec& sums, Vec& norm, real& d, real& err)
00596 {
00597   if( sums[0] >= 3 )
00598   {
00599     real one_over_n = 1./sums[0];
00600 
00601     Mat inertia( 3, 3 );
00602 
00603     inertia(0,0) = ( sums[4] - sums[1]*sums[1]*one_over_n );
00604     inertia(1,1) = ( sums[5] - sums[2]*sums[2]*one_over_n );
00605     inertia(2,2) = ( sums[6] - sums[3]*sums[3]*one_over_n );
00606     inertia(1,0) = inertia(0,1) = (sums[7]-sums[1]*sums[2]*one_over_n );
00607     inertia(2,0) = inertia(0,2) = (sums[8]-sums[1]*sums[3]*one_over_n );
00608     inertia(2,1) = inertia(1,2) = (sums[9]-sums[2]*sums[3]*one_over_n );
00609 
00610     Mat e( 3, 3 );
00611     Vec ev( 3 );
00612     int nrot;
00613 
00614     if( jacobi( inertia, ev, e, nrot ) )
00615     {
00616       // The eigen vector corresponding to the smallest eigen value of
00617       // the inertia matrix is the normal of the plane.
00618 
00619       int sm_ev = getNormFromEigVecs( ev, e, norm );
00620       err = fabs( ev[sm_ev] );
00621       d = -( sums[1]*norm[0] + sums[2]*norm[1]+sums[3]*norm[2])/sums[0];
00622       return;
00623     }
00624   }
00625 
00626   norm.resize( 3 );
00627   norm << "0 0 1";
00628   d = 0;
00629   err = INFINITY;
00630   return;
00631 }
00632 
00633 /*
00634  ******************************************************************************
00635  + FUNCTION: GetNormFromEigVecs
00636  + AUTHOR:   Andrew E. Johnson (aej@ri.cmu.edu)
00637  + DATE:     29-Jun-94
00638  + PURPOSE:  Finds the minimum eigenvalue in a vector of eigenvalues and sets
00639  +           the normal equal to this eigenvector. Returns the index of the
00640  +           smallest eigen value
00641  ******************************************************************************
00642  */
00643 
00644 
00645 int getNormFromEigVecs( const Vec& ev, const Mat& e, Vec& norm)
00646 {
00647   int sm_ev;
00648   if( fabs(ev[0]) <= fabs(ev[1]) )
00649   {
00650     if( fabs(ev[0]) <= fabs(ev[2]) )
00651     { sm_ev = 0; }
00652     else
00653     { sm_ev = 2; }
00654   }
00655   else
00656   {
00657     if( fabs(ev[1]) <= fabs(ev[2]) )
00658     { sm_ev = 1; }
00659     else
00660     { sm_ev = 2; }
00661   }
00662 
00663   /* set normal */
00664   if( e(2, sm_ev) >= 0 )
00665   {
00666     norm[0] = e(0, sm_ev);
00667     norm[1] = e(1, sm_ev);
00668     norm[2] = e(2, sm_ev);
00669   }
00670   else
00671   {
00672     norm[0] = -e(0, sm_ev);
00673     norm[1] = -e(1, sm_ev);
00674     norm[2] = -e(2, sm_ev);
00675   }
00676 
00677   return sm_ev;
00678 }
00679 
00680 /*
00681  ******************************************************************************
00682  + FUNCTION: Cross
00683  + AUTHOR:   Andrew E. Johnson (aej@ri.cmu.edu)
00684  + DATE:     14-oct-94
00685  + PURPOSE:  returns the cross product of two 3 vectors
00686  ******************************************************************************
00687  */
00688 
00689 Vec cross( const Vec& v1, const Vec& v2 )
00690 {
00691   if( v1.size()!=3 || v2.size()!=3 )
00692   {
00693     PLERROR("cross-product of 2 Vec is only defined for Vec of size 3");
00694   }
00695 
00696   Vec res( 3 );
00697 
00698   res[0] = v1[1]*v2[2] - v2[1]*v1[2];
00699   res[1] = -( v1[0]*v2[2] - v2[0]*v1[2] );
00700   res[2] = v1[0]*v2[1] - v2[0]*v1[1];
00701 
00702   return res;
00703 }
00704 
00705 /*
00706  ******************************************************************************
00707  + FUNCTION: RandomTransformation
00708  + AUTHOR:   Andrew E. Johnson (aej@ri.cmu.edu)
00709  + DATE:     26-Jun-96
00710  + PURPOSE:  Creates a random rotation matrix with a maximun rotation angle of
00711  +           max_angle degrees.
00712  ******************************************************************************
00713  */
00714 
00715 void randomTransformation( real max_angle, real max_dist,
00716                            Mat& rot, Vec& trans )
00717 {
00718   rot = randomRotation( max_angle );
00719 
00720   trans[0] = bounded_uniform( -max_dist, max_dist );
00721   trans[1] = bounded_uniform( -max_dist, max_dist );
00722   trans[2] = bounded_uniform( -max_dist, max_dist );
00723 }
00724 
00725 
00726 /*
00727  ******************************************************************************
00728  + FUNCTION: RandomRotation
00729  + AUTHOR:   Andrew E. Johnson (aej@ri.cmu.edu)
00730  + DATE:     26-Jun-96
00731  + PURPOSE:  Creates a random rotation matrix with a maximun rotation angle of
00732  +           max_angle degrees.-
00733  ******************************************************************************
00734  */
00735 
00736 Mat randomRotation( real max_angle )
00737 {
00738   Mat rot( 3, 3 );
00739 
00740   real x1 = uniform_sample();
00741   real x2 = uniform_sample();
00742   real x3 = uniform_sample();
00743 
00744   /* scale x3 by max_angle */
00745   x3 *= (max_angle/180.0);
00746 
00747   real z = x1;
00748   real t = 2*Pi*x2;
00749   real r = sqrt( 1 - z*z );
00750   real w = Pi*x3;
00751 
00752   /* create quaternion */
00753   real a = cos(w);
00754   real b = sin(w) * cos(t) * r;
00755   real c = sin(w) * sin(t) * r;
00756   real d = sin(w) * z;
00757 
00758   /* create rotation matrix */
00759   rot(0,0) = 1-2*(c*c+d*d);
00760   rot(0,1) = 2*(b*c+a*d);
00761   rot(0,2) = 2*(b*d-a*c);
00762   rot(1,0) = 2*(b*c-a*d);
00763   rot(1,1) = 1-2*(b*b+d*d);
00764   rot(1,2) = 2*(c*d+a*b);
00765   rot(2,0) = 2*(b*d+a*c);
00766   rot(2,1) = 2*(c*d-a*b);
00767   rot(2,2) = 1-2*(b*b+c*c);
00768 
00769   return rot;
00770 }
00771 
00772 void getNearestVertex( const Vec& test_pt, const SurfMesh& mesh2,
00773                        const GenericNN& btl,
00774                        int& closest_vertex, Vec& closest_pt,
00775                        real& closest_dist )
00776 {
00777   // find closest vertex on mesh2
00778   Vec dists;
00779   Vec outputs;
00780   btl-> computeOutputAndCosts( test_pt, Vec(), outputs, dists );
00781 
00782   int dimension = outputs.size()-1;
00783   closest_pt << outputs.subVec( 0, dimension );
00784   closest_vertex = (int) outputs[dimension];
00785   closest_dist = dists[0];
00786 }
00787 
00788 
00789 
00790 // lookup tables for classifying triangles
00791 static TriType r1_table[3][3] = {
00792   {VERTEX1, VERTEX2, EDGE1},
00793   {VERTEX2, VERTEX3, EDGE2},
00794   {VERTEX3, VERTEX1, EDGE3} };
00795 
00796 static TriType r2_table[3][5] = {
00797   {VERTEX3, VERTEX1, VERTEX2, EDGE3, EDGE1},
00798   {VERTEX1, VERTEX2, VERTEX3, EDGE1, EDGE2},
00799   {VERTEX2, VERTEX3, VERTEX1, EDGE2, EDGE3} };
00800 
00801 // need to be sure face_cache is filled in first
00802 /******************************************************************************
00803 Description:
00804   Returns true if the test point overlaps mesh2.  If the function returns true,
00805   it also returns the closest point on the surface of mesh2.
00806 
00807 Arguments:
00808   test_pt, test_normal - coords and normal of test point
00809   mesh2
00810   face_cache - foreach vertex on mesh 2, lists adjacent faces
00811   kdt - for finding closest vertices
00812   normalT - threshold on angle between normals of test_pt and closest_pt
00813   closest_pt - return - closest point on the surface of mesh2
00814 
00815 Return:
00816   Returns true if the test point overlaps mesh2.  It also returns the closest
00817   point on the surface of mesh2.
00818 
00819 ******************************************************************************/
00820 
00821 bool isOverlapping( Vec& test_pt,
00822                     Vec& test_normal,
00823                     const SurfMesh& mesh2,
00824                     const TVec< set<int> >& face_cache,
00825                     GenericNN& btl,
00826                     const real init_dist_t,
00827                     const real normal_t, //rads
00828                     int& closest_vertex,
00829                     Vec& closest_pt,
00830                     real& closest_dist )
00831 {
00832   real dist_t = init_dist_t;
00833 
00834   getNearestVertex( test_pt, mesh2, btl,
00835                     closest_vertex, closest_pt, closest_dist );
00836 /*
00837   // find closest vertex on mesh2
00838   Vec dists;
00839   Vec outputs;
00840 //  Vec targets;
00841   btl->computeOutputAndCosts( test_pt, Vec(), outputs, dists );
00842 
00843   closest_vertex = (int) outputs[0];
00844   closest_dist = dists[0];
00845 */
00846   int closest_face;
00847   TriType closest_tri_type;
00848 
00849   // find closest face point on mesh2
00850   if( !closestFacePoint( test_pt, face_cache[closest_vertex], mesh2, dist_t,
00851                          closest_pt, closest_dist, closest_face,
00852                          closest_tri_type ) )
00853   {
00854     // should not happen
00855     closest_dist = dist_t;
00856     closest_pt = Vec( 3, MISSING_VALUE );
00857     PLWARNING( "no closest face point found for %i.\n", closest_vertex );
00858     return false;
00859   }
00860 
00861   if( !pointIsInterior( closest_tri_type, closest_face, mesh2 ) )
00862   {
00863     return false;
00864   }
00865 
00866   // check if normals agree within threshold
00867   // 1 compute normal for point on mesh2
00868   // 2 compare normals with dot product
00869   MFace mf = mesh2->getFace( closest_face );
00870   MVertex p1 = mesh2->getVertex( mf->pts[0] );
00871   MVertex p2 = mesh2->getVertex( mf->pts[1] );
00872   MVertex p3 = mesh2->getVertex( mf->pts[2] );
00873 
00874   Vec m2_normal = calcNormal( p1->coord, p2->coord, p3->coord,
00875                               p1->norm, p2->norm, p3->norm,
00876                               closest_pt );
00877 
00878   if( dot( test_normal, m2_normal ) < cos( normal_t ) )
00879   {
00880     return false;
00881   }
00882 
00883   return true;
00884 }
00885 
00886 // returns true if point of triangle type tri_type on face m2face of
00887 // mesh mesh2 is is interior to the boundary
00888 bool pointIsInterior( const TriType tri_type, const int m2face,
00889                       const SurfMesh& mesh2 )
00890 {
00891   // depending on tri_type, check whether the mesh points are boundary points
00892   MFace mf = mesh2->getFace( m2face );
00893   int bf1 = mesh2->getVertex( mf->pts[0] )->bf;
00894   int bf2 = mesh2->getVertex( mf->pts[1] )->bf;
00895   int bf3 = mesh2->getVertex( mf->pts[2] )->bf;
00896 
00897   switch( tri_type )
00898   {
00899     case VERTEX1:
00900       if( bf1 ) return false;
00901       break;
00902     case VERTEX2:
00903       if( bf2 ) return false;
00904       break;
00905     case VERTEX3:
00906       if( bf3 ) return false;
00907       break;
00908     case EDGE1:
00909       if( bf1 && bf2 ) return false;
00910       break;
00911     case EDGE2:
00912       if( bf2 && bf3 ) return false;
00913       break;
00914     case EDGE3:
00915       if( bf3 && bf1 ) return false;
00916       break;
00917     default:
00918       break;
00919   }
00920   return true;
00921 }
00922 
00923 
00924 
00925 /******************************************************************************
00926 Description:
00927   Find the closest face point with distance less than distT.
00928 
00929 Arguments:
00930   m1pt - reference point
00931   m2faces - set of faces (face_ids) that could contain closest point
00932   mesh2 - for looking up tha actual m2faces values
00933   distT - distance threshold-
00934   closest_pt - return - closest point on m2faces
00935   closest_dist - return - dist from m1pt to closest_pt
00936   closest_face - return - index of closest face
00937   closest_tri_type - return - reln of point to closest face
00938 
00939 Return:
00940   computes closest_pt and closest_dist
00941   returns true if a point was found with distance <= distT, false otherwise
00942   if function returns false, closest_pt is not set!
00943 ******************************************************************************/
00944 
00945 bool closestFacePoint( const Vec& m1pt,
00946                        const set<int>& m2faces,
00947                        const SurfMesh& mesh2,
00948                        const real dist_t,
00949                        Vec& closest_pt,
00950                        real& closest_dist,
00951                        int& closest_face,
00952                        TriType& closest_tri_type )
00953 {
00954   bool found_closer( false );
00955   closest_dist = dist_t;
00956 
00957   set<int>::const_iterator loop_iter;
00958   for( loop_iter = m2faces.begin() ; loop_iter != m2faces.end() ; loop_iter++ )
00959   {
00960     int i = *loop_iter;
00961     MFace mf = mesh2->getFace( i );
00962     Vec m2coord1 = mesh2->getVertex( mf->pts[0] )->coord;
00963     Vec m2coord2 = mesh2->getVertex( mf->pts[1] )->coord;
00964     Vec m2coord3 = mesh2->getVertex( mf->pts[2] )->coord;
00965 
00966     Vec face_pt(3);
00967     TriType tri_type;
00968     real dist;
00969     if( closestPointOnTriangle( m1pt, m2coord1, m2coord2, m2coord3,
00970                                 closest_dist, face_pt, tri_type, dist ) )
00971     {
00972       if( dist < closest_dist + REAL_EPSILON )
00973       {
00974         found_closer = true;
00975         closest_pt << face_pt;
00976         closest_dist = dist;
00977         closest_face = i;
00978         closest_tri_type = tri_type;
00979       }
00980     }
00981   }
00982   return found_closer;
00983 }
00984 
00985 /******************************************************************************
00986 Description:
00987   Find the closest point on a given triangle to the point p if it is less than-
00988   distT distance away from the triangle.
00989 
00990   Basic algorithm is to:
00991   1. project p onto the plane containing the triangle.
00992   2. determine which region the point projects into (see below)
00993   3. depending on region, perform test to see which vertex or edge is closest.
00994 
00995 Arguments:
00996   p - target point
00997   v1, v2, v3 - vertices of the triangle
00998   distT - threshold distance-
00999   closest - return value of closest point
01000   tri_type - classification of closest point (interior, one of the edges, or
01001   one of the vertices.
01002   dist - distance to closest point on triangle
01003 
01004 Return:
01005   closest, tri_type, and dist
01006   function returns true if a point was found <= distT away, false otherwise.
01007   If false, the values of closest, tri_type, and dist are not determined.
01008 ******************************************************************************/
01009 /*
01010      .                       .
01011         .  Region 2       .
01012            .           .
01013               .     .
01014                  .
01015               .     .   Region 1
01016            .           .
01017         .                 .
01018      .       Region 3        .
01019   ............................................
01020 
01021 note: there are three areas that are region 1 and three that are region 2.
01022 
01023 */
01024 
01025 bool closestPointOnTriangle( const Vec& p,
01026                              const Vec& v1,
01027                              const Vec& v2,
01028                              const Vec& v3,
01029                              const real dist_t,
01030                              Vec& closest,
01031                              TriType& tri_type,
01032                              real& dist )
01033 {
01034   bool stop = false;
01035 
01036   if( powdistance( p, v1, 2 ) < REAL_EPSILON )
01037   {
01038     tri_type = VERTEX1;
01039     stop = true;
01040   }
01041   else if( powdistance( p, v2, 2 ) < REAL_EPSILON )
01042   {
01043     tri_type = VERTEX2;
01044     stop = true;
01045   }
01046   else if( powdistance( p, v3, 2 ) < REAL_EPSILON )
01047   {
01048     tri_type = VERTEX3;
01049     stop = true;
01050   }
01051 
01052   if( stop )
01053   {
01054     closest << p;
01055     dist = 0;
01056   }
01057 
01058   // determine triangle plane equation nx+d=0 and make sure triangle is
01059   // well defined
01060   Vec normal = cross( v2-v1, v3-v2 );
01061   real norm_length = norm( normal );
01062 
01063   if( norm_length < REAL_EPSILON ) // 2 edges of triangle pll (singularity)
01064   {
01065     return false;
01066   }
01067 
01068   // normalize the normal
01069   normalize( normal, 2 );
01070 
01071   // determine distance to plane
01072   dist = dot( normal, p ) - dot( normal, v1 );
01073 
01074   // quick test -- no point can be less than dist_t if the distance to
01075   // the plane containing the triangle is greater than dist_t
01076   if( fabs(dist) > dist_t + REAL_EPSILON )
01077   {
01078     return false;
01079   }
01080 
01081   // determine point on plane (planep = p - dist*normal)
01082   Vec planep = p - (dist*normal);
01083 
01084   // determine the position of planep with respect to the 3 lines making
01085   // up the triangle
01086   // si > 0 => point is to the left of the edge
01087   // si = (ei cross planep - vi) dot n
01088 
01089   Vec e1 = v2-v1;
01090   Vec e2 = v3-v2;
01091   Vec e3 = v1-v3;
01092 
01093   real s1 = dot( cross( e1, planep ), normal );
01094   real s2 = dot( cross( e2, planep ), normal );
01095   real s3 = dot( cross( e3, planep ), normal );
01096 
01097   // region 3 - point projects inside triangle, so return planep
01098   if( (s1 >= 0) && (s2 >= 0) && (s3 >= 0 ) )
01099   {
01100     closest << planep;
01101     dist = fabs( dist );
01102     tri_type = FACE;
01103 
01104     if( dist > dist_t + REAL_EPSILON )
01105       return false;
01106     else
01107       return true;
01108 
01109   }
01110 
01111   // region 1 tests - point is inside the u-shaped region formed by one
01112   // edge and the extension of the adjacent edges
01113   if( (s1<0) && (s2 >= 0) && (s3 >= 0) )
01114   {
01115     int edge_type = region1ClosestPoint( planep, v1, v2, e1, closest );
01116     tri_type = r1_table[0][edge_type];
01117     stop = true;
01118   }
01119   else if( (s1 >= 0) && (s2 < 0) && (s3 >= 0) )
01120   {
01121     int edge_type = region1ClosestPoint( planep, v2, v3, e2, closest );
01122     tri_type = r1_table[1][edge_type];
01123     stop = true;
01124   }
01125   else if( (s1 >= 0) && (s2 >= 0) && (s3 < 0) )
01126   {
01127     int edge_type = region1ClosestPoint( planep, v3, v1, e3, closest );
01128     tri_type = r1_table[2][edge_type];
01129     stop = true;
01130   }
01131 
01132   if( stop )
01133   {
01134     dist = norm( p-closest );
01135 
01136     if( dist > dist_t + REAL_EPSILON )
01137       return false;
01138     else
01139       return true;
01140 
01141   }
01142 
01143   // region 2 tests - point is inside the v-shaped region formed by the
01144   // extension of two edges
01145   if( (s1 < 0) && (s3 < 0) )
01146   {
01147     int edge_type = region2ClosestPoint( planep, v3, v1, v2, e3, e1, closest );
01148     tri_type = r2_table[0][edge_type];
01149     stop = true;
01150   }
01151   else if( (s2 < 0) && (s1 < 0) )
01152   {
01153     int edge_type = region2ClosestPoint( planep, v1, v2, v3, e1, e2, closest );
01154     tri_type = r2_table[1][edge_type];
01155     stop = true;
01156   }
01157   else if( (s3 < 0) && (s2 < 0) )
01158   {
01159     int edge_type = region2ClosestPoint( planep, v2, v3, v1, e2, e3, closest );
01160     tri_type = r2_table[2][edge_type];
01161     stop = true;
01162   }
01163 
01164   if( stop )
01165   {
01166     dist = norm( p-closest );
01167 
01168     if( dist > dist_t + REAL_EPSILON )
01169       return false;
01170     else
01171       return true;
01172   }
01173   else
01174   {
01175     // should not occur, since all solutions should be covered by
01176     // regions 1, 2 and 3
01177     cout << "closestPointOnTriangle failed" << endl;
01178     return false;
01179   }
01180 }
01181 
01182 
01183 
01184 /******************************************************************************
01185 Description:
01186   Compute the closest point for a target point in region 1.  Alg is:
01187   1. compute the scaled distances (ta) of the closest point on the edge.
01188   2. depending on the value, the closest point will be one of the adjacent
01189   vertices or somewhere on the edge.
01190 
01191 Arguments:
01192    planep - target point in plane
01193    va, vb - vertices of the triangle (see diagram below.
01194    ea - edge (see below)
01195    closest - return value of closest point
01196 
01197 Return:
01198    closest
01199    function returns edge_type (used to determine triangle class
01200    edge_type =
01201      0 -> point is closest to va
01202      1 -> closest to vb
01203      2 -> closest to ea
01204 
01205 ******************************************************************************/
01206 /*
01207       .                       .
01208          .                 .
01209             .           .
01210                . va  .
01211                   .     Region 1
01212                .     .
01213             .           . ea
01214          .                 .
01215       .                       . vb
01216    ............................................
01217 */
01218 
01219 inline int region1ClosestPoint( const Vec& planep,
01220                                 const Vec& va, const  Vec& vb, const Vec& ea,
01221                                 Vec& closest )
01222 {
01223   real ta = dot( (planep - va), ea ) / dot( ea, ea );
01224 
01225   if( ta >= 1 ) // then vb
01226   {
01227     closest << vb;
01228     return( 1 );
01229   }
01230   else if( ta <= 0 ) // then va
01231   {
01232     closest << va;
01233     return( 0 );
01234   }
01235   else // then ea
01236   {
01237     closest << ( va + ta*ea );
01238     return( 2 );
01239   }
01240 }
01241 
01242 /******************************************************************************
01243 Description:
01244   Compute the closest point for a target point in region 2.  Alg is:
01245   1. compute the scaled distances (ta, tb) of the closest point on each edge.
01246   2. depending on the value, the closest point will be one of the vertices
01247   or one of the two adjacent edges.
01248 
01249 Arguments:
01250    planep - target point in plane
01251    va, vb, vc - vertices of the triangle (see diagram below.
01252    ea, eb - edges (see below)
01253    closest - return value of closest point
01254 
01255 Return:
01256    closest
01257    function returns edge_type (used to determine triangle class
01258    edge_type =
01259      0 -> point is closest to va
01260      1 -> closest to vb
01261      2 -> closest to vc
01262      3 -> closest to ea
01263      4 -> closest to eb
01264 
01265 ******************************************************************************/
01266 /*
01267            .                       .
01268               .  Region 2       .
01269                  .           .
01270                     . vb  .
01271                        .
01272                     .     .
01273               ea .           . eb
01274               .                 .
01275            .                       . vc
01276     va  ............................................
01277 */
01278 
01279 inline int region2ClosestPoint( const Vec& planep,
01280                                 const Vec& va, const Vec& vb, const Vec& vc,
01281                                 const Vec& ea, const Vec& eb,
01282                                 Vec closest )
01283 {
01284   real ta = dot( planep - va, ea ) / dot( ea, ea );
01285   real tb = dot( planep - vb, eb ) / dot( eb, eb );
01286 
01287   if( ta <= 0 ) // then va
01288   {
01289     closest << va;
01290     return( 0 );
01291   }
01292   else if( tb >=1 ) // then vc
01293   {
01294     closest << vc;
01295     return( 2 );
01296   }
01297   else if( ta < 1 ) // then ea
01298   {
01299     closest << ( va + ta*ea );
01300     return( 3 );
01301   }
01302   else if( tb > 0 ) // then eb
01303   {
01304     closest << (vb + tb*eb );
01305     return( 4 );
01306   }
01307   else // then vb
01308   {
01309     closest << vb;
01310     return( 1 );
01311   }
01312 }
01313 
01314 
01315 
01316 
01317 
01318 
01319 
01320 
01321 
01322 
01323 
01324 
01325 
01326 
01327 
01328                               
01329 } // end of namespace PLearn
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines