cgma
geometry.cpp
Go to the documentation of this file.
00001 #include "geometry.hpp"
00002 #include <cfloat>
00003 #include <iostream>
00004 #include <cassert>
00005 
00006 #include "options.hpp"
00007 
00008 std::ostream& operator<<(std::ostream& str, const Vector3d& v ){
00009   str << "(" << v.v[0] << ", " << v.v[1] << ", " << v.v[2] << ")";
00010   return str;
00011 }
00012 
00013 double matrix_det( double mat[9] ){
00014   return (mat[0]*mat[4]*mat[8] -
00015           mat[0]*mat[5]*mat[7] -
00016           mat[1]*mat[3]*mat[8] +
00017           mat[1]*mat[5]*mat[6] +
00018           mat[2]*mat[3]*mat[7] -
00019           mat[2]*mat[4]*mat[6]);
00020 }
00021 
00026 void Transform::set_rots_from_matrix( double raw_matrix[9], enum mat_format f ){
00027     
00028   double mat[3][3]  = {{ raw_matrix[0], raw_matrix[3], raw_matrix[6] },
00029                        { raw_matrix[1], raw_matrix[4], raw_matrix[7] },
00030                        { raw_matrix[2], raw_matrix[5], raw_matrix[8] } };
00031 
00032 
00033   if( f == C_STYLE ){
00034 
00035     /* row-major ordering: rewrite mat as
00036        mat = { { raw_matrix[0], raw_matrix[1], raw_matrix[2] }; 
00037                { raw_matrix[3], raw_matrix[4], raw_matrix[5] },
00038                { raw_matrix[6], raw_matrix[7], raw_matrix[8] } };
00039     */
00040 
00041     mat[0][1] = raw_matrix [1];
00042     mat[0][2] = raw_matrix [2];
00043     mat[1][0] = raw_matrix [3];
00044     mat[1][2] = raw_matrix [5];
00045     mat[2][0] = raw_matrix [6];
00046     mat[2][1] = raw_matrix [7];
00047 
00048   }
00049 
00050   double det = matrix_det(raw_matrix); // determinant is the same regardless of ordering
00051 
00052   if( OPT_DEBUG ){
00053     std::cout << "Constructing rotation: " << std::endl;
00054     for( int i = 0; i < 3; i++ ){
00055       std::cout << "  [ ";
00056       for ( int j = 0; j < 3; j++ ){
00057         std::cout << mat[i][j] << " ";
00058       }
00059       std::cout << "]" << std::endl;
00060     }
00061     std::cout << "  det = " << det << std::endl;
00062   }
00063 
00064   if( det < 0.0 ){
00065     // negative determinant-> this transformation contains a reflection.
00066     invert = true;
00067     det *= -1;
00068     for( int i = 0; i < 9; i++){  mat[i/3][i%3] = -mat[i/3][i%3]; } 
00069     if( OPT_DEBUG ) std::cout << "  negative determinant => improper rotation (adding inversion)" << std::endl;
00070   }
00071   
00072   if( fabs( det - 1.0 ) > DBL_EPSILON ){
00073     std::cout << "Warning: determinant of rotation matrix " << det << " != +-1" << std::endl;
00074   }
00075 
00076   /* Older, more straightforward approach:
00077     theta = acos( (mat[0][0] + mat[1][1] + mat[2][2] - 1) / 2 );
00078     double twoSinTheta = 2 * sin(theta);
00079     axis.v[0] = ( mat[2][1]-mat[1][2]) / twoSinTheta;
00080     axis.v[1] = ( mat[0][2]-mat[2][0]) / twoSinTheta;
00081     axis.v[2] = ( mat[1][0]-mat[0][1]) / twoSinTheta;
00082   */
00083 
00084   /* I have switched from the simple implementation above to the more robust and complex
00085    * approach below.  It has better numerical stability and (what is more important)
00086    * handles correctly the cases where theta is a multiple of 180 degrees.
00087    * See also
00088    * en.wikipedia.org/wiki/Rotation_matrix#Axis_and_angle (for why to use atan2 instead of acos)
00089    * www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm (for handling 180-degree case)
00090    */
00091 
00092   double x = mat[2][1]-mat[1][2];
00093   double y = mat[0][2]-mat[2][0];
00094   double z = mat[1][0]-mat[0][1];
00095   double r = hypot( x, hypot( y,z ));
00096   double t = mat[0][0] + mat[1][1] + mat[2][2];
00097   theta = atan2(r,t-1);
00098   
00099   if( OPT_DEBUG ){
00100     std:: cout << "  r = " << r << " t = " << t << " theta = " << theta << std::endl;
00101     std:: cout << "  x = " << x << " y = " << y << " z = " << z << std::endl;
00102   }
00103 
00104   if( std::fabs(theta) <= DBL_EPSILON ){ 
00105     // theta is 0 or extremely close to it, so let's say there's no rotation after all.
00106     has_rot = false; 
00107     axis = Vector3d(); // zero vector
00108     if( OPT_DEBUG ) std::cout << "  (0) "; // std::endl comes below
00109   }
00110   else if( std::fabs( theta - M_PI ) <= DBL_EPSILON ){
00111     // theta is pi (180 degrees) or extremely close to it
00112     // find the column of mat with the largest diagonal
00113     int col = 0;
00114     if( mat[1][1] > mat[col][col] ){ col = 1; }
00115     if( mat[2][2] > mat[col][col] ){ col = 2; }
00116 
00117     axis.v[col] = sqrt( (mat[col][col]+1)/2 ); 
00118     double denom = 2*axis.v[col];
00119     axis.v[(col+1)%3] = mat[col][(col+1)%3] / denom;
00120     axis.v[(col+2)%3] = mat[col][(col+2)%3] / denom;
00121 
00122     if( OPT_DEBUG ) std::cout << "  (180) "; // std::endl comes below
00123 
00124   }
00125   else{ 
00126     // standard case: theta isn't 0 or 180
00127     axis.v[0] = x / r;
00128     axis.v[1] = y / r;
00129     axis.v[2] = z / r;
00130   }
00131 
00132 
00133   // convert theta back to degrees, as used by iGeom
00134   theta *= 180.0 / M_PI;
00135   
00136   if( OPT_DEBUG ) std::cerr << "computed rotation: " << *this << std::endl;
00137 
00138 }
00139 
00140 Transform::Transform( double rot[9], const Vector3d& trans, enum mat_format f ) :
00141   translation(trans), has_rot(true), invert(false)
00142 {
00143   set_rots_from_matrix( rot, f );
00144 }
00145 
00146 Transform::Transform( const std::vector< double >& inputs,  bool degree_format_p, enum mat_format f ) : 
00147   has_rot(false), invert(false)
00148 {
00149   
00150   size_t num_inputs = inputs.size();
00151   
00152   // translation is always defined by first three inputs
00153   translation = Vector3d(inputs); 
00154 
00155   if( num_inputs == 9 ||                         // translation matrix with third vector missing
00156       num_inputs == 12 || num_inputs == 13 )  // translation matrix fully specified
00157     {
00158     
00159       has_rot = true;
00160       double raw_matrix[9];
00161     
00162     if( num_inputs == 9 ){
00163       for( int i = 3; i < 9; ++i){
00164         raw_matrix[i-3] = degree_format_p ? cos(inputs.at(i) * M_PI / 180.0 ) : inputs.at(i);
00165       }
00166       
00167       Vector3d v1( raw_matrix ); //v1 = v1.normalize();
00168       Vector3d v2( raw_matrix+3 ); //v2 = v2.normalize();
00169       Vector3d v3 = v1.cross(v2);//.normalize();
00170       raw_matrix[6] = v3.v[0];
00171       raw_matrix[7] = v3.v[1];
00172       raw_matrix[8] = v3.v[2];
00173     }
00174     else{
00175       for( int i = 3; i < 12; ++i){
00176         raw_matrix[i-3] = degree_format_p ? cos(inputs.at(i) * M_PI / 180.0 ) : inputs.at(i);
00177       }
00178       if( num_inputs == 13 && inputs.at(12) == -1.0 ){
00179         std::cout << "Notice: a transformation has M = -1.  Inverting the translation;" << std::endl;
00180         std::cout << " though this might not be what you wanted." << std::endl;
00181         translation = -translation;
00182       }
00183     }
00184 
00185     set_rots_from_matrix( raw_matrix, f );
00186     
00187   }
00188   else if( num_inputs != 3 ){
00189     // an unsupported number of transformation inputs
00190     std::cerr << "Warning: transformation with " << num_inputs << " input items is unsupported" << std::endl;
00191     std::cerr << "  (will pretend there's no rotation: expect incorrect geometry.)" << std::endl;
00192   }
00193   
00194 }  
00195 
00196 Transform Transform::reverse() const {
00197   Transform t;
00198   t.translation = -this->translation;
00199   t.has_rot = this->has_rot;
00200   t.invert = this->invert;
00201   t.axis = -this->axis;
00202   t.theta = this->theta;
00203   return t;
00204 }
00205 
00206 void Transform::print( std::ostream& str ) const{
00207   str << "[trans " << translation;
00208   if(has_rot){
00209     str << "(" << theta << ":" << axis << ")";
00210   }
00211   if(invert){
00212     str << "(I)";
00213   }
00214   str << "]";
00215 }
00216 
00217 
00218 
00219 std::ostream& operator<<( std::ostream& str, const Transform& t ){
00220   t.print(str);
00221   return str;
00222 }
00223 
00224 size_t Fill::indicesToSerialIndex( int x, int y, int z ) const {
00225   int grid_x = x - xrange.first;
00226   int grid_y = y - yrange.first;
00227   int grid_z = z - zrange.first;
00228 
00229   int dx = xrange.second - xrange.first + 1;
00230   int dy = yrange.second - yrange.first + 1;
00231   //  int dz = zrange.second - zrange.first;
00232   
00233   int index = grid_z * (dy*dx) + grid_y * dx + grid_x;
00234 
00235   assert( index >= 0 && (unsigned)(index) <= nodes.size() );
00236   return static_cast<size_t>( index );
00237 }
00238 
00239 const FillNode& Fill::getOriginNode() const { 
00240   if( !has_grid ){
00241     return nodes.at(0); 
00242   }
00243   else return nodes.at(indicesToSerialIndex( 0, 0, 0));
00244 }
00245 
00246 const FillNode& Fill::getNode( int x, int y, int z ) const {
00247   assert( has_grid );
00248   return nodes.at( indicesToSerialIndex(x, y, z) );
00249 }
00250 
00251 
00252 
00253 
00254 
00255 
00256 Lattice::Lattice( int dims, const Vector3d& v1_p, const Vector3d& v2_p, const Vector3d& v3_p, const FillNode& node ) : 
00257   num_finite_dims(dims), v1(v1_p), v2(v2_p), v3(v3_p), fill(new ImmediateRef<Fill>( Fill(node) ))
00258 {}
00259 
00260 Lattice::Lattice( int dims, const Vector3d& v1_p, const Vector3d& v2_p, const Vector3d& v3_p, const Fill& fill_p ) :
00261   num_finite_dims(dims), v1(v1_p), v2(v2_p), v3(v3_p), fill(new PointerRef<Fill>(&fill_p) )
00262 {}
00263 
00264 
00265 Lattice::Lattice( const Lattice& l ) :
00266   num_finite_dims( l.num_finite_dims ), v1( l.v1 ), v2( l.v2 ), v3( l.v3 ), fill( l.fill->clone() )
00267 {}
00268 
00269 Lattice& Lattice::operator=( const Lattice& l ){
00270   if( this != &l ){
00271 
00272     num_finite_dims = l.num_finite_dims;
00273     fill = l.fill->clone();
00274     v1 = l.v1;
00275     v2 = l.v2;
00276     v3 = l.v3;
00277     
00278   }
00279   return *this;
00280 }
00281 
00282 
00283 Transform Lattice::getTxForNode( int x, int y, int z ) const {
00284 
00285   Vector3d v;
00286   switch( num_finite_dims ){
00287   case 3:
00288     v = v3 * z; // fallthrough
00289   case 2:
00290     v = v + v2 * y; // fallthrough
00291   case 1:
00292     v = v + v1 * x;
00293   default:
00294     break;
00295   }
00296 
00297   return Transform(v);
00298 }
00299 
00300 const FillNode& Lattice::getFillForNode( int x, int y, int z ) const {
00301   if( fill->getData().has_grid ){
00302     return fill->getData().getNode( x, y, z );
00303   }
00304   else{
00305     return fill->getData().getOriginNode();
00306   }
00307 }
00308 
00309   
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines