PLearn 0.1
|
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