cgma
|
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