Main Page | Modules | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members

transform.cpp

Go to the documentation of this file.
00001 
00005 /*
00006  * Author: Steven Ludtke, 04/10/2003 (sludtke@bcm.edu)
00007  * Copyright (c) 2000-2006 Baylor College of Medicine
00008  *
00009  * This software is issued under a joint BSD/GNU license. You may use the
00010  * source code in this file under either license. However, note that the
00011  * complete EMAN2 and SPARX software packages have some GPL dependencies,
00012  * so you are responsible for compliance with the licenses of these packages
00013  * if you opt to use BSD licensing. The warranty disclaimer below holds
00014  * in either instance.
00015  *
00016  * This complete copyright notice must be included in any revised version of the
00017  * source code. Additional authorship citations may be added, but existing
00018  * author citations must be preserved.
00019  *
00020  * This program is free software; you can redistribute it and/or modify
00021  * it under the terms of the GNU General Public License as published by
00022  * the Free Software Foundation; either version 2 of the License, or
00023  * (at your option) any later version.
00024  *
00025  * This program is distributed in the hope that it will be useful,
00026  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00028  * GNU General Public License for more details.
00029  *
00030  * You should have received a copy of the GNU General Public License
00031  * along with this program; if not, write to the Free Software
00032  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033  *
00034  * */
00035 
00036 #include "transform.h"
00037 #include "util.h"
00038 #include "emobject.h"
00039 #include <cctype> // for std::tolower
00040 #include <cstring>  // for memcpy
00041 #include "symmetry.h"
00042 using namespace EMAN;
00043 
00044 #ifdef WIN32
00045 #ifndef M_PI
00046 #define M_PI 3.14159265358979323846
00047 #endif
00048 #endif
00049 
00050 #include <algorithm> // for std::transform
00051 
00052 #include <gsl_matrix.h>
00053 #include <gsl_blas.h>
00054 #include <gsl_linalg.h>
00055 
00056 #include <ostream>
00057 using std::ostream_iterator;
00058 
00059 const float Transform3D::ERR_LIMIT = 0.000001f;
00060 
00061 const float Transform::ERR_LIMIT = 0.000001f;
00062 
00063 vector<string>  Transform::permissable_2d_not_rot;
00064 vector<string>  Transform::permissable_3d_not_rot;
00065 map<string,vector<string> >  Transform::permissable_rot_keys;
00066 
00067 Transform Transform::icos_5_to_2() {
00068         Transform t;
00069         Dict d;
00070         d["type"] = "eman";
00071         d["phi"] = 0.0f;
00072         d["az"] = 270.0f;
00073         d["alt"] = 58.282523f; // 5 fold to a 3 fold to a 2 fold
00079         t.set_rotation(d);
00080         return t;
00081 }
00082 
00083 Transform Transform::tet_3_to_2() {
00084         Transform t;
00085         Dict d;
00086         d["type"] = "eman";
00087         d["phi"] = 45.0f;
00088         d["az"] = 0.0f;
00089         d["alt"] = 54.73561f; // 3 fold to a 2 fold
00093         t.set_rotation(d);
00094         return t;
00095 }
00096 
00097 
00098 Transform::Transform()
00099 {
00100         to_identity();
00101 }
00102 
00103 Transform::Transform( const Transform& that )
00104 {
00105         *this = that;
00106 }
00107 
00108 Transform& Transform::operator=(const Transform& that ) {
00109 
00110         if (this != &that ) {
00111                 memcpy(matrix,that.matrix,12*sizeof(float));
00112 //              transform_type = that.transform_type;
00113         }
00114         return *this;
00115 }
00116 
00117 Transform::Transform(const Dict& d)  {
00118         to_identity();
00119         set_params(d);
00120 }
00121 
00122 
00123 Transform::Transform(const float array[12]) {
00124         memcpy(matrix,array,12*sizeof(float));
00125 }
00126 
00127 Transform::Transform(const vector<float> array)
00128 {
00129         set_matrix(array);
00130 }
00131 
00132 void Transform::set_matrix(const vector<float>& v)
00133 {
00134         if (v.size() != 12 ) throw InvalidParameterException("The construction array must be of size 12");
00135 
00136         for(int i=0; i<3; ++i) {
00137                 for(int j=0; j<4; ++j) {
00138                         matrix[i][j] = v[i*4+j];
00139                 }
00140         }
00141 }
00142 
00143 void Transform::copy_matrix_into_array(float* const array) const {
00144 
00145         int idx = 0;
00146         for(int i=0; i<3; ++i) {
00147                 for(int j=0; j<4; ++j) {
00148                         array[idx] = matrix[i][j];
00149                         idx ++;
00150                 }
00151         }
00152 }
00153 
00154 vector<float> Transform::get_matrix() const
00155 {
00156         vector<float> ret(12);
00157         for(int i=0; i<3; ++i) {
00158                 for(int j=0; j<4; ++j) {
00159                         ret[i*4+j] = matrix[i][j];
00160                 }
00161         }
00162         return ret;
00163 }
00164 
00165 void Transform::to_identity()
00166 {
00167 //      transform_type = UNKNOWN;
00168         for(int i=0; i<3; ++i) {
00169                 for(int j=0; j<4; ++j) {
00170                         if(i==j) {
00171                                 matrix[i][j] = 1;
00172                         }
00173                         else {
00174                                 matrix[i][j] = 0;
00175                         }
00176                 }
00177         }
00178 }
00179 
00180 bool Transform::is_identity() const {
00181         for(int i=0; i<3; ++i) {
00182                 for(int j=0; j<4; ++j) {
00183                         float c = matrix[i][j];
00184                         Util::apply_precision(c,ERR_LIMIT);
00185                         if(i==j) {
00186                                 if (c != 1.0) return false;
00187                         }
00188                         else {
00189                                 if (c != 0.0) return false;
00190                         }
00191                 }
00192         }
00193         return true;
00194 }
00195 
00196 
00197 void Transform::set_params(const Dict& d) {
00198         detect_problem_keys(d);
00199 
00200         if (d.has_key_ci("type") ) set_rotation(d);
00201 
00202         if (d.has_key_ci("scale")) {
00203                 float scale = static_cast<float>(d.get_ci("scale"));
00204                 set_scale(scale);
00205         }
00206 
00207         float dx=0,dy=0,dz=0;
00208 
00209         if (d.has_key_ci("tx")) dx = static_cast<float>(d.get_ci("tx"));
00210         if (d.has_key_ci("ty")) dy = static_cast<float>(d.get_ci("ty"));
00211         if (d.has_key_ci("tz")) dz = static_cast<float>(d.get_ci("tz"));
00212 
00213         if ( dx != 0.0 || dy != 0.0 || dz != 0.0 ) {
00214                 set_trans(dx,dy,dz);
00215         }
00216 
00217         if (d.has_key_ci("mirror")) {
00218                 EMObject e = d.get_ci("mirror");
00219                 if ( (e.get_type() != EMObject::BOOL ) && (e.get_type() != EMObject::INT ) && (e.get_type() != EMObject::UNSIGNEDINT ) )
00220                         throw InvalidParameterException("Error, mirror must be a bool or an int");
00221 
00222                 bool mirror = static_cast<bool>(e);
00223                 set_mirror(mirror);
00224         }
00225 }
00226 
00227 
00228 void Transform::init_permissable_keys()
00229 {
00230 
00231         permissable_2d_not_rot.push_back("tx");
00232         permissable_2d_not_rot.push_back("ty");
00233         permissable_2d_not_rot.push_back("scale");
00234         permissable_2d_not_rot.push_back("mirror");
00235         permissable_2d_not_rot.push_back("type");
00236 
00237         permissable_3d_not_rot.push_back("tx");
00238         permissable_3d_not_rot.push_back("ty");
00239         permissable_3d_not_rot.push_back("tz");
00240         permissable_3d_not_rot.push_back("scale");
00241         permissable_3d_not_rot.push_back("mirror");
00242         permissable_3d_not_rot.push_back("type");
00243 
00244         vector<string> tmp;
00245         tmp.push_back("alpha");
00246         permissable_rot_keys["2d"] = tmp;
00247 
00248         tmp.clear();
00249         tmp.push_back("alt");
00250         tmp.push_back("az");
00251         tmp.push_back("phi");
00252         permissable_rot_keys["eman"] = tmp;
00253 
00254         tmp.clear();
00255         tmp.push_back("psi");
00256         tmp.push_back("theta");
00257         tmp.push_back("phi");
00258         permissable_rot_keys["spider"] = tmp;
00259 
00260         tmp.clear();
00261         tmp.push_back("alpha");
00262         tmp.push_back("beta");
00263         tmp.push_back("gamma");
00264         permissable_rot_keys["imagic"] = tmp;
00265 
00266         tmp.clear();
00267         tmp.push_back("ztilt");
00268         tmp.push_back("xtilt");
00269         tmp.push_back("ytilt");
00270         permissable_rot_keys["xyz"] = tmp;
00271 
00272         tmp.clear();
00273         tmp.push_back("phi");
00274         tmp.push_back("theta");
00275         tmp.push_back("omega");
00276         permissable_rot_keys["mrc"] = tmp;
00277 
00278         tmp.clear();
00279         tmp.push_back("e0");
00280         tmp.push_back("e1");
00281         tmp.push_back("e2");
00282         tmp.push_back("e3");
00283         permissable_rot_keys["quaternion"] = tmp;
00284 
00285         tmp.clear();
00286         tmp.push_back("n1");
00287         tmp.push_back("n2");
00288         tmp.push_back("n3");
00289         tmp.push_back("Omega");
00290         permissable_rot_keys["spin"] = tmp;
00291 
00292         tmp.clear();
00293         tmp.push_back("n1");
00294         tmp.push_back("n2");
00295         tmp.push_back("n3");
00296         tmp.push_back("q");
00297         permissable_rot_keys["sgirot"] = tmp;
00298 
00299         tmp.clear();
00300         tmp.push_back("m11");
00301         tmp.push_back("m12");
00302         tmp.push_back("m13");
00303         tmp.push_back("m21");
00304         tmp.push_back("m22");
00305         tmp.push_back("m23");
00306         tmp.push_back("m31");
00307         tmp.push_back("m32");
00308         tmp.push_back("m33");
00309         permissable_rot_keys["matrix"] = tmp;
00310 }
00311 
00312 
00313 void Transform::detect_problem_keys(const Dict& d) {
00314         if (permissable_rot_keys.size() == 0 ) {
00315                 init_permissable_keys();
00316         }
00317 
00318         vector<string> verification;
00319         vector<string> problem_keys;
00320         bool is_2d = false;
00321         if (d.has_key_ci("type") ) {
00322                 string type = Util::str_to_lower((string)d["type"]);
00323                 bool problem = false;
00324                 if (permissable_rot_keys.find(type) == permissable_rot_keys.end() ) {
00325                         problem_keys.push_back(type);
00326                         problem = true;
00327                 }
00328                 if ( !problem ) {
00329                         vector<string> perm = permissable_rot_keys[type];
00330                         std::copy(perm.begin(),perm.end(),back_inserter(verification));
00331 
00332                         if ( type == "2d" ) {
00333                                 is_2d = true;
00334                                 std::copy(permissable_2d_not_rot.begin(),permissable_2d_not_rot.end(),back_inserter(verification));
00335                         }
00336                 }
00337         }
00338         if ( !is_2d ) {
00339                 std::copy(permissable_3d_not_rot.begin(),permissable_3d_not_rot.end(),back_inserter(verification));
00340         }
00341 
00342         for (Dict::const_iterator it = d.begin(); it != d.end();  ++it) {
00343                 if ( std::find(verification.begin(),verification.end(), it->first) == verification.end() ) {
00344                         problem_keys.push_back(it->first);
00345                 }
00346         }
00347 
00348         if (problem_keys.size() != 0 ) {
00349                 string error;
00350                 if (problem_keys.size() == 1) {
00351                         error = "Transform Error: The \"" +problem_keys[0]+ "\" key is unsupported";
00352                 } else {
00353                         error = "Transform Error: The ";
00354                         for(vector<string>::const_iterator cit = problem_keys.begin(); cit != problem_keys.end(); ++cit ) {
00355                                 if ( cit != problem_keys.begin() ) {
00356                                         if (cit == (problem_keys.end() -1) ) error += " and ";
00357                                         else error += ", ";
00358                                 }
00359                                 error += "\"";
00360                                 error += *cit;
00361                                 error += "\"";
00362                         }
00363                         error += " keys are unsupported";
00364                 }
00365                 throw InvalidParameterException(error);
00366         }
00367 }
00368 
00369 void Transform::set_params_inverse(const Dict& d) {
00370         detect_problem_keys(d);
00371 
00372         if (d.has_key_ci("type") ) set_rotation(d);
00373 
00374         float dx=0,dy=0,dz=0;
00375         if (d.has_key_ci("tx")) dx = static_cast<float>(d.get_ci("tx"));
00376         if (d.has_key_ci("ty")) dy = static_cast<float>(d.get_ci("ty"));
00377         if (d.has_key_ci("tz")) dz = static_cast<float>(d.get_ci("tz"));
00378 
00379         if ( (dx != 0.0 || dy != 0.0 || dz != 0.0) && d.has_key_ci("type") ) {
00380                 Transform pre_trans;
00381                 pre_trans.set_trans(dx,dy,dz);
00382 
00383                 Transform tmp;
00384                 tmp.set_rotation(d);
00385 
00386                 if (d.has_key_ci("scale")) {
00387                         float scale = static_cast<float>(d.get_ci("scale"));
00388                         tmp.set_scale(scale);
00389                 }
00390 
00391                 Transform solution_trans = tmp*pre_trans;
00392 
00393                 if (d.has_key_ci("scale")) {
00394                         Transform tmp;
00395                         float scale = static_cast<float>(d.get_ci("scale"));
00396                         tmp.set_scale(scale);
00397                         solution_trans = solution_trans*tmp;
00398                 }
00399 
00400                 tmp = Transform();
00401                 tmp.set_rotation(d);
00402                 solution_trans = solution_trans*tmp;
00403                 set_trans(solution_trans.get_trans());
00404         }
00405 
00406         if (d.has_key_ci("scale")) {
00407                 float scale = static_cast<float>(d.get_ci("scale"));
00408                 set_scale(scale);
00409         }
00410 
00411         if (d.has_key_ci("mirror")) {
00412                 EMObject e = d.get_ci("mirror");
00413                 if ( (e.get_type() != EMObject::BOOL ) && (e.get_type() != EMObject::INT ) && (e.get_type() != EMObject::UNSIGNEDINT ) )
00414                         throw InvalidParameterException("Error, mirror must be a bool or an int");
00415 
00416                 bool mirror = static_cast<bool>(e);
00417                 set_mirror(mirror);
00418         }
00419         invert();
00420 }
00421 
00422 
00423 Dict Transform::get_params(const string& euler_type) const {
00424         Dict params = get_rotation(euler_type);
00425 
00426         Vec3f v = get_trans();
00427         params["tx"] = v[0]; params["ty"] = v[1];
00428 
00429         string type = Util::str_to_lower(euler_type);
00430         if ( type != "2d") params["tz"] = v[2];
00431 
00432         float scale = get_scale();
00433         params["scale"] = scale;
00434 
00435         bool mirror = get_mirror();
00436         params["mirror"] = mirror;
00437 
00438         return params;
00439 }
00440 
00441 
00442 
00443 Dict Transform::get_params_inverse(const string& euler_type) const {
00444         Transform inv(inverse());
00445 
00446         Dict params = inv.get_rotation(euler_type);
00447         Vec3f v = inv.get_pre_trans();
00448         params["tx"] = v[0]; params["ty"] = v[1];
00449 
00450         string type = Util::str_to_lower(euler_type);
00451         if ( type != "2d") params["tz"] = v[2];
00452 
00453         float scale = inv.get_scale();
00454         params["scale"] = scale;
00455 
00456         bool mirror = inv.get_mirror();
00457         params["mirror"] = mirror;
00458 
00459         return params;
00460 }
00461 
00462 // Dict Transform::get_params_2d() {
00463 //      Dict params = get_rotation("2d");
00464 //
00465 //      Vec2f v = get_trans_2d();
00466 //      params["tx"] = v[0]; params["ty"] = v[1];
00467 //
00468 //      float scale = get_scale();
00469 //      params["scale"] = scale;
00470 //
00471 //      bool mirror = get_mirror();
00472 //      params["mirror"] = mirror;
00473 //
00474 //      return params;
00475 // }
00476 
00477 
00478 void Transform::set_rotation(const Dict& rotation)
00479 {
00480         detect_problem_keys(rotation);
00481         string euler_type;
00482 
00483         if (!rotation.has_key_ci("type") ){
00484                         throw InvalidParameterException("argument dictionary does not contain the type key");
00485         }
00486 
00487         euler_type = static_cast<string>(rotation.get_ci("type"));// Warning, will throw
00488 
00489 
00490         float e0=0;float e1=0; float e2=0; float e3=0;
00491         float Omega=0;
00492         float az  = 0;
00493         float alt = 0;
00494         float phi = 0;
00495         float cxtilt = 0;
00496         float sxtilt = 0;
00497         float cytilt = 0;
00498         float sytilt = 0;
00499         bool is_quaternion = 0;
00500         bool is_matrix = 0;
00501 
00502         bool x_mirror;
00503         float scale;
00504         // Get these before anything changes so we can apply them again after the rotation is set
00505         get_scale_and_mirror(scale,x_mirror);
00506         if (scale == 0) throw UnexpectedBehaviorException("The determinant of the Transform is 0. This is unexpected.");
00507 
00508         string type = Util::str_to_lower(euler_type);
00509         if (type == "2d") {
00510                 assert_valid_2d();
00511                 az  = 0;
00512                 alt = 0;
00513                 phi = (float)rotation["alpha"] ;
00514         } else if ( type == "eman" ) {
00515 //              validate_and_set_type(THREED);
00516                 az  = (float)rotation["az"] ;
00517                 alt = (float)rotation["alt"]  ;
00518                 phi = (float)rotation["phi"] ;
00519         } else if ( type == "imagic" ) {
00520 //              validate_and_set_type(THREED);
00521                 az  = (float)rotation["alpha"] ;
00522                 alt = (float)rotation["beta"]  ;
00523                 phi = (float)rotation["gamma"] ;
00524         } else if ( type == "spider" ) {
00525 //              validate_and_set_type(THREED);
00526                 az =  (float)rotation["phi"]    + 90.0f;
00527                 alt = (float)rotation["theta"] ;
00528                 phi = (float)rotation["psi"]    - 90.0f;
00529         } else if ( type == "xyz" ) {
00530 //              validate_and_set_type(THREED);
00531                 cxtilt = cos( (M_PI/180.0f)*(float)rotation["xtilt"]);
00532                 sxtilt = sin( (M_PI/180.0f)*(float)rotation["xtilt"]);
00533                 cytilt = cos( (M_PI/180.0f)*(float)rotation["ytilt"]);
00534                 sytilt = sin( (M_PI/180.0f)*(float)rotation["ytilt"]);
00535                 az =  (180.0f/M_PI)*atan2(-cytilt*sxtilt,sytilt)   + 90.0f ;
00536                 alt = (180.0f/M_PI)*acos(cytilt*cxtilt)  ;
00537                 phi = (float)rotation["ztilt"] +(180.0f/M_PI)*atan2(sxtilt,cxtilt*sytilt)   - 90.0f ;
00538         } else if ( type == "mrc" ) {
00539 //              validate_and_set_type(THREED);
00540                 az  = (float)rotation["phi"]   + 90.0f ;
00541                 alt = (float)rotation["theta"] ;
00542                 phi = (float)rotation["omega"] - 90.0f ;
00543         } else if ( type == "quaternion" ) {
00544 //              validate_and_set_type(THREED);
00545                 is_quaternion = 1;
00546                 e0 = (float)rotation["e0"];
00547                 e1 = (float)rotation["e1"];
00548                 e2 = (float)rotation["e2"];
00549                 e3 = (float)rotation["e3"];
00550         } else if ( type == "spin" ) {
00551 //              validate_and_set_type(THREED);
00552                 is_quaternion = 1;
00553                 Omega = (float)rotation["Omega"];
00554                 e0 = cos(Omega*M_PI/360.0f);
00555                 e1 = sin(Omega*M_PI/360.0f)* (float)rotation["n1"];
00556                 e2 = sin(Omega*M_PI/360.0f)* (float)rotation["n2"];
00557                 e3 = sin(Omega*M_PI/360.0f)* (float)rotation["n3"];
00558         } else if ( type == "sgirot" ) {
00559 //              validate_and_set_type(THREED);
00560                 is_quaternion = 1;
00561                 Omega = (float)rotation["q"] ;
00562                 e0 = cos(Omega*M_PI/360.0f);
00563                 e1 = sin(Omega*M_PI/360.0f)* (float)rotation["n1"];
00564                 e2 = sin(Omega*M_PI/360.0f)* (float)rotation["n2"];
00565                 e3 = sin(Omega*M_PI/360.0f)* (float)rotation["n3"];
00566         } else if ( type == "matrix" ) {
00567                 is_matrix = 1;
00568                 matrix[0][0] = (float)rotation["m11"];
00569                 matrix[0][1] = (float)rotation["m12"];
00570                 matrix[0][2] = (float)rotation["m13"];
00571                 matrix[1][0] = (float)rotation["m21"];
00572                 matrix[1][1] = (float)rotation["m22"];
00573                 matrix[1][2] = (float)rotation["m23"];
00574                 matrix[2][0] = (float)rotation["m31"];
00575                 matrix[2][1] = (float)rotation["m32"];
00576                 matrix[2][2] = (float)rotation["m33"];
00577         } else {
00578 //              transform_type = UNKNOWN;
00579                 throw InvalidStringException(euler_type, "unknown Euler Type");
00580         }
00581 
00582         float azp  = fmod(az,360.0f)*M_PI/180.0f;
00583         float altp  = alt*M_PI/180.0f;
00584         float phip = fmod(phi,360.0f)*M_PI/180.0f;
00585 
00586         if (!is_quaternion && !is_matrix) {
00587                 matrix[0][0] =  cos(phip)*cos(azp) - cos(altp)*sin(azp)*sin(phip);
00588                 matrix[0][1] =  cos(phip)*sin(azp) + cos(altp)*cos(azp)*sin(phip);
00589                 matrix[0][2] =  sin(altp)*sin(phip);
00590                 matrix[1][0] = -sin(phip)*cos(azp) - cos(altp)*sin(azp)*cos(phip);
00591                 matrix[1][1] = -sin(phip)*sin(azp) + cos(altp)*cos(azp)*cos(phip);
00592                 matrix[1][2] =  sin(altp)*cos(phip);
00593                 matrix[2][0] =  sin(altp)*sin(azp);
00594                 matrix[2][1] = -sin(altp)*cos(azp);
00595                 matrix[2][2] =  cos(altp);
00596         }
00597         if (is_quaternion){
00598                 matrix[0][0] = e0 * e0 + e1 * e1 - e2 * e2 - e3 * e3;
00599                 matrix[0][1] = 2.0f * (e1 * e2 + e0 * e3);
00600                 matrix[0][2] = 2.0f * (e1 * e3 - e0 * e2);
00601                 matrix[1][0] = 2.0f * (e2 * e1 - e0 * e3);
00602                 matrix[1][1] = e0 * e0 - e1 * e1 + e2 * e2 - e3 * e3;
00603                 matrix[1][2] = 2.0f * (e2 * e3 + e0 * e1);
00604                 matrix[2][0] = 2.0f * (e3 * e1 + e0 * e2);
00605                 matrix[2][1] = 2.0f * (e3 * e2 - e0 * e1);
00606                 matrix[2][2] = e0 * e0 - e1 * e1 - e2 * e2 + e3 * e3;
00607                 // keep in mind matrix[0][2] is M13 gives an e0 e2 piece, etc
00608         }
00609 
00610         // Apply scale if it existed previously
00611         if (scale != 1.0f) {
00612                 for(int i=0; i<3; ++i) {
00613                         for(int j=0; j<3; ++j) {
00614                                 matrix[i][j] *= scale;
00615                         }
00616                 }
00617         }
00618 
00619         // Apply post x mirroring if it was applied previouslys
00620         if ( x_mirror ) {
00621                 for(int j=0; j<3; ++j) {
00622                         matrix[0][j] *= -1.0f;
00623                 }
00624         }
00625 }
00626 
00627 Transform Transform::get_rotation_transform() const
00628 {
00629         Transform ret(*this);
00630         ret.set_scale(1.0);
00631         ret.set_mirror(false);
00632         ret.set_trans(0,0,0);
00633         //ret.orthogonalize(); // ?
00634         return ret;
00635 }
00636 
00637 void Transform::set_rotation(const Vec3f & v)
00638 {
00639         if ( v[0] == 0 && v[1] == 0 && v[2] == 0 )
00640                 throw UnexpectedBehaviorException("Can't set rotation for the null vector");
00641 
00642         Vec3f v1(v);
00643         v1.normalize();
00644 
00645         float theta = acos(v1[2]); // in radians
00646         float psi = atan2(v1[1],-v1[0]);
00647 //      if (v1[1] != 0) psi = asin(v1[1]/sin(theta)); // in radians
00648 
00649         Dict d;
00650         d["theta"] = (float)EMConsts::rad2deg*theta;
00651         d["psi"] = (float)EMConsts::rad2deg*psi;
00652         d["phi"] = (float)0.0;
00653         d["type"] = "spider";
00654 
00655         set_rotation(d);
00656 
00657 
00658 }
00659 
00660 Transform Transform::negate() const
00661 {
00662         Transform t(*this);
00663         for(unsigned int i = 0; i < 3; ++i) {
00664                 for(unsigned int j = 0; j < 4; ++j) {
00665                         t.set(i,j,t[i][j]*-1);
00666                 }
00667         }
00668         return t;
00669 }
00670 
00671 Transform Transform::get_hflip_transform() const {
00672 
00673         Dict rot = get_rotation("eman");
00674         rot["alt"] = 180.0f + static_cast<float>(rot["alt"]);
00675         rot["phi"] = 180.0f - static_cast<float>(rot["phi"]);
00676 
00677 
00678 
00679         Transform ret(*this); // Is the identity
00680         ret.set_rotation(rot);
00681 
00682         Vec3f trans = get_trans();
00683         trans[0] = -trans[0];
00684         ret.set_trans(trans);
00685 
00686 //      ret.set_mirror(self.get_mirror());
00687 
00688         return ret;
00689 }
00690 
00691 Transform Transform::get_vflip_transform() const {
00692 
00693         Dict rot = get_rotation("eman");
00694         rot["alt"] = 180.0f + static_cast<float>(rot["alt"]);
00695         rot["phi"] = - static_cast<float>(rot["phi"]);
00696 
00697         Transform ret(*this);
00698         ret.set_rotation(rot);
00699 
00700         Vec3f trans = get_trans();
00701         trans[1] = -trans[1];
00702         ret.set_trans(trans);
00703 
00704         return ret;
00705 }
00706 
00707 Dict Transform::get_rotation(const string& euler_type) const
00708 {
00709         Dict result;
00710 
00711         float max = 1 - ERR_LIMIT;
00712         float scale;
00713         bool x_mirror;
00714         get_scale_and_mirror(scale,x_mirror);
00715         if (scale == 0) throw UnexpectedBehaviorException("The determinant of the Transform is 0. This is unexpected.");
00716         float cosalt=matrix[2][2]/scale;
00717         float x_mirror_scale = (x_mirror ? -1.0f : 1.0f);
00718         float inv_scale = 1.0f/scale;
00719 
00720         float az=0;
00721         float alt = 0;
00722         float phi=0;
00723         float phiS = 0; // like az   (but in SPIDER ZXZ)
00724         float psiS =0;  // like phi  (but in SPIDER ZYZ)
00725 
00726         // get alt, az, phi in EMAN convention
00727         if (cosalt > max) {  // that is, alt close to 0
00728                 alt = 0;
00729                 az=0;
00730                 phi = (float)EMConsts::rad2deg*(float)atan2(x_mirror_scale*matrix[0][1], x_mirror_scale*matrix[0][0]);
00731         }
00732         else if (cosalt < -max) { // alt close to pi
00733                 alt = 180;
00734                 az=0;
00735                 phi=360.0f-(float)EMConsts::rad2deg*(float)atan2(x_mirror_scale*matrix[0][1], x_mirror_scale*matrix[0][0]);
00736         }
00737         else {
00738                 alt = (float)EMConsts::rad2deg*(float) acos(cosalt);
00739                 az  = 360.0f+(float)EMConsts::rad2deg*(float)atan2(matrix[2][0], -matrix[2][1]);
00740                 phi = 360.0f+(float)EMConsts::rad2deg*(float)atan2(x_mirror_scale*matrix[0][2], matrix[1][2]);
00741         }
00742 //      az=fmod(az+180.0f,360.0f)-180.0f;
00743 //      phi=fmod(phi+180.0f,360.0f)-180.0f;
00744 
00745         if (phi > 0) phi = fmod(phi,360.f);
00746         else phi = 360.f - fmod(fabs(phi),360.f);
00747         if (phi == 360.f) phi = 0.f;
00748 
00749         if (az > 0 ) az = fmod(az,360.f);
00750         else az = 360.f - fmod(fabs(az),360.f);
00751         if (az == 360.f) az = 0.f;
00752 
00753 //   get phiS, psiS ; SPIDER
00754         if (fabs(cosalt) > max) {  // that is, alt close to 0
00755                 phiS=0;
00756                 psiS = phi;
00757         }
00758         else {
00759                 phiS = az   - 90.0f;
00760                 psiS = phi  + 90.0f;
00761         }
00762         phiS = fmod((phiS   + 360.0f ), 360.0f) ;
00763         psiS = fmod((psiS   + 360.0f ), 360.0f) ;
00764 
00765 //   do some quaternionic stuff here
00766 
00767         float nphi = (az-phi)/2.0f;
00768     // The next is also e0
00769         float cosOover2 = (cos((az+phi)*M_PI/360) * cos(alt*M_PI/360)) ;
00770         float sinOover2 = sqrt(1 -cosOover2*cosOover2);
00771         float cosnTheta = sin((az+phi)*M_PI/360) * cos(alt*M_PI/360) / sqrt(1-cosOover2*cosOover2) ;
00772         float sinnTheta = sqrt(1-cosnTheta*cosnTheta);
00773         float n1 = sinnTheta*cos(nphi*M_PI/180);
00774         float n2 = sinnTheta*sin(nphi*M_PI/180);
00775         float n3 = cosnTheta;
00776         float xtilt = 0;
00777         float ytilt = 0;
00778         float ztilt = 0;
00779 
00780 
00781         if (cosOover2<0) {
00782                 cosOover2*=-1; n1 *=-1; n2*=-1; n3*=-1;
00783         }
00784 
00785         string type = Util::str_to_lower(euler_type);
00786 
00787         result["type"] = type;
00788         if (type == "2d") {
00789                 assert_valid_2d();
00790                 result["alpha"]  = phi;
00791         } else if (type == "eman") {
00792 //              assert_consistent_type(THREED);
00793                 result["az"]  = az;
00794                 result["alt"] = alt;
00795                 result["phi"] = phi;
00796         } else if (type == "imagic") {
00797 //              assert_consistent_type(THREED);
00798                 result["alpha"] = az;
00799                 result["beta"] = alt;
00800                 result["gamma"] = phi;
00801         } else if (type == "spider") {
00802 //              assert_consistent_type(THREED);
00803                 result["phi"]   = phiS;  // The first Euler like az
00804                 result["theta"] = alt;
00805                 result["psi"]   = psiS;
00806         } else if (type == "mrc") {
00807 //              assert_consistent_type(THREED);
00808                 result["phi"]   = phiS;
00809                 result["theta"] = alt;
00810                 result["omega"] = psiS;
00811         } else if (type == "xyz") {
00812 //              assert_consistent_type(THREED);
00813                 xtilt = atan2(-sin((M_PI/180.0f)*phiS)*sin((M_PI/180.0f)*alt),cos((M_PI/180.0f)*alt));
00814                 ytilt = asin(  cos((M_PI/180.0f)*phiS)*sin((M_PI/180.0f)*alt));
00815                 ztilt = psiS*M_PI/180.0f - atan2(sin(xtilt), cos(xtilt) *sin(ytilt));
00816 
00817                 xtilt=fmod(xtilt*180/M_PI+540.0f,360.0f) -180.0f;
00818                 ztilt=fmod(ztilt*180/M_PI+540.0f,360.0f) -180.0f;
00819 
00820                 result["xtilt"]  = xtilt;
00821                 result["ytilt"]  = ytilt*180/M_PI;
00822                 result["ztilt"]  = ztilt;
00823         } else if (type == "quaternion") {
00824 //              assert_consistent_type(THREED);
00825                 result["e0"] = cosOover2 ;
00826                 result["e1"] = sinOover2 * n1 ;
00827                 result["e2"] = sinOover2 * n2;
00828                 result["e3"] = sinOover2 * n3;
00829         } else if (type == "spin") {
00830 //              assert_consistent_type(THREED);
00831                 result["Omega"] =360.0f* acos(cosOover2)/ M_PI ;
00832                 result["n1"] = n1;
00833                 result["n2"] = n2;
00834                 result["n3"] = n3;
00835         } else if (type == "sgirot") {
00836 //              assert_consistent_type(THREED);
00837                 result["q"] = 360.0f*acos(cosOover2)/M_PI ;
00838                 result["n1"] = n1;
00839                 result["n2"] = n2;
00840                 result["n3"] = n3;
00841         } else if (type == "matrix") {
00842 //              assert_consistent_type(THREED);
00843                 result["m11"] = x_mirror_scale*matrix[0][0]*inv_scale;
00844                 result["m12"] = x_mirror_scale*matrix[0][1]*inv_scale;
00845                 result["m13"] = x_mirror_scale*matrix[0][2]*inv_scale;
00846                 result["m21"] = matrix[1][0]*inv_scale;
00847                 result["m22"] = matrix[1][1]*inv_scale;
00848                 result["m23"] = matrix[1][2]*inv_scale;
00849                 result["m31"] = matrix[2][0]*inv_scale;
00850                 result["m32"] = matrix[2][1]*inv_scale;
00851                 result["m33"] = matrix[2][2]*inv_scale;
00852         } else {
00853                 throw InvalidStringException(euler_type, "unknown Euler Type");
00854         }
00855 
00856         return result;
00857 }
00858 
00859 void Transform::set_trans(const float& x, const float& y, const float& z)
00860 {
00861 //      if ( z != 0.0 ) {
00862 //              validate_and_set_type(THREED);
00863 //      }
00864         bool x_mirror = get_mirror();
00865 
00866         if (x_mirror) matrix[0][3] = -x;
00867         else matrix[0][3] = x;
00868         matrix[1][3] = y;
00869         matrix[2][3] = z;
00870 }
00871 
00872 Vec3f Transform::get_trans() const
00873 {
00874         // No type asserted
00875         bool x_mirror = get_mirror();
00876         Vec3f v;
00877         if (x_mirror) v[0] = -matrix[0][3];
00878         else v[0] = matrix[0][3];
00879         v[1] = matrix[1][3];
00880         v[2] = matrix[2][3];
00881 
00882         Util::apply_precision(v[0],ERR_LIMIT);
00883         Util::apply_precision(v[1],ERR_LIMIT);
00884         Util::apply_precision(v[2],ERR_LIMIT);
00885 
00886         return v;
00887 }
00888 
00889 Vec2f Transform::get_trans_2d() const
00890 {
00891         bool x_mirror = get_mirror();
00892         Vec2f v;
00893         if (x_mirror) v[0] = -matrix[0][3];
00894         else v[0] = matrix[0][3];
00895         v[1] = matrix[1][3];
00896         return v;
00897 }
00898 
00899 
00900 
00901 Vec3f Transform::get_pre_trans() const
00902 {
00903         Transform T(*this);
00904         T.set_trans(0,0,0);
00905         T.invert();
00906 
00907         Transform soln  = T*(*this);
00908 //      soln.printme();
00909         return soln.get_trans();
00910 }
00911 
00912 Vec2f Transform::get_pre_trans_2d() const
00913 {
00914         Transform T(*this);
00915         T.set_trans(0,0,0);
00916         T.invert();
00917 
00918         Transform soln  = T*(*this);
00919 //      soln.printme();
00920         return soln.get_trans_2d();
00921 }
00922 
00923 // Vec2f Transform::get_pre_trans_2d() const
00924 // {
00925 //      Vec3f v = get_pre_trans();
00926 //      return Vec2f(v[0],v[1]);
00927 // }
00928 
00929 
00930 void Transform::set_scale(const float& new_scale) {
00931         if (new_scale <= 0) {
00932                 throw InvalidValueException(new_scale,"The scale factor in a Transform object must be positive and non zero");
00933         }
00934         // Transform = MTSR (Mirroring, Translation, Scaling, Rotate)
00935         // So changing the scale boils down to this....
00936 
00937         float old_scale = get_scale();
00938 
00939         float n_scale = new_scale;
00940         Util::apply_precision(n_scale,ERR_LIMIT);
00941 
00942         float corrected_scale = n_scale/old_scale;
00943         if ( corrected_scale != 1.0 ) {
00944                 for(int i = 0; i < 3;  ++i ) {
00945                         for(int j = 0; j < 3; ++j ) {
00946                                 matrix[i][j] *= corrected_scale;
00947                         }
00948                 }
00949         }
00950 }
00951 
00952 float Transform::get_scale() const {
00953         float determinant = get_determinant();
00954         if (determinant < 0 ) determinant *= -1;
00955 
00956         float scale = std::pow(determinant,1.0f/3.0f);
00957         int int_scale = static_cast<int>(scale);
00958         float scale_residual = scale-static_cast<float>(int_scale);
00959         if  ( scale_residual < ERR_LIMIT ) { scale = static_cast<float>(int_scale); };
00960 
00961         Util::apply_precision(scale, ERR_LIMIT);
00962 
00963         return scale;
00964 }
00965 
00966 void print_matrix(gsl_matrix* M, unsigned int r, unsigned int c, const string& message ) {
00967         cout << "Message is " << message << endl;
00968         for ( unsigned int i = 0; i < r; ++i )
00969         {
00970                 for ( unsigned int j = 0; j < c; ++j )
00971                 {
00972                         cout << gsl_matrix_get(M,i,j) << " ";
00973                 }
00974                 cout << endl;
00975         }
00976 }
00977 
00978 void Transform::orthogonalize()
00979 {
00980         float scale;
00981         bool x_mirror;
00982         get_scale_and_mirror(scale,x_mirror);
00983         if (scale == 0) throw UnexpectedBehaviorException("The determinant of the Transform is 0. This is unexpected.");
00984         double inv_scale = 1.0/static_cast<double>(scale);
00985         double mirror_scale = (x_mirror == true ? -1.0:1.0);
00986 
00987         gsl_matrix * R = gsl_matrix_calloc(3,3);
00988         for ( unsigned int i = 0; i < 3; ++i )
00989         {
00990                 for ( unsigned int j = 0; j < 3; ++j )
00991                 {
00992                         if (i == 0 && mirror_scale != 1.0 ) {
00993                                 gsl_matrix_set( R, i, j, static_cast<double>(matrix[i][j])*mirror_scale*inv_scale );
00994                         }
00995                         else {
00996                                 gsl_matrix_set( R, i, j, static_cast<double>(matrix[i][j])*inv_scale );
00997                         }
00998                 }
00999         }
01000 
01001         gsl_matrix * V = gsl_matrix_calloc(3,3);
01002         gsl_vector * S = gsl_vector_calloc(3);
01003         gsl_vector * work = gsl_vector_calloc(3);
01004         gsl_linalg_SV_decomp (R, V, S, work); // Now R is U of the SVD R = USV^T
01005 
01006         gsl_matrix * Soln = gsl_matrix_calloc(3,3);
01007         gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, R, V, 0.0, Soln);
01008 
01009         for ( unsigned int i = 0; i < 3; ++i )
01010         {
01011                 for ( unsigned int j = 0; j < 3; ++j )
01012                 {
01013                         matrix[i][j] = static_cast<float>( gsl_matrix_get(Soln,i,j) );
01014                 }
01015         }
01016 
01017         // Apply scale if it existed previously
01018         if (scale != 1.0f) {
01019                 for(int i=0; i<3; ++i) {
01020                         for(int j=0; j<3; ++j) {
01021                                 matrix[i][j] *= scale;
01022                         }
01023                 }
01024         }
01025 
01026         // Apply post x mirroring if it was applied previouslys
01027         if ( x_mirror ) {
01028                 for(int j=0; j<3; ++j) {
01029                         matrix[0][j] *= -1.0f;
01030                 }
01031         }
01032 
01033         gsl_matrix_free(V); gsl_matrix_free(R); gsl_matrix_free(Soln);
01034         gsl_vector_free(S); gsl_vector_free(work);
01035 }
01036 
01037 void Transform::set_mirror(const bool x_mirror ) {
01038 
01039         bool old_x_mirror = get_mirror();
01040         if (old_x_mirror == x_mirror) return; // The user is setting the same value
01041         else {
01042                 // Toggle the mirroring operation
01043                 for (int j = 0; j < 4; ++j ) {
01044                         matrix[0][j] *= -1;
01045                 }
01046         }
01047 }
01048 
01049 bool Transform::get_mirror() const {
01050         float determinant = get_determinant();
01051 
01052         bool x_mirror = false;
01053         if ( determinant < 0 ) x_mirror = true;
01054 
01055         return x_mirror;
01056 
01057 }
01058 
01059 void Transform::get_scale_and_mirror(float& scale, bool& x_mirror) const {
01060 
01061         float determinant = get_determinant();
01062         x_mirror = false;
01063         if ( determinant < 0 ) {
01064                 x_mirror = true;
01065                 determinant *= -1;
01066         }
01067         if (determinant != 1 ) {
01068                 scale = std::pow(determinant,1.0f/3.0f);
01069                 int int_scale = static_cast<int>(scale);
01070                 float scale_residual = scale-static_cast<float>(int_scale);
01071                 if  ( scale_residual < ERR_LIMIT ) { scale = static_cast<float>(int_scale); };
01072         }
01073         else scale = 1;
01074 
01075         Util::apply_precision(scale,ERR_LIMIT);
01076 }
01077 
01078 float Transform::get_determinant() const
01079 {
01080         float det = matrix[0][0]*(matrix[1][1]*matrix[2][2]-matrix[2][1]*matrix[1][2]);
01081         det -= matrix[0][1]*(matrix[1][0]*matrix[2][2]-matrix[2][0]*matrix[1][2]);
01082         det += matrix[0][2]*(matrix[1][0]*matrix[2][1]-matrix[2][0]*matrix[1][1]);
01083 
01084         Util::apply_precision(det,ERR_LIMIT);
01085 
01086         return det;
01087 }
01088 
01089 void Transform::invert() {
01090 
01091         float m00 = matrix[0][0]; float m01=matrix[0][1]; float m02=matrix[0][2];
01092         float m10 = matrix[1][0]; float m11=matrix[1][1]; float m12=matrix[1][2];
01093         float m20 = matrix[2][0]; float m21=matrix[2][1]; float m22=matrix[2][2];
01094         float v0  = matrix[0][3]; float v1 =matrix[1][3]; float v2 =matrix[2][3];
01095 
01096         float cof00 = m11*m22-m12*m21;
01097         float cof11 = m22*m00-m20*m02;
01098         float cof22 = m00*m11-m01*m10;
01099         float cof01 = m10*m22-m20*m12;
01100         float cof02 = m10*m21-m20*m11;
01101         float cof12 = m00*m21-m01*m20;
01102         float cof10 = m01*m22-m02*m21;
01103         float cof20 = m01*m12-m02*m11;
01104         float cof21 = m00*m12-m10*m02;
01105 
01106         float det = m00* cof00 + m02* cof02 -m01*cof01;
01107 
01108         matrix[0][0] =   cof00/det;
01109         matrix[0][1] = - cof10/det;
01110         matrix[0][2] =   cof20/det;
01111         matrix[1][0] = - cof01/det;
01112         matrix[1][1] =   cof11/det;
01113         matrix[1][2] = - cof21/det;
01114         matrix[2][0] =   cof02/det;
01115         matrix[2][1] = - cof12/det;
01116         matrix[2][2] =   cof22/det;
01117 
01118         matrix[0][3] =  (- cof00*v0 + cof10*v1 - cof20*v2 )/det;
01119         matrix[1][3] =  (  cof01*v0 - cof11*v1 + cof21*v2 )/det;
01120         matrix[2][3] =  (- cof02*v0 + cof12*v1 - cof22*v2 )/det;
01121 }
01122 
01123 Transform Transform::inverse() const {
01124         Transform t(*this);
01125         t.invert();
01126         return t;
01127 }
01128 
01129 void Transform::transpose_inplace() {
01130         float tempij;
01131         for (int i = 0; i < 3; i++) {
01132                 for (int j = 0; j < i; j++) {
01133                         if (i != j) {
01134                                 tempij= matrix[i][j];
01135                                 matrix[i][j] = matrix[j][i];
01136                                 matrix[j][i] = tempij;
01137                         }
01138                 }
01139         }
01140 }
01141 
01142 Transform Transform::transpose() const {
01143         Transform t(*this);
01144         t.transpose_inplace();
01145         return t;
01146 }
01147 
01148 
01149 Transform EMAN::operator*(const Transform & M2, const Transform & M1)     // YYY
01150 {
01151         Transform result;
01152         for (int i=0; i<3; i++) {
01153                 for (int j=0; j<4; j++) {
01154                         result[i][j] = M2[i][0] * M1[0][j] +  M2[i][1] * M1[1][j] + M2[i][2] * M1[2][j];
01155                 }
01156                 result[i][3] += M2[i][3];
01157         }
01158 
01159         return result;
01160 }
01161 
01162 void Transform::assert_valid_2d() const {
01163         int rotation_error = 0;
01164         int translation_error = 0;
01165         if (fabs(matrix[2][0]) > ERR_LIMIT) rotation_error++;
01166         if (fabs(matrix[2][1]) > ERR_LIMIT) rotation_error++;
01167         if (fabs(matrix[2][3]) > ERR_LIMIT) translation_error++;
01168         if (fabs(matrix[0][2]) > ERR_LIMIT) rotation_error++;
01169         if (fabs(matrix[1][2]) > ERR_LIMIT) rotation_error++;
01170         if ( translation_error && rotation_error ) {
01171                 throw UnexpectedBehaviorException("Error, the internal matrix contains 3D rotations and 3D translations. This object can not be considered 2D");
01172         } else if ( translation_error ) {
01173                 throw UnexpectedBehaviorException("Error, the internal matrix contains a non zero z component for a 3D translation. This object can not be considered 2D");
01174         }
01175         else if ( rotation_error ) {
01176                 throw UnexpectedBehaviorException("Error, the internal matrix contains 3D rotations and this object can not be considered 2D");
01177         }
01178 
01179 }
01180 
01181 
01182 
01183 Transform Transform::get_sym(const string & sym_name, int n) const
01184 {
01185         Symmetry3D* sym = Factory<Symmetry3D>::get(sym_name);
01186         Transform ret;
01187         ret = (*this) * sym->get_sym(n);
01188         delete sym;
01189         return ret;
01190 }
01191 
01192 int Transform::get_nsym(const string & sym_name)
01193 {
01194         Symmetry3D* sym = Factory<Symmetry3D>::get(sym_name);
01195         int nsym = sym->get_nsym();
01196         delete sym;
01197         return nsym;
01198 }
01199 
01200 Transform3D::Transform3D()  //    C1
01201 {
01202         init();
01203 }
01204 
01205 Transform3D::Transform3D( const Transform3D& rhs )
01206 {
01207     for( int i=0; i < 4; ++i )
01208     {
01209         for( int j=0; j < 4; ++j )
01210         {
01211             matrix[i][j] = rhs.matrix[i][j];
01212         }
01213     }
01214 }
01215 
01216 // C2
01217 Transform3D::Transform3D(const float& az, const float& alt, const float& phi)
01218 {
01219         init();
01220         set_rotation(az,alt,phi);
01221 }
01222 
01223 
01224 //  C3  Usual Constructor: Post Trans, after appying Rot
01225 Transform3D::Transform3D(const float& az, const float& alt, const float& phi, const Vec3f& posttrans )
01226 {
01227         init(); // This is called in set_rotation
01228         set_rotation(az,alt,phi);
01229         set_posttrans(posttrans);
01230 }
01231 
01232 Transform3D::Transform3D(const float& m11, const float& m12, const float& m13,
01233                                                  const float& m21, const float& m22, const float& m23,
01234            const float& m31, const float& m32, const float& m33)
01235 {
01236         init();
01237         set_rotation(m11,m12,m13,m21,m22,m23,m31,m32,m33);
01238 }
01239 
01240 // C4
01241 Transform3D::Transform3D(EulerType euler_type, const float& a1, const float& a2, const float& a3)
01242 {
01243         init();
01244         set_rotation(euler_type,a1,a2,a3);
01245 }
01246 
01247 Transform3D::Transform3D(EulerType euler_type, const float& a1, const float& a2, const float& a3, const float& a4)
01248 {
01249         init();
01250         set_rotation(euler_type,a1,a2,a3,a4);
01251 }
01252 
01253 
01254 // C5
01255 Transform3D::Transform3D(EulerType euler_type, const Dict& rotation)  //YYY
01256 {
01257         init();
01258         set_rotation(euler_type,rotation);
01259 }
01260 
01261 
01262 // C6   First apply pretrans: Then rotation: Then posttrans
01263 
01264 Transform3D::Transform3D(  const Vec3f& pretrans,  const float& az, const float& alt, const float& phi, const Vec3f& posttrans )  //YYY  by default EMAN
01265 {
01266         init();
01267         set_pretrans(pretrans);
01268         set_rotation(az,alt,phi);
01269         set_posttrans(posttrans);
01270 }
01271 
01272 
01273 
01274 
01275 Transform3D::~Transform3D()
01276 {
01277 }
01278 
01279 
01280 
01281 void Transform3D::to_identity()
01282 {
01283 //      for (int i = 0; i < 3; i++) {
01284 //              matrix[i][i] = 1;
01285 //      }
01286 
01287         for(int i=0; i<4; ++i) {
01288                 for(int j=0; j<4; ++j) {
01289                         if(i==j) {
01290                                 matrix[i][j] = 1;
01291                         }
01292                         else {
01293                                 matrix[i][j] = 0;
01294                         }
01295                 }
01296         }
01297         post_x_mirror = false;
01298         set_center(Vec3f(0,0,0));
01299 }
01300 
01301 
01302 
01303 bool Transform3D::is_identity()  // YYY
01304 {
01305         for (int i=0; i<4; i++) {
01306                 for (int j=0; j<4; j++) {
01307                         if (i==j && matrix[i][j]!=1.0) return 0;
01308                         if (i!=j && matrix[i][j]!=0.0) return 0;
01309                 }
01310         }
01311         return 1;
01312 }
01313 
01314 
01315 void Transform3D::set_center(const Vec3f & center) //YYN
01316 {
01317         set_pretrans( Vec3f(0,0,0)-center);
01318         for (int i = 0; i < 3; i++) {
01319                 matrix[i][3]=center[i];
01320         }
01321 }
01322 
01323 //            METHODS
01324 //   Note Transform3Ds are initialized as identities
01325 void Transform3D::init()  // M1
01326 {
01327         to_identity();
01328 }
01329 
01330 //      Set Methods
01331 
01332 void Transform3D::set_pretrans(const float& dx, const float& dy, const float& dz) // YYY
01333 {    set_pretrans( Vec3f(dx,dy,dz)); }
01334 
01335 
01336 void Transform3D::set_pretrans(const float& dx, const float& dy) // YYY
01337 {    set_pretrans( Vec3f(dx,dy,0)); }
01338 
01339 void Transform3D::set_pretrans(const Vec2f& pretrans) // YYY
01340 {    set_pretrans( Vec3f(pretrans[0],pretrans[1],0)); }
01341 
01342 void Transform3D::set_pretrans(const Vec3f & preT)  // flag=1 means keep the old value of total trans
01343 {
01344                 int flag=0;
01345 
01346 //     transFinal = transPost +  Rotation * transPre;
01347 //    This will keep the old value of transPost and change the value of pretrans and the total matrix
01348     if (flag==0){
01349                 matrix[0][3] = matrix[3][0] + matrix[0][0]*preT[0] + matrix[0][1]*preT[1] + matrix[0][2]*preT[2]  ;
01350                 matrix[1][3] = matrix[3][1] + matrix[1][0]*preT[0] + matrix[1][1]*preT[1] + matrix[1][2]*preT[2]  ;
01351                 matrix[2][3] = matrix[3][2] + matrix[2][0]*preT[0] + matrix[2][1]*preT[1] + matrix[2][2]*preT[2]  ;
01352         }
01353 //    This will keep the old value of total translation and change the value of posttrans
01354     if (flag==1){
01355                 matrix[3][0] = matrix[0][3] - (matrix[0][0]*preT[0] + matrix[0][1]*preT[1] + matrix[0][2]*preT[2])  ;
01356                 matrix[3][1] = matrix[1][3] - (matrix[1][0]*preT[0] + matrix[1][1]*preT[1] + matrix[1][2]*preT[2])  ;
01357                 matrix[3][2] = matrix[2][3] - (matrix[2][0]*preT[0] + matrix[2][1]*preT[1] + matrix[2][2]*preT[2])  ;
01358         }
01359 }
01360 
01361 
01362 void Transform3D::set_posttrans(const float& dx, const float& dy, const float& dz) // YYY
01363 {    set_posttrans( Vec3f(dx,dy,dz)); }
01364 
01365 
01366 void Transform3D::set_posttrans(const float& dx, const float& dy) // YYY
01367 {    set_posttrans( Vec3f(dx,dy,0)); }
01368 
01369 void Transform3D::set_posttrans(const Vec2f& posttrans) // YYY
01370 {    set_pretrans( Vec3f(posttrans[0],posttrans[1],0)); }
01371 
01372 void Transform3D::set_posttrans(const Vec3f & posttrans) // flag=1 means keep the old value of total trans
01373 {
01374         int flag=0;
01375     Vec3f preT   = get_pretrans(0) ;
01376         for (int i = 0; i < 3; i++) {
01377                 matrix[3][i] = posttrans[i];
01378         }
01379 //     transFinal = transPost +  Rotation * transPre;
01380 //   This will keep the old value of pretrans and change the value of posttrans and the total matrix
01381         if (flag==0) {
01382                 matrix[0][3] = matrix[3][0] + matrix[0][0]*preT[0] + matrix[0][1]*preT[1] + matrix[0][2]*preT[2]  ;
01383                 matrix[1][3] = matrix[3][1] + matrix[1][0]*preT[0] + matrix[1][1]*preT[1] + matrix[1][2]*preT[2]  ;
01384                 matrix[2][3] = matrix[3][2] + matrix[2][0]*preT[0] + matrix[2][1]*preT[1] + matrix[2][2]*preT[2]  ;
01385         }
01386 //   This will keep the old value of the total matrix, and c
01387         if (flag==1) { // Don't do anything
01388         }
01389 }
01390 
01391 
01392 
01393 
01394 void Transform3D::apply_scale(const float& scale)    // YYY
01395 {
01396         for (int i = 0; i < 3; i++) {
01397                 for (int j = 0; j < 4; j++) {
01398                         matrix[i][j] *= scale;
01399                 }
01400         }
01401         for (int j = 0; j < 3; j++) {
01402                 matrix[3][j] *= scale;
01403         }
01404 }
01405 
01406 void Transform3D::orthogonalize()  // YYY
01407 {
01408         //EulerType EMAN;
01409         float scale = get_scale() ;
01410         float inverseScale= 1/scale ;
01411         apply_scale(inverseScale);
01412 //      Dict angs = get_rotation(EMAN);
01413 //      set_Rotation(EMAN,angs);
01414 }
01415 
01416 
01417 void Transform3D::transpose()  // YYY
01418 {
01419         float tempij;
01420         for (int i = 0; i < 3; i++) {
01421                 for (int j = 0; j < i; j++) {
01422                         tempij= matrix[i][j];
01423                         matrix[i][j] = matrix[j][i];
01424                         matrix[j][i] = tempij;
01425                 }
01426         }
01427 }
01428 
01429 void Transform3D::set_scale(const float& scale)    // YYY
01430 {
01431         float OldScale= get_scale();
01432         float Scale2Apply = scale/OldScale;
01433         apply_scale(Scale2Apply);
01434 }
01435 
01436 float Transform3D::get_mag() const //
01437 {
01438         EulerType eulertype= SPIN ;
01439         Dict AA= get_rotation(eulertype);
01440         return AA["Omega"];
01441 }
01442 
01443 Vec3f Transform3D::get_finger() const //
01444 {
01445         EulerType eulertype= SPIN ;
01446         Dict AA= get_rotation(eulertype);
01447         return Vec3f(AA["n1"],AA["n2"],AA["n3"]);
01448 }
01449 
01450 Vec3f Transform3D::get_posttrans(int flag) const    //
01451 {
01452         if (flag==0){
01453                 return Vec3f(matrix[3][0], matrix[3][1], matrix[3][2]);
01454         }
01455         // otherwise as if all the translation was post
01456         return Vec3f(matrix[0][3], matrix[1][3], matrix[2][3]);
01457 }
01458 
01459 Vec3f Transform3D::get_total_posttrans() const {
01460         return get_posttrans(1);
01461 }
01462 
01463 Vec3f Transform3D::get_total_pretrans() const {
01464         return get_pretrans(1);
01465 }
01466 
01467 
01468 Vec3f Transform3D::get_pretrans(int flag) const    // Fix Me
01469 {
01470 //      The expression is R^T(v_total - v_post);
01471 
01472         Vec3f pretrans;
01473         Vec3f posttrans(matrix[3][0], matrix[3][1], matrix[3][2]);
01474         Vec3f tottrans(matrix[0][3], matrix[1][3], matrix[2][3]);
01475         Vec3f totminuspost;
01476 
01477         totminuspost = tottrans;
01478         if (flag==0) {
01479                 totminuspost = tottrans-posttrans;
01480         }
01481 
01482         Transform3D Rinv = inverse();
01483         for (int i=0; i<3; i++) {
01484                 float ptnow=0;
01485                 for (int j=0; j<3; j++) {
01486                         ptnow +=   Rinv.matrix[i][j]* totminuspost[j] ;
01487                 }
01488                 pretrans.set_value_at(i,ptnow) ;  //
01489         }
01490         return pretrans;
01491 }
01492 
01493 
01494  Vec3f Transform3D::get_center() const  // YYY
01495  {
01496         return Vec3f();
01497  }
01498 
01499 
01500 
01501 Vec3f Transform3D::get_matrix3_col(int i) const     // YYY
01502 {
01503         return Vec3f(matrix[0][i], matrix[1][i], matrix[2][i]);
01504 }
01505 
01506 
01507 Vec3f Transform3D::get_matrix3_row(int i) const     // YYY
01508 {
01509         return Vec3f(matrix[i][0], matrix[i][1], matrix[i][2]);
01510 }
01511 
01512 Vec3f Transform3D::transform(const Vec3f & v3f) const     // YYY
01513 {
01514 //      This is the transformation of a vector, v by a matrix M
01515         float x = matrix[0][0] * v3f[0] + matrix[0][1] * v3f[1] + matrix[0][2] * v3f[2] + matrix[0][3] ;
01516         float y = matrix[1][0] * v3f[0] + matrix[1][1] * v3f[1] + matrix[1][2] * v3f[2] + matrix[1][3] ;
01517         float z = matrix[2][0] * v3f[0] + matrix[2][1] * v3f[1] + matrix[2][2] * v3f[2] + matrix[2][3] ;
01518         return Vec3f(x, y, z);
01519 }
01520 
01521 
01522 Vec3f Transform3D::rotate(const Vec3f & v3f) const     // YYY
01523 {
01524 //      This is the rotation of a vector, v by a matrix M
01525         float x = matrix[0][0] * v3f[0] + matrix[0][1] * v3f[1] + matrix[0][2] * v3f[2]  ;
01526         float y = matrix[1][0] * v3f[0] + matrix[1][1] * v3f[1] + matrix[1][2] * v3f[2]  ;
01527         float z = matrix[2][0] * v3f[0] + matrix[2][1] * v3f[1] + matrix[2][2] * v3f[2]  ;
01528         return Vec3f(x, y, z);
01529 }
01530 
01531 
01532 Transform3D EMAN::operator*(const Transform3D & M2, const Transform3D & M1)     // YYY
01533 {
01534 //       This is the  left multiplication of a matrix M1 by a matrix M2; that is M2*M1
01535 //       It returns a new matrix
01536         Transform3D resultant;
01537         for (int i=0; i<3; i++) {
01538                 for (int j=0; j<4; j++) {
01539                         resultant[i][j] = M2[i][0] * M1[0][j] +  M2[i][1] * M1[1][j] + M2[i][2] * M1[2][j];
01540                 }
01541                 resultant[i][3] += M2[i][3];  // add on the new translation (not included above)
01542         }
01543 
01544         for (int j=0; j<3; j++) {
01545                 resultant[3][j] = M2[3][j];
01546         }
01547 
01548         return resultant; // This will have the post_trans of M2
01549 }
01550 
01551 /*             Here starts the pure rotation stuff */
01552 
01623 void Transform3D::set_rotation(const float& az, const float& alt, const float& phi )
01624 {
01625         EulerType euler_type=EMAN;
01626         Dict rot;
01627         rot["az"]  = az;
01628         rot["alt"] = alt;
01629         rot["phi"] = phi;
01630         set_rotation(euler_type, rot);
01631 }
01632 
01633 // This is where it all happens;
01634 void Transform3D::set_rotation(EulerType euler_type, const float& a1, const float& a2, const float& a3)
01635 {
01636         init();
01637         Dict rot;
01638         switch(euler_type) {
01639                 case EMAN:
01640                         rot["az"]  = a1;
01641                         rot["alt"] = a2;
01642                         rot["phi"] = a3;
01643                         break;
01644                 case SPIDER:
01645                         rot["phi"]   = a1;
01646                         rot["theta"] = a2;
01647                         rot["psi"]   = a3;
01648                         break;
01649                 case IMAGIC:
01650                         rot["alpha"]   = a1;
01651                         rot["beta"] = a2;
01652                         rot["gamma"]   = a3;
01653                         break;
01654                 case MRC:
01655                         rot["phi"]   = a1;
01656                         rot["theta"] = a2;
01657                         rot["omega"]   = a3;
01658                         break;
01659                 case XYZ:
01660                         rot["xtilt"]   = a1;
01661                         rot["ytilt"] = a2;
01662                         rot["ztilt"]   = a3;
01663                         break;
01664                 default:
01665                 throw InvalidValueException(euler_type, "cannot instantiate this Euler Type");
01666         }  // ends switch euler_type
01667         set_rotation(euler_type, rot);
01668 }
01669 
01670 // This is where it all happens;
01671 void Transform3D::set_rotation(EulerType euler_type, const float& a1, const float& a2, const float& a3, const float& a4)
01672 {
01673         init();
01674         Dict rot;
01675         switch(euler_type) {
01676                 case QUATERNION:
01677                         rot["e0"]  = a1;
01678                         rot["e1"] = a2;
01679                         rot["e2"] = a3;
01680                         rot["e3"] = a4;
01681                         break;
01682                 case SGIROT:
01683                         rot["q"]  = a1;
01684                         rot["n1"] = a2;
01685                         rot["n2"] = a3;
01686                         rot["n3"] = a4;
01687                 case SPIN:
01688                         rot["Omega"]  = a1;
01689                         rot["n1"] = a2;
01690                         rot["n2"] = a3;
01691                         rot["n3"] = a4;
01692                         break;
01693                 default:
01694                         throw InvalidValueException(euler_type, "cannot instantiate this Euler Type");
01695         }  // ends switch euler_type
01696         set_rotation(euler_type, rot);
01697 }
01698 
01699 void Transform3D::set_rotation(EulerType euler_type, const Dict& rotation)
01700 {
01701         float e0  = 0;float e1=0; float e2=0; float e3=0;
01702         float Omega=0;
01703         float az  = 0;
01704         float alt = 0;
01705         float phi = 0;
01706         float cxtilt = 0;
01707         float sxtilt = 0;
01708         float cytilt = 0;
01709         float sytilt = 0;
01710         bool is_quaternion = 0;
01711         bool is_matrix = 0;
01712 
01713         switch(euler_type) {
01714         case EMAN:
01715                 az  = (float)rotation["az"] ;
01716                 alt = (float)rotation["alt"]  ;
01717                 phi = (float)rotation["phi"] ;
01718                 break;
01719         case IMAGIC:
01720                 az  = (float)rotation["alpha"] ;
01721                 alt = (float)rotation["beta"]  ;
01722                 phi = (float)rotation["gamma"] ;
01723                 break;
01724 
01725         case SPIDER:
01726                 az =  (float)rotation["phi"]    + 90.0f;
01727                 alt = (float)rotation["theta"] ;
01728                 phi = (float)rotation["psi"]    - 90.0f;
01729                 break;
01730 
01731         case XYZ:
01732                 cxtilt = cos( (M_PI/180.0f)*(float)rotation["xtilt"]);
01733                 sxtilt = sin( (M_PI/180.0f)*(float)rotation["xtilt"]);
01734                 cytilt = cos( (M_PI/180.0f)*(float)rotation["ytilt"]);
01735                 sytilt = sin( (M_PI/180.0f)*(float)rotation["ytilt"]);
01736                 az =  (180.0f/M_PI)*atan2(-cytilt*sxtilt,sytilt)   + 90.0f ;
01737                 alt = (180.0f/M_PI)*acos(cytilt*cxtilt)  ;
01738                 phi = (float)rotation["ztilt"] +(180.0f/M_PI)*atan2(sxtilt,cxtilt*sytilt)   - 90.0f ;
01739                 break;
01740 
01741         case MRC:
01742                 az  = (float)rotation["phi"]   + 90.0f ;
01743                 alt = (float)rotation["theta"] ;
01744                 phi = (float)rotation["omega"] - 90.0f ;
01745                 break;
01746 
01747         case QUATERNION:
01748                 is_quaternion = 1;
01749                 e0 = (float)rotation["e0"];
01750                 e1 = (float)rotation["e1"];
01751                 e2 = (float)rotation["e2"];
01752                 e3 = (float)rotation["e3"];
01753                 break;
01754 
01755         case SPIN:
01756                 is_quaternion = 1;
01757                 Omega = (float)rotation["Omega"];
01758                 e0 = cos(Omega*M_PI/360.0f);
01759                 e1 = sin(Omega*M_PI/360.0f)* (float)rotation["n1"];
01760                 e2 = sin(Omega*M_PI/360.0f)* (float)rotation["n2"];
01761                 e3 = sin(Omega*M_PI/360.0f)* (float)rotation["n3"];
01762                 break;
01763 
01764         case SGIROT:
01765                 is_quaternion = 1;
01766                 Omega = (float)rotation["q"]  ;
01767                 e0 = cos(Omega*M_PI/360.0f);
01768                 e1 = sin(Omega*M_PI/360.0f)* (float)rotation["n1"];
01769                 e2 = sin(Omega*M_PI/360.0f)* (float)rotation["n2"];
01770                 e3 = sin(Omega*M_PI/360.0f)* (float)rotation["n3"];
01771                 break;
01772 
01773         case MATRIX:
01774                 is_matrix = 1;
01775                 matrix[0][0] = (float)rotation["m11"]  ;
01776                 matrix[0][1] = (float)rotation["m12"]  ;
01777                 matrix[0][2] = (float)rotation["m13"]  ;
01778                 matrix[1][0] = (float)rotation["m21"]  ;
01779                 matrix[1][1] = (float)rotation["m22"]  ;
01780                 matrix[1][2] = (float)rotation["m23"]  ;
01781                 matrix[2][0] = (float)rotation["m31"]  ;
01782                 matrix[2][1] = (float)rotation["m32"]  ;
01783                 matrix[2][2] = (float)rotation["m33"]  ;
01784                 break;
01785 
01786         default:
01787                 throw InvalidValueException(euler_type, "unknown Euler Type");
01788         }  // ends switch euler_type
01789 
01790 
01791         Vec3f postT  = get_posttrans( ) ;
01792         Vec3f preT   = get_pretrans( ) ;
01793 
01794 
01795         float azp  = fmod(az,360.0f)*M_PI/180.0f;
01796         float altp  = alt*M_PI/180.0f;
01797         float phip = fmod(phi,360.0f)*M_PI/180.0f;
01798 
01799         if (!is_quaternion && !is_matrix) {
01800                 matrix[0][0] =  cos(phip)*cos(azp) - cos(altp)*sin(azp)*sin(phip);
01801                 matrix[0][1] =  cos(phip)*sin(azp) + cos(altp)*cos(azp)*sin(phip);
01802                 matrix[0][2] =  sin(altp)*sin(phip);
01803                 matrix[1][0] = -sin(phip)*cos(azp) - cos(altp)*sin(azp)*cos(phip);
01804                 matrix[1][1] = -sin(phip)*sin(azp) + cos(altp)*cos(azp)*cos(phip);
01805                 matrix[1][2] =  sin(altp)*cos(phip);
01806                 matrix[2][0] =  sin(altp)*sin(azp);
01807                 matrix[2][1] = -sin(altp)*cos(azp);
01808                 matrix[2][2] =  cos(altp);
01809         }
01810         if (is_quaternion){
01811                 matrix[0][0] = e0 * e0 + e1 * e1 - e2 * e2 - e3 * e3;
01812                 matrix[0][1] = 2.0f * (e1 * e2 + e0 * e3);
01813                 matrix[0][2] = 2.0f * (e1 * e3 - e0 * e2);
01814                 matrix[1][0] = 2.0f * (e2 * e1 - e0 * e3);
01815                 matrix[1][1] = e0 * e0 - e1 * e1 + e2 * e2 - e3 * e3;
01816                 matrix[1][2] = 2.0f * (e2 * e3 + e0 * e1);
01817                 matrix[2][0] = 2.0f * (e3 * e1 + e0 * e2);
01818                 matrix[2][1] = 2.0f * (e3 * e2 - e0 * e1);
01819                 matrix[2][2] = e0 * e0 - e1 * e1 - e2 * e2 + e3 * e3;
01820                 // keep in mind matrix[0][2] is M13 gives an e0 e2 piece, etc
01821         }
01822         // Now do post and pretrans: vfinal = vpost + R vpre;
01823 
01824         matrix[0][3] = postT[0] + matrix[0][0]*preT[0] + matrix[0][1]*preT[1] + matrix[0][2]*preT[2]  ;
01825         matrix[1][3] = postT[1] + matrix[1][0]*preT[0] + matrix[1][1]*preT[1] + matrix[1][2]*preT[2]  ;
01826         matrix[2][3] = postT[2] + matrix[2][0]*preT[0] + matrix[2][1]*preT[1] + matrix[2][2]*preT[2]  ;
01827 }
01828 
01829 
01830 void Transform3D::set_rotation(const float& m11, const float& m12, const float& m13,
01831                                                            const float& m21, const float& m22, const float& m23,
01832                   const float& m31, const float& m32, const float& m33)
01833 {
01834         EulerType euler_type = MATRIX;
01835         Dict rot;
01836         rot["m11"]  = m11;
01837         rot["m12"]  = m12;
01838         rot["m13"]  = m13;
01839         rot["m21"]  = m21;
01840         rot["m22"]  = m22;
01841         rot["m23"]  = m23;
01842         rot["m31"]  = m31;
01843         rot["m32"]  = m32;
01844         rot["m33"]  = m33;
01845         set_rotation(euler_type, rot);  // Or should it be &rot ?
01846 }
01847 
01848 void Transform3D::set_rotation(const Vec3f & eahat, const Vec3f & ebhat,
01849                                     const Vec3f & eAhat, const Vec3f & eBhat)
01850 {// this rotation rotates unit vectors a,b into A,B;
01851 //    The program assumes a dot b must equal A dot B
01852         Vec3f eahatcp(eahat);
01853         Vec3f ebhatcp(ebhat);
01854         Vec3f eAhatcp(eAhat);
01855         Vec3f eBhatcp(eBhat);
01856 
01857         eahatcp.normalize();
01858         ebhatcp.normalize();
01859         eAhatcp.normalize();
01860         eBhatcp.normalize();
01861 
01862         Vec3f aMinusA(eahatcp);
01863         aMinusA  -= eAhatcp;
01864         Vec3f bMinusB(ebhatcp);
01865         bMinusB  -= eBhatcp;
01866 
01867 
01868         Vec3f  nhat;
01869         float aAlength = aMinusA.length();
01870         float bBlength = bMinusB.length();
01871         if (aAlength==0){
01872                 nhat=eahatcp;
01873         }else if (bBlength==0){
01874                 nhat=ebhatcp;
01875         }else{
01876                 nhat= aMinusA.cross(bMinusB);
01877                 nhat.normalize();
01878         }
01879 
01880 //              printf("nhat=%f,%f,%f \n",nhat[0],nhat[1],nhat[2]);
01881 
01882         Vec3f neahat  = eahatcp.cross(nhat);
01883         Vec3f nebhat  = ebhatcp.cross(nhat);
01884         Vec3f neAhat  = eAhatcp.cross(nhat);
01885         Vec3f neBhat  = eBhatcp.cross(nhat);
01886 
01887         float cosOmegaA = (neahat.dot(neAhat))  / (neahat.dot(neahat));
01888 //      float cosOmegaB = (nebhat.dot(neBhat))  / (nebhat.dot(nebhat));
01889         float sinOmegaA = (neahat.dot(eAhatcp)) / (neahat.dot(neahat));
01890 //      printf("cosOmegaA=%f \n",cosOmegaA);    printf("sinOmegaA=%f \n",sinOmegaA);
01891 
01892         float OmegaA = atan2(sinOmegaA,cosOmegaA);
01893 //      printf("OmegaA=%f \n",OmegaA*180/M_PI);
01894 
01895         EulerType euler_type=SPIN;
01896         Dict rotation;
01897         rotation["n1"]= nhat[0];
01898         rotation["n2"]= nhat[1];
01899         rotation["n3"]= nhat[2];
01900         rotation["Omega"] =OmegaA*180.0/M_PI;
01901         set_rotation(euler_type,  rotation);
01902 }
01903 
01904 
01905 float Transform3D::get_scale() const     // YYY
01906 {
01907         // Assumes uniform scaling, calculation uses Z only.
01908         float scale =0;
01909         for (int i=0; i<3; i++) {
01910                 for (int j=0; j<3; j++) {
01911                         scale = scale + matrix[i][j]*matrix[i][j];
01912                 }
01913         }
01914 
01915         return sqrt(scale/3);
01916 }
01917 
01918 
01919 
01920 Dict Transform3D::get_rotation(EulerType euler_type) const
01921 {
01922         Dict result;
01923 
01924         float max = 1 - ERR_LIMIT;
01925         float sca=get_scale();
01926         float cosalt=matrix[2][2]/sca;
01927 
01928 
01929         float az=0;
01930         float alt = 0;
01931         float phi=0;
01932         float phiS = 0; // like az   (but in SPIDER ZXZ)
01933         float psiS =0;  // like phi  (but in SPIDER ZYZ)
01934 
01935 
01936 // get alt, az, phi;  EMAN
01937 
01938         if (cosalt > max) {  // that is, alt close to 0
01939                 alt = 0;
01940                 az=0;
01941                 phi = (float)EMConsts::rad2deg*(float)atan2(matrix[0][1], matrix[0][0]);
01942         }
01943         else if (cosalt < -max) { // alt close to pi
01944                 alt = 180;
01945                 az=0;
01946                 phi=360.0f-(float)EMConsts::rad2deg*(float)atan2(matrix[0][1], matrix[0][0]);
01947         }
01948         else {
01949                 alt = (float)EMConsts::rad2deg*(float) acos(cosalt);
01950                 az  = 360.0f+(float)EMConsts::rad2deg*(float)atan2(matrix[2][0], -matrix[2][1]);
01951                 phi = 360.0f+(float)EMConsts::rad2deg*(float)atan2(matrix[0][2], matrix[1][2]);
01952         }
01953         az=fmod(az+180.0f,360.0f)-180.0f;
01954         phi=fmod(phi+180.0f,360.0f)-180.0f;
01955 
01956 //   get phiS, psiS ; SPIDER
01957         if (fabs(cosalt) > max) {  // that is, alt close to 0
01958                 phiS=0;
01959                 psiS = phi;
01960         }
01961         else {
01962                 phiS = az   - 90.0f;
01963                 psiS = phi  + 90.0f;
01964         }
01965         phiS = fmod((phiS   + 360.0f ), 360.0f) ;
01966         psiS = fmod((psiS   + 360.0f ), 360.0f) ;
01967 
01968 //   do some quaternionic stuff here
01969 
01970         float nphi = (az-phi)/2.0f;
01971     // The next is also e0
01972         float cosOover2 = (cos((az+phi)*M_PI/360) * cos(alt*M_PI/360)) ;
01973         float sinOover2 = sqrt(1 -cosOover2*cosOover2);
01974         float cosnTheta = sin((az+phi)*M_PI/360) * cos(alt*M_PI/360) / sqrt(1-cosOover2*cosOover2) ;
01975         float sinnTheta = sqrt(1-cosnTheta*cosnTheta);
01976         float n1 = sinnTheta*cos(nphi*M_PI/180);
01977         float n2 = sinnTheta*sin(nphi*M_PI/180);
01978         float n3 = cosnTheta;
01979         float xtilt = 0;
01980         float ytilt = 0;
01981         float ztilt = 0;
01982 
01983 
01984         if (cosOover2<0) {
01985                 cosOover2*=-1; n1 *=-1; n2*=-1; n3*=-1;
01986         }
01987 
01988 
01989         switch (euler_type) {
01990         case EMAN:
01991                 result["az"]  = az;
01992                 result["alt"] = alt;
01993                 result["phi"] = phi;
01994                 break;
01995 
01996         case IMAGIC:
01997                 result["alpha"] = az;
01998                 result["beta"] = alt;
01999                 result["gamma"] = phi;
02000                 break;
02001 
02002         case SPIDER:
02003                 result["phi"]   = phiS;  // The first Euler like az
02004                 result["theta"] = alt;
02005                 result["psi"]   = psiS;
02006                 break;
02007 
02008         case MRC:
02009                 result["phi"]   = phiS;
02010                 result["theta"] = alt;
02011                 result["omega"] = psiS;
02012                 break;
02013 
02014         case XYZ:
02015                 xtilt = atan2(-sin((M_PI/180.0f)*phiS)*sin((M_PI/180.0f)*alt),cos((M_PI/180.0f)*alt));
02016                 ytilt = asin(  cos((M_PI/180.0f)*phiS)*sin((M_PI/180.0f)*alt));
02017                 ztilt = psiS*M_PI/180.0f - atan2(sin(xtilt), cos(xtilt) *sin(ytilt));
02018 
02019                 xtilt=fmod(xtilt*180/M_PI+540.0f,360.0f) -180.0f;
02020                 ztilt=fmod(ztilt*180/M_PI+540.0f,360.0f) -180.0f;
02021 
02022                 result["xtilt"]  = xtilt;
02023                 result["ytilt"]  = ytilt*180/M_PI;
02024                 result["ztilt"]  = ztilt;
02025                 break;
02026 
02027         case QUATERNION:
02028                 result["e0"] = cosOover2 ;
02029                 result["e1"] = sinOover2 * n1 ;
02030                 result["e2"] = sinOover2 * n2;
02031                 result["e3"] = sinOover2 * n3;
02032                 break;
02033 
02034         case SPIN:
02035                 result["Omega"] =360.0f* acos(cosOover2)/ M_PI ;
02036                 result["n1"] = n1;
02037                 result["n2"] = n2;
02038                 result["n3"] = n3;
02039                 break;
02040 
02041         case SGIROT:
02042                 result["q"] = 360.0f*acos(cosOover2)/M_PI ;
02043                 result["n1"] = n1;
02044                 result["n2"] = n2;
02045                 result["n3"] = n3;
02046                 break;
02047 
02048         case MATRIX:
02049                 result["m11"] = matrix[0][0] ;
02050                 result["m12"] = matrix[0][1] ;
02051                 result["m13"] = matrix[0][2] ;
02052                 result["m21"] = matrix[1][0] ;
02053                 result["m22"] = matrix[1][1] ;
02054                 result["m23"] = matrix[1][2] ;
02055                 result["m31"] = matrix[2][0] ;
02056                 result["m32"] = matrix[2][1] ;
02057                 result["m33"] = matrix[2][2] ;
02058                 break;
02059 
02060         default:
02061                 throw InvalidValueException(euler_type, "unknown Euler Type");
02062         }
02063 
02064         return result;
02065 }
02066 
02067 Transform3D Transform3D::inverseUsingAngs() const    //   YYN need to test it for sure
02068 {
02069         // First Find the scale
02070         EulerType eE=EMAN;
02071 
02072 
02073         float scale   = get_scale();
02074         Vec3f preT   = get_pretrans( ) ;
02075         Vec3f postT   = get_posttrans( ) ;
02076         Dict angs     = get_rotation(eE);
02077         Dict invAngs  ;
02078 
02079         invAngs["phi"]   = 180.0f - (float) angs["az"] ;
02080         invAngs["az"]    = 180.0f - (float) angs["phi"] ;
02081         invAngs["alt"]   = angs["alt"] ;
02082 
02083 //    The inverse result
02084 //
02085 //           Z_phi   X_alt     Z_az
02086 //                 is
02087 //       Z_{pi-az}   X_alt  Z_{pi-phi}
02088 //      The reason for the extra pi's, is because one would like to keep alt positive
02089 
02090         float inverseScale= 1/scale ;
02091 
02092         Transform3D invM;
02093 
02094         invM.set_rotation(EMAN, invAngs);
02095         invM.apply_scale(inverseScale);
02096         invM.set_pretrans(-postT );
02097         invM.set_posttrans(-preT );
02098 
02099 
02100         return invM;
02101 
02102 }
02103 
02104 Transform3D Transform3D::inverse() const    //   YYN need to test it for sure
02105 {
02106         // This assumes the matrix is 4 by 4 and the last row reads [0 0 0 1]
02107 
02108         float m00 = matrix[0][0]; float m01=matrix[0][1]; float m02=matrix[0][2];
02109         float m10 = matrix[1][0]; float m11=matrix[1][1]; float m12=matrix[1][2];
02110         float m20 = matrix[2][0]; float m21=matrix[2][1]; float m22=matrix[2][2];
02111         float v0  = matrix[0][3]; float v1 =matrix[1][3]; float v2 =matrix[2][3];
02112 
02113     float cof00 = m11*m22-m12*m21;
02114     float cof11 = m22*m00-m20*m02;
02115     float cof22 = m00*m11-m01*m10;
02116     float cof01 = m10*m22-m20*m12;
02117     float cof02 = m10*m21-m20*m11;
02118     float cof12 = m00*m21-m01*m20;
02119     float cof10 = m01*m22-m02*m21;
02120     float cof20 = m01*m12-m02*m11;
02121     float cof21 = m00*m12-m10*m02;
02122 
02123     float Det = m00* cof00 + m02* cof02 -m01*cof01;
02124 
02125     Transform3D invM;
02126 
02127     invM.matrix[0][0] =   cof00/Det;
02128     invM.matrix[0][1] = - cof10/Det;
02129     invM.matrix[0][2] =   cof20/Det;
02130     invM.matrix[1][0] = - cof01/Det;
02131     invM.matrix[1][1] =   cof11/Det;
02132     invM.matrix[1][2] = - cof21/Det;
02133     invM.matrix[2][0] =   cof02/Det;
02134     invM.matrix[2][1] = - cof12/Det;
02135     invM.matrix[2][2] =   cof22/Det;
02136 
02137     invM.matrix[0][3] =  (- cof00*v0 + cof10*v1 - cof20*v2 )/Det;
02138     invM.matrix[1][3] =  (  cof01*v0 - cof11*v1 + cof21*v2 )/Det;
02139     invM.matrix[2][3] =  (- cof02*v0 + cof12*v1 - cof22*v2 )/Det;
02140 
02141         Vec3f postT   = get_posttrans( ) ;
02142         Vec3f invMpre   = - postT;
02143         Vec3f invMpost   ;
02144         for ( int i = 0; i < 3; i++) {
02145                 invMpost[i] = invM.matrix[i][3];
02146                 for ( int j = 0; j < 3; j++) {
02147                         invMpost[i] += - invM.matrix[i][j]*invMpre[j];
02148                 }
02149                 invM.matrix[3][i] = invMpost[i];
02150         }
02151 
02152         return invM;
02153 }
02154 
02155 
02156 
02157 // Symmetry Stuff
02158 
02159 Transform3D Transform3D::get_sym(const string & symname, int n) const
02160 {
02161         int nsym = get_nsym(symname);
02162 
02163 //      Transform3D invalid;
02164 //      invalid.set_rotation( -0.1f, -0.1f, -0.1f);
02165 
02166         // see www.math.utah.edu/~alfeld/math/polyhedra/polyhedra.html for pictures
02167         // By default we will put largest symmetry along z-axis.
02168 
02169         // Each Platonic Solid has 2E symmetry elements.
02170 
02171 
02172         // An icosahedron has   m=5, n=3, F=20 E=30=nF/2, V=12=nF/m,since vertices shared by 5 triangles;
02173         // It is composed of 20 triangles. E=3*20/2;
02174 
02175 
02176         // An dodecahedron has m=3, n=5   F=12 E=30  V=20
02177         // It is composed of 12 pentagons. E=5*12/2;   V= 5*12/3, since vertices shared by 3 pentagons;
02178 
02179 
02180 
02181     // The ICOS symmetry group has the face along z-axis
02182 
02183         float lvl0=0;                             //  there is one pentagon on top; five-fold along z
02184         float lvl1= 63.4349f; // that is atan(2)  // there are 5 pentagons with centers at this height (angle)
02185         float lvl2=116.5651f; //that is 180-lvl1  // there are 5 pentagons with centers at this height (angle)
02186         float lvl3=180.f;                           // there is one pentagon on the bottom
02187              // Notice that 63.439 is the angle between two faces of the dual object
02188 
02189         static double ICOS[180] = { // This is with a pentagon normal to z
02190                   0,lvl0,0,    0,lvl0,288,   0,lvl0,216,   0,lvl0,144,  0,lvl0,72,
02191                   0,lvl1,36,   0,lvl1,324,   0,lvl1,252,   0,lvl1,180,  0,lvl1,108,
02192                  72,lvl1,36,  72,lvl1,324,  72,lvl1,252,  72,lvl1,180,  72,lvl1,108,
02193                 144,lvl1,36, 144,lvl1,324, 144,lvl1,252, 144,lvl1,180, 144,lvl1,108,
02194                 216,lvl1,36, 216,lvl1,324, 216,lvl1,252, 216,lvl1,180, 216,lvl1,108,
02195                 288,lvl1,36, 288,lvl1,324, 288,lvl1,252, 288,lvl1,180, 288,lvl1,108,
02196                  36,lvl2,0,   36,lvl2,288,  36,lvl2,216,  36,lvl2,144,  36,lvl2,72,
02197                 108,lvl2,0,  108,lvl2,288, 108,lvl2,216, 108,lvl2,144, 108,lvl2,72,
02198                 180,lvl2,0,  180,lvl2,288, 180,lvl2,216, 180,lvl2,144, 180,lvl2,72,
02199                 252,lvl2,0,  252,lvl2,288, 252,lvl2,216, 252,lvl2,144, 252,lvl2,72,
02200                 324,lvl2,0,  324,lvl2,288, 324,lvl2,216, 324,lvl2,144, 324,lvl2,72,
02201                   0,lvl3,0,    0,lvl3,288,   0,lvl3,216,   0,lvl3,144,   0,lvl3,72
02202         };
02203 
02204 
02205         // A cube has   m=3, n=4, F=6 E=12=nF/2, V=8=nF/m,since vertices shared by 3 squares;
02206         // It is composed of 6 squares.
02207 
02208 
02209         // An octahedron has   m=4, n=3, F=8 E=12=nF/2, V=6=nF/m,since vertices shared by 4 triangles;
02210         // It is composed of 8 triangles.
02211 
02212     // We have placed the OCT symmetry group with a face along the z-axis
02213         lvl0=0;
02214         lvl1=90;
02215         lvl2=180;
02216 
02217         static float OCT[72] = {// This is with a face of a cube along z
02218                       0,lvl0,0,   0,lvl0,90,    0,lvl0,180,    0,lvl0,270,
02219                       0,lvl1,0,   0,lvl1,90,    0,lvl1,180,    0,lvl1,270,
02220                      90,lvl1,0,  90,lvl1,90,   90,lvl1,180,   90,lvl1,270,
02221                     180,lvl1,0, 180,lvl1,90,  180,lvl1,180,  180,lvl1,270,
02222                     270,lvl1,0, 270,lvl1,90,  270,lvl1,180,  270,lvl1,270,
02223                       0,lvl2,0,   0,lvl2,90,    0,lvl2,180,    0,lvl2,270
02224         };
02225         // B^4=A^3=1;  BABA=1; implies   AA=BAB, ABA=B^3 , AB^2A = BBBABBB and
02226         //   20 words with at most a single A
02227     //   1 B BB BBB A  BA AB BBA BAB ABB BBBA BBAB BABB ABBB BBBAB BBABB BABBB
02228     //                        BBBABB BBABBB BBBABBB
02229      // also     ABBBA is distinct yields 4 more words
02230      //    ABBBA   BABBBA BBABBBA BBBABBBA
02231      // for a total of 24 words
02232      // Note A BBB A BBB A  reduces to BBABB
02233      //  and  B A BBB A is the same as A BBB A BBB etc.
02234 
02235     // The TET symmetry group has a face along the z-axis
02236     // It has n=m=3; F=4, E=6=nF/2, V=4=nF/m
02237         lvl0=0;         // There is a face along z
02238         lvl1=109.4712f;  //  that is acos(-1/3)  // There  are 3 faces at this angle
02239 
02240         static float TET[36] = {// This is with the face along z
02241               0,lvl0,0,   0,lvl0,120,    0,lvl0,240,
02242               0,lvl1,60,   0,lvl1,180,    0,lvl1,300,
02243             120,lvl1,60, 120,lvl1,180,  120,lvl1,300,
02244             240,lvl1,60, 240,lvl1,180,  240,lvl1,300
02245         };
02246         // B^3=A^3=1;  BABA=1; implies   A^2=BAB, ABA=B^2 , AB^2A = B^2AB^2 and
02247         //   12 words with at most a single A
02248     //   1 B BB  A  BA AB BBA BAB ABB BBAB BABB BBABB
02249     // at most one A is necessary
02250 
02251         Transform3D ret;
02252         SymType type = get_sym_type(symname);
02253 
02254         switch (type) {
02255         case CSYM:
02256                 ret.set_rotation( n * 360.0f / nsym, 0, 0);
02257                 break;
02258         case DSYM:
02259                 if (n >= nsym / 2) {
02260                         ret.set_rotation((n - nsym/2) * 360.0f / (nsym / 2),180.0f, 0);
02261                 }
02262                 else {
02263                         ret.set_rotation( n * 360.0f / (nsym / 2),0, 0);
02264                 }
02265                 break;
02266         case ICOS_SYM:
02267                 ret.set_rotation((float)ICOS[n * 3 ],
02268                                  (float)ICOS[n * 3 + 1],
02269                                  (float)ICOS[n * 3 + 2] );
02270                 break;
02271         case OCT_SYM:
02272                 ret.set_rotation((float)OCT[n * 3],
02273                                  (float)OCT[n * 3 + 1],
02274                                  (float)OCT[n * 3 + 2] );
02275                 break;
02276         case TET_SYM:
02277                 ret.set_rotation((float)TET[n * 3 ],
02278                                  (float)TET[n * 3 + 1] ,
02279                                  (float)TET[n * 3 + 2] );
02280                 break;
02281         case ISYM:
02282                 ret.set_rotation(0, 0, 0);
02283                 break;
02284         default:
02285                 throw InvalidValueException(type, symname);
02286         }
02287 
02288         ret = (*this) * ret;
02289 
02290         return ret;
02291 }
02292 
02293 int Transform3D::get_nsym(const string & name)
02294 {
02295         string symname = name;
02296 
02297         for (size_t i = 0; i < name.size(); i++) {
02298                 if (isalpha(name[i])) {
02299                         symname[i] = (char)tolower(name[i]);
02300                 }
02301         }
02302 
02303         SymType type = get_sym_type(symname);
02304         int nsym = 0;
02305 
02306         switch (type) {
02307         case CSYM:
02308                 nsym = atoi(symname.c_str() + 1);
02309                 break;
02310         case DSYM:
02311                 nsym = atoi(symname.c_str() + 1) * 2;
02312                 break;
02313         case ICOS_SYM:
02314                 nsym = 60;
02315                 break;
02316         case OCT_SYM:
02317                 nsym = 24;
02318                 break;
02319         case TET_SYM:
02320                 nsym = 12;
02321                 break;
02322         case ISYM:
02323                 nsym = 1;
02324                 break;
02325         case UNKNOWN_SYM:
02326         default:
02327                 throw InvalidValueException(type, name);
02328         }
02329         return nsym;
02330 }
02331 
02332 
02333 
02334 Transform3D::SymType Transform3D::get_sym_type(const string & name)
02335 {
02336         SymType t = UNKNOWN_SYM;
02337 
02338         if (name[0] == 'c') {
02339                 t = CSYM;
02340         }
02341         else if (name[0] == 'd') {
02342                 t = DSYM;
02343         }
02344         else if (name == "icos") {
02345                 t = ICOS_SYM;
02346         }
02347         else if (name == "oct") {
02348                 t = OCT_SYM;
02349         }
02350         else if (name == "tet") {
02351                 t = TET_SYM;
02352         }
02353         else if (name == "i" || name == "") {
02354                 t = ISYM;
02355         }
02356         return t;
02357 }
02358 
02359 vector<Transform3D*>
02360 Transform3D::angles2tfvec(EulerType eulertype, const vector<float> ang) {
02361         int nangles = ang.size() / 3;
02362         vector<Transform3D*> tfvec;
02363         for (int i = 0; i < nangles; i++) {
02364                 tfvec.push_back(new Transform3D(eulertype,ang[3*i],ang[3*i+1],ang[3*i+2]));
02365         }
02366         return tfvec;
02367 }
02368 
02369 
02370 
02371 
02372 /* vim: set ts=4 noet: */

Generated on Fri Apr 30 15:38:57 2010 for EMAN2 by  doxygen 1.3.9.1