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 
00728         if (cosalt >= 1) {  // that is, alt close to 0 or pi
00729                         alt = 0;
00730                         az=0;
00731                         phi = (float)EMConsts::rad2deg*(float)atan2(x_mirror_scale*matrix[0][1], x_mirror_scale*matrix[0][0]);
00732         } else if (cosalt <=-1) {
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         } else {   // for non exceptional cases:  alt well within 0, pi interval
00737 
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]);// CHANGE ME: This way also works
00741                 phi = 360.0f+(float)EMConsts::rad2deg*(float)atan2(matrix[0][2], matrix[1][2]);  // CHANGE ME: This way works
00742         //      phi = 360.0f+(float)EMConsts::rad2deg*(float)atan2(x_mirror_scale*matrix[0][2], matrix[1][2]);
00743 */
00744 
00745                 az  = 360.0f+(float)EMConsts::rad2deg*(float)atan2(scale*matrix[2][0], -scale*matrix[2][1]);
00746 //              if (sin(az* (float)EMConsts::deg2rad )* matrix[2][0]< 0) az=az+180.0f;
00747                 alt =     (float)EMConsts::rad2deg* atan(sqrt(matrix[2][0]*matrix[2][0]+matrix[2][1]*matrix[2][1])/fabs(matrix[2][2]) );
00748                 if (matrix[2][2] *scale < 0 ) {
00749                         alt=180.0f-alt;  }
00750                 phi = 360.0f+(float)EMConsts::rad2deg*(float)atan2(x_mirror_scale*matrix[0][2], matrix[1][2]);
00751         } // ends separate cases: alt close to 0, pi, or neither
00752 
00753 
00754 //      az =fmod( az+180.0f,360.0f)-180.0f;
00755 //      phi=fmod(phi+180.0f,360.0f)-180.0f;
00756 
00757         if (phi > 0) phi = fmod(phi,360.f);
00758         else phi = 360.f - fmod(fabs(phi),360.f);
00759         if (phi == 360.f) phi = 0.f;
00760 
00761         if (az > 0 ) az = fmod(az,360.f);
00762         else az = 360.f - fmod(fabs(az),360.f);
00763         if (az == 360.f) az = 0.f;
00764 
00765 //   get phiS, psiS ; SPIDER
00766         if (fabs(cosalt) > 1) {  // that is, alt close to 0
00767                 phiS=0;              // used to be if cosalt > max...
00768                 psiS = az+phi;
00769         } else {
00770                 phiS = az   - 90.0f;
00771                 psiS = phi  + 90.0f;
00772         }
00773         phiS = fmod((phiS   + 360.0f ), 360.0f) ;
00774         psiS = fmod((psiS   + 360.0f ), 360.0f) ;
00775 
00776 //   do some quaternionic stuff here
00777 
00778         float nphi = (az-phi)/2.0f;
00779     // The next is also e0
00780         float cosOover2 = (cos((az+phi)*M_PI/360) * cos(alt*M_PI/360)) ;
00781         float sinOover2 = sqrt(1 -cosOover2*cosOover2);
00782         float cosnTheta = sin((az+phi)*M_PI/360) * cos(alt*M_PI/360) / sqrt(1-cosOover2*cosOover2) ;
00783         float sinnTheta = sqrt(1-cosnTheta*cosnTheta);
00784         float n1 = sinnTheta*cos(nphi*M_PI/180);
00785         float n2 = sinnTheta*sin(nphi*M_PI/180);
00786         float n3 = cosnTheta;
00787         float xtilt = 0;
00788         float ytilt = 0;
00789         float ztilt = 0;
00790 
00791 
00792         if (cosOover2<0) {
00793                 cosOover2*=-1; n1 *=-1; n2*=-1; n3*=-1;
00794         }
00795 
00796         string type = Util::str_to_lower(euler_type);
00797 
00798         result["type"] = type;
00799         if (type == "2d") {
00800                 assert_valid_2d();
00801                 result["alpha"]  = phi;
00802         } else if (type == "eman") {
00803 //              assert_consistent_type(THREED);
00804                 result["az"]  = az;
00805                 result["alt"] = alt;
00806                 result["phi"] = phi;
00807         } else if (type == "imagic") {
00808 //              assert_consistent_type(THREED);
00809                 result["alpha"] = az;
00810                 result["beta"] = alt;
00811                 result["gamma"] = phi;
00812         } else if (type == "spider") {
00813 //              assert_consistent_type(THREED);
00814                 result["phi"]   = phiS;  // The first Euler like az
00815                 result["theta"] = alt;
00816                 result["psi"]   = psiS;
00817         } else if (type == "mrc") {
00818 //              assert_consistent_type(THREED);
00819                 result["phi"]   = phiS;
00820                 result["theta"] = alt;
00821                 result["omega"] = psiS;
00822         } else if (type == "xyz") {
00823 //              assert_consistent_type(THREED);
00824                 xtilt = atan2(-sin((M_PI/180.0f)*phiS)*sin((M_PI/180.0f)*alt),cos((M_PI/180.0f)*alt));
00825                 ytilt = asin(  cos((M_PI/180.0f)*phiS)*sin((M_PI/180.0f)*alt));
00826                 ztilt = psiS*M_PI/180.0f - atan2(sin(xtilt), cos(xtilt) *sin(ytilt));
00827 
00828                 xtilt=fmod(xtilt*180/M_PI+540.0f,360.0f) -180.0f;
00829                 ztilt=fmod(ztilt*180/M_PI+540.0f,360.0f) -180.0f;
00830 
00831                 result["xtilt"]  = xtilt;
00832                 result["ytilt"]  = ytilt*180/M_PI;
00833                 result["ztilt"]  = ztilt;
00834         } else if (type == "quaternion") {
00835 //              assert_consistent_type(THREED);
00836                 result["e0"] = cosOover2 ;
00837                 result["e1"] = sinOover2 * n1 ;
00838                 result["e2"] = sinOover2 * n2;
00839                 result["e3"] = sinOover2 * n3;
00840         } else if (type == "spin") {
00841 //              assert_consistent_type(THREED);
00842                 result["Omega"] =360.0f* acos(cosOover2)/ M_PI ;
00843                 result["n1"] = n1;
00844                 result["n2"] = n2;
00845                 result["n3"] = n3;
00846         } else if (type == "sgirot") {
00847 //              assert_consistent_type(THREED);
00848                 result["q"] = 360.0f*acos(cosOover2)/M_PI ;
00849                 result["n1"] = n1;
00850                 result["n2"] = n2;
00851                 result["n3"] = n3;
00852         } else if (type == "matrix") {
00853 //              assert_consistent_type(THREED);
00854                 result["m11"] = x_mirror_scale*matrix[0][0]*inv_scale;
00855                 result["m12"] = x_mirror_scale*matrix[0][1]*inv_scale;
00856                 result["m13"] = x_mirror_scale*matrix[0][2]*inv_scale;
00857                 result["m21"] = matrix[1][0]*inv_scale;
00858                 result["m22"] = matrix[1][1]*inv_scale;
00859                 result["m23"] = matrix[1][2]*inv_scale;
00860                 result["m31"] = matrix[2][0]*inv_scale;
00861                 result["m32"] = matrix[2][1]*inv_scale;
00862                 result["m33"] = matrix[2][2]*inv_scale;
00863         } else {
00864                 throw InvalidStringException(euler_type, "unknown Euler Type");
00865         }
00866 
00867         return result;
00868 }
00869 
00870 void Transform::set_trans(const float& x, const float& y, const float& z)
00871 {
00872 //      if ( z != 0.0 ) {
00873 //              validate_and_set_type(THREED);
00874 //      }
00875         bool x_mirror = get_mirror();
00876 
00877         if (x_mirror) matrix[0][3] = -x;
00878         else matrix[0][3] = x;
00879         matrix[1][3] = y;
00880         matrix[2][3] = z;
00881 }
00882 
00883 Vec3f Transform::get_trans() const
00884 {
00885         // No type asserted
00886         bool x_mirror = get_mirror();
00887         Vec3f v;
00888         if (x_mirror) v[0] = -matrix[0][3];
00889         else v[0] = matrix[0][3];
00890         v[1] = matrix[1][3];
00891         v[2] = matrix[2][3];
00892 
00893         Util::apply_precision(v[0],ERR_LIMIT);
00894         Util::apply_precision(v[1],ERR_LIMIT);
00895         Util::apply_precision(v[2],ERR_LIMIT);
00896 
00897         return v;
00898 }
00899 
00900 Vec2f Transform::get_trans_2d() const
00901 {
00902         bool x_mirror = get_mirror();
00903         Vec2f v;
00904         if (x_mirror) v[0] = -matrix[0][3];
00905         else v[0] = matrix[0][3];
00906         v[1] = matrix[1][3];
00907         return v;
00908 }
00909 
00910 
00911 
00912 Vec3f Transform::get_pre_trans() 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();
00921 }
00922 
00923 Vec2f Transform::get_pre_trans_2d() const
00924 {
00925         Transform T(*this);
00926         T.set_trans(0,0,0);
00927         T.invert();
00928 
00929         Transform soln  = T*(*this);
00930 //      soln.printme();
00931         return soln.get_trans_2d();
00932 }
00933 
00934 // Vec2f Transform::get_pre_trans_2d() const
00935 // {
00936 //      Vec3f v = get_pre_trans();
00937 //      return Vec2f(v[0],v[1]);
00938 // }
00939 
00940 
00941 void Transform::set_scale(const float& new_scale) {
00942         if (new_scale <= 0) {
00943                 throw InvalidValueException(new_scale,"The scale factor in a Transform object must be positive and non zero");
00944         }
00945         // Transform = MTSR (Mirroring, Translation, Scaling, Rotate)
00946         // So changing the scale boils down to this....
00947 
00948         float old_scale = get_scale();
00949 
00950         float n_scale = new_scale;
00951         Util::apply_precision(n_scale,ERR_LIMIT);
00952 
00953         float corrected_scale = n_scale/old_scale;
00954         if ( corrected_scale != 1.0 ) {
00955                 for(int i = 0; i < 3;  ++i ) {
00956                         for(int j = 0; j < 3; ++j ) {
00957                                 matrix[i][j] *= corrected_scale;
00958                         }
00959                 }
00960         }
00961 }
00962 
00963 float Transform::get_scale() const {
00964         float determinant = get_determinant();
00965         if (determinant < 0 ) determinant *= -1;
00966 
00967         float scale = std::pow(determinant,1.0f/3.0f);
00968         int int_scale = static_cast<int>(scale);
00969         float scale_residual = scale-static_cast<float>(int_scale);
00970         if  ( scale_residual < ERR_LIMIT ) { scale = static_cast<float>(int_scale); };
00971 
00972         Util::apply_precision(scale, ERR_LIMIT);
00973 
00974         return scale;
00975 }
00976 
00977 void print_matrix(gsl_matrix* M, unsigned int r, unsigned int c, const string& message ) {
00978         cout << "Message is " << message << endl;
00979         for ( unsigned int i = 0; i < r; ++i )
00980         {
00981                 for ( unsigned int j = 0; j < c; ++j )
00982                 {
00983                         cout << gsl_matrix_get(M,i,j) << " ";
00984                 }
00985                 cout << endl;
00986         }
00987 }
00988 
00989 void Transform::orthogonalize()
00990 {
00991         float scale;
00992         bool x_mirror;
00993         get_scale_and_mirror(scale,x_mirror);
00994         if (scale == 0) throw UnexpectedBehaviorException("The determinant of the Transform is 0. This is unexpected.");
00995         double inv_scale = 1.0/static_cast<double>(scale);
00996         double mirror_scale = (x_mirror == true ? -1.0:1.0);
00997 
00998         gsl_matrix * R = gsl_matrix_calloc(3,3);
00999         for ( unsigned int i = 0; i < 3; ++i )
01000         {
01001                 for ( unsigned int j = 0; j < 3; ++j )
01002                 {
01003                         if (i == 0 && mirror_scale != 1.0 ) {
01004                                 gsl_matrix_set( R, i, j, static_cast<double>(matrix[i][j])*mirror_scale*inv_scale );
01005                         }
01006                         else {
01007                                 gsl_matrix_set( R, i, j, static_cast<double>(matrix[i][j])*inv_scale );
01008                         }
01009                 }
01010         }
01011 
01012         gsl_matrix * V = gsl_matrix_calloc(3,3);
01013         gsl_vector * S = gsl_vector_calloc(3);
01014         gsl_vector * work = gsl_vector_calloc(3);
01015         gsl_linalg_SV_decomp (R, V, S, work); // Now R is U of the SVD R = USV^T
01016 
01017         gsl_matrix * Soln = gsl_matrix_calloc(3,3);
01018         gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, R, V, 0.0, Soln);
01019 
01020         for ( unsigned int i = 0; i < 3; ++i )
01021         {
01022                 for ( unsigned int j = 0; j < 3; ++j )
01023                 {
01024                         matrix[i][j] = static_cast<float>( gsl_matrix_get(Soln,i,j) );
01025                 }
01026         }
01027 
01028         // Apply scale if it existed previously
01029         if (scale != 1.0f) {
01030                 for(int i=0; i<3; ++i) {
01031                         for(int j=0; j<3; ++j) {
01032                                 matrix[i][j] *= scale;
01033                         }
01034                 }
01035         }
01036 
01037         // Apply post x mirroring if it was applied previouslys
01038         if ( x_mirror ) {
01039                 for(int j=0; j<3; ++j) {
01040                         matrix[0][j] *= -1.0f;
01041                 }
01042         }
01043 
01044         gsl_matrix_free(V); gsl_matrix_free(R); gsl_matrix_free(Soln);
01045         gsl_vector_free(S); gsl_vector_free(work);
01046 }
01047 
01048 void Transform::set_mirror(const bool x_mirror ) {
01049 
01050         bool old_x_mirror = get_mirror();
01051         if (old_x_mirror == x_mirror) return; // The user is setting the same value
01052         else {
01053                 // Toggle the mirroring operation
01054                 for (int j = 0; j < 4; ++j ) {
01055                         matrix[0][j] *= -1;
01056                 }
01057         }
01058 }
01059 
01060 bool Transform::get_mirror() const {
01061         float determinant = get_determinant();
01062 
01063         bool x_mirror = false;
01064         if ( determinant < 0 ) x_mirror = true;
01065 
01066         return x_mirror;
01067 
01068 }
01069 
01070 void Transform::get_scale_and_mirror(float& scale, bool& x_mirror) const {
01071 
01072         float determinant = get_determinant();
01073         x_mirror = false;
01074         if ( determinant < 0 ) {
01075                 x_mirror = true;
01076                 determinant *= -1;
01077         }
01078         if (determinant != 1 ) {
01079                 scale = std::pow(determinant,1.0f/3.0f);
01080                 int int_scale = static_cast<int>(scale);
01081                 float scale_residual = scale-static_cast<float>(int_scale);
01082                 if  ( scale_residual < ERR_LIMIT ) { scale = static_cast<float>(int_scale); };
01083         }
01084         else scale = 1;
01085 
01086         Util::apply_precision(scale,ERR_LIMIT);
01087 }
01088 
01089 float Transform::get_determinant() const
01090 {
01091         float det = matrix[0][0]*(matrix[1][1]*matrix[2][2]-matrix[2][1]*matrix[1][2]);
01092         det -= matrix[0][1]*(matrix[1][0]*matrix[2][2]-matrix[2][0]*matrix[1][2]);
01093         det += matrix[0][2]*(matrix[1][0]*matrix[2][1]-matrix[2][0]*matrix[1][1]);
01094 
01095         Util::apply_precision(det,ERR_LIMIT);
01096 
01097         return det;
01098 }
01099 
01100 void Transform::invert() {
01101 
01102         float m00 = matrix[0][0]; float m01=matrix[0][1]; float m02=matrix[0][2];
01103         float m10 = matrix[1][0]; float m11=matrix[1][1]; float m12=matrix[1][2];
01104         float m20 = matrix[2][0]; float m21=matrix[2][1]; float m22=matrix[2][2];
01105         float v0  = matrix[0][3]; float v1 =matrix[1][3]; float v2 =matrix[2][3];
01106 
01107         float cof00 = m11*m22-m12*m21;
01108         float cof11 = m22*m00-m20*m02;
01109         float cof22 = m00*m11-m01*m10;
01110         float cof01 = m10*m22-m20*m12;
01111         float cof02 = m10*m21-m20*m11;
01112         float cof12 = m00*m21-m01*m20;
01113         float cof10 = m01*m22-m02*m21;
01114         float cof20 = m01*m12-m02*m11;
01115         float cof21 = m00*m12-m10*m02;
01116 
01117         float det = m00* cof00 + m02* cof02 -m01*cof01;
01118 
01119         matrix[0][0] =   cof00/det;
01120         matrix[0][1] = - cof10/det;
01121         matrix[0][2] =   cof20/det;
01122         matrix[1][0] = - cof01/det;
01123         matrix[1][1] =   cof11/det;
01124         matrix[1][2] = - cof21/det;
01125         matrix[2][0] =   cof02/det;
01126         matrix[2][1] = - cof12/det;
01127         matrix[2][2] =   cof22/det;
01128 
01129         matrix[0][3] =  (- cof00*v0 + cof10*v1 - cof20*v2 )/det;
01130         matrix[1][3] =  (  cof01*v0 - cof11*v1 + cof21*v2 )/det;
01131         matrix[2][3] =  (- cof02*v0 + cof12*v1 - cof22*v2 )/det;
01132 }
01133 
01134 Transform Transform::inverse() const {
01135         Transform t(*this);
01136         t.invert();
01137         return t;
01138 }
01139 
01140 void Transform::transpose_inplace() {
01141         float tempij;
01142         for (int i = 0; i < 3; i++) {
01143                 for (int j = 0; j < i; j++) {
01144                         if (i != j) {
01145                                 tempij= matrix[i][j];
01146                                 matrix[i][j] = matrix[j][i];
01147                                 matrix[j][i] = tempij;
01148                         }
01149                 }
01150         }
01151 }
01152 
01153 Transform Transform::transpose() const {
01154         Transform t(*this);
01155         t.transpose_inplace();
01156         return t;
01157 }
01158 
01159 
01160 Transform EMAN::operator*(const Transform & M2, const Transform & M1)     // YYY
01161 {
01162         Transform result;
01163         for (int i=0; i<3; i++) {
01164                 for (int j=0; j<4; j++) {
01165                         result[i][j] = M2[i][0] * M1[0][j] +  M2[i][1] * M1[1][j] + M2[i][2] * M1[2][j];
01166                 }
01167                 result[i][3] += M2[i][3];
01168         }
01169 
01170         return result;
01171 }
01172 
01173 void Transform::assert_valid_2d() const {
01174         int rotation_error = 0;
01175         int translation_error = 0;
01176         if (fabs(matrix[2][0]) > ERR_LIMIT) rotation_error++;
01177         if (fabs(matrix[2][1]) > ERR_LIMIT) rotation_error++;
01178         if (fabs(matrix[2][3]) > ERR_LIMIT) translation_error++;
01179         if (fabs(matrix[0][2]) > ERR_LIMIT) rotation_error++;
01180         if (fabs(matrix[1][2]) > ERR_LIMIT) rotation_error++;
01181         if (fabs(matrix[2][2]-1.0) >ERR_LIMIT) rotation_error++; 
01182         if ( translation_error && rotation_error ) {
01183                 throw UnexpectedBehaviorException("Error, the internal matrix contains 3D rotations and 3D translations. This object can not be considered 2D");
01184         } else if ( translation_error ) {
01185                 throw UnexpectedBehaviorException("Error, the internal matrix contains a non zero z component for a 3D translation. This object can not be considered 2D");
01186         }
01187         else if ( rotation_error ) {
01188                 throw UnexpectedBehaviorException("Error, the internal matrix contains 3D rotations and this object can not be considered 2D");
01189         }
01190 
01191 }
01192 
01193 
01194 
01195 Transform Transform::get_sym(const string & sym_name, int n) const
01196 {
01197         Symmetry3D* sym = Factory<Symmetry3D>::get(sym_name);
01198         Transform ret;
01199         ret = (*this) * sym->get_sym(n);
01200         delete sym;
01201         return ret;
01202 }
01203 
01204 int Transform::get_nsym(const string & sym_name)
01205 {
01206         Symmetry3D* sym = Factory<Symmetry3D>::get(sym_name);
01207         int nsym = sym->get_nsym();
01208         delete sym;
01209         return nsym;
01210 }
01211 
01212 Transform3D::Transform3D()  //    C1
01213 {
01214         init();
01215 }
01216 
01217 Transform3D::Transform3D( const Transform3D& rhs )
01218 {
01219     for( int i=0; i < 4; ++i )
01220     {
01221         for( int j=0; j < 4; ++j )
01222         {
01223             matrix[i][j] = rhs.matrix[i][j];
01224         }
01225     }
01226 }
01227 
01228 // C2
01229 Transform3D::Transform3D(const float& az, const float& alt, const float& phi)
01230 {
01231         init();
01232         set_rotation(az,alt,phi);
01233 }
01234 
01235 
01236 //  C3  Usual Constructor: Post Trans, after appying Rot
01237 Transform3D::Transform3D(const float& az, const float& alt, const float& phi, const Vec3f& posttrans )
01238 {
01239         init(); // This is called in set_rotation
01240         set_rotation(az,alt,phi);
01241         set_posttrans(posttrans);
01242 }
01243 
01244 Transform3D::Transform3D(const float& m11, const float& m12, const float& m13,
01245                                                  const float& m21, const float& m22, const float& m23,
01246            const float& m31, const float& m32, const float& m33)
01247 {
01248         init();
01249         set_rotation(m11,m12,m13,m21,m22,m23,m31,m32,m33);
01250 }
01251 
01252 // C4
01253 Transform3D::Transform3D(EulerType euler_type, const float& a1, const float& a2, const float& a3)
01254 {
01255         init();
01256         set_rotation(euler_type,a1,a2,a3);
01257 }
01258 
01259 Transform3D::Transform3D(EulerType euler_type, const float& a1, const float& a2, const float& a3, const float& a4)
01260 {
01261         init();
01262         set_rotation(euler_type,a1,a2,a3,a4);
01263 }
01264 
01265 
01266 // C5
01267 Transform3D::Transform3D(EulerType euler_type, const Dict& rotation)  //YYY
01268 {
01269         init();
01270         set_rotation(euler_type,rotation);
01271 }
01272 
01273 
01274 // C6   First apply pretrans: Then rotation: Then posttrans
01275 
01276 Transform3D::Transform3D(  const Vec3f& pretrans,  const float& az, const float& alt, const float& phi, const Vec3f& posttrans )  //YYY  by default EMAN
01277 {
01278         init();
01279         set_pretrans(pretrans);
01280         set_rotation(az,alt,phi);
01281         set_posttrans(posttrans);
01282 }
01283 
01284 
01285 
01286 
01287 Transform3D::~Transform3D()
01288 {
01289 }
01290 
01291 
01292 
01293 void Transform3D::to_identity()
01294 {
01295 //      for (int i = 0; i < 3; i++) {
01296 //              matrix[i][i] = 1;
01297 //      }
01298 
01299         for(int i=0; i<4; ++i) {
01300                 for(int j=0; j<4; ++j) {
01301                         if(i==j) {
01302                                 matrix[i][j] = 1;
01303                         }
01304                         else {
01305                                 matrix[i][j] = 0;
01306                         }
01307                 }
01308         }
01309         post_x_mirror = false;
01310         set_center(Vec3f(0,0,0));
01311 }
01312 
01313 
01314 
01315 bool Transform3D::is_identity()  // YYY
01316 {
01317         for (int i=0; i<4; i++) {
01318                 for (int j=0; j<4; j++) {
01319                         if (i==j && matrix[i][j]!=1.0) return 0;
01320                         if (i!=j && matrix[i][j]!=0.0) return 0;
01321                 }
01322         }
01323         return 1;
01324 }
01325 
01326 
01327 void Transform3D::set_center(const Vec3f & center) //YYN
01328 {
01329         set_pretrans( Vec3f(0,0,0)-center);
01330         for (int i = 0; i < 3; i++) {
01331                 matrix[i][3]=center[i];
01332         }
01333 }
01334 
01335 //            METHODS
01336 //   Note Transform3Ds are initialized as identities
01337 void Transform3D::init()  // M1
01338 {
01339         to_identity();
01340 }
01341 
01342 //      Set Methods
01343 
01344 void Transform3D::set_pretrans(const float& dx, const float& dy, const float& dz) // YYY
01345 {    set_pretrans( Vec3f(dx,dy,dz)); }
01346 
01347 
01348 void Transform3D::set_pretrans(const float& dx, const float& dy) // YYY
01349 {    set_pretrans( Vec3f(dx,dy,0)); }
01350 
01351 void Transform3D::set_pretrans(const Vec2f& pretrans) // YYY
01352 {    set_pretrans( Vec3f(pretrans[0],pretrans[1],0)); }
01353 
01354 void Transform3D::set_pretrans(const Vec3f & preT)  // flag=1 means keep the old value of total trans
01355 {
01356                 int flag=0;
01357 
01358 //     transFinal = transPost +  Rotation * transPre;
01359 //    This will keep the old value of transPost and change the value of pretrans and the total matrix
01360     if (flag==0){
01361                 matrix[0][3] = matrix[3][0] + matrix[0][0]*preT[0] + matrix[0][1]*preT[1] + matrix[0][2]*preT[2]  ;
01362                 matrix[1][3] = matrix[3][1] + matrix[1][0]*preT[0] + matrix[1][1]*preT[1] + matrix[1][2]*preT[2]  ;
01363                 matrix[2][3] = matrix[3][2] + matrix[2][0]*preT[0] + matrix[2][1]*preT[1] + matrix[2][2]*preT[2]  ;
01364         }
01365 //    This will keep the old value of total translation and change the value of posttrans
01366     if (flag==1){
01367                 matrix[3][0] = matrix[0][3] - (matrix[0][0]*preT[0] + matrix[0][1]*preT[1] + matrix[0][2]*preT[2])  ;
01368                 matrix[3][1] = matrix[1][3] - (matrix[1][0]*preT[0] + matrix[1][1]*preT[1] + matrix[1][2]*preT[2])  ;
01369                 matrix[3][2] = matrix[2][3] - (matrix[2][0]*preT[0] + matrix[2][1]*preT[1] + matrix[2][2]*preT[2])  ;
01370         }
01371 }
01372 
01373 
01374 void Transform3D::set_posttrans(const float& dx, const float& dy, const float& dz) // YYY
01375 {    set_posttrans( Vec3f(dx,dy,dz)); }
01376 
01377 
01378 void Transform3D::set_posttrans(const float& dx, const float& dy) // YYY
01379 {    set_posttrans( Vec3f(dx,dy,0)); }
01380 
01381 void Transform3D::set_posttrans(const Vec2f& posttrans) // YYY
01382 {    set_pretrans( Vec3f(posttrans[0],posttrans[1],0)); }
01383 
01384 void Transform3D::set_posttrans(const Vec3f & posttrans) // flag=1 means keep the old value of total trans
01385 {
01386         int flag=0;
01387     Vec3f preT   = get_pretrans(0) ;
01388         for (int i = 0; i < 3; i++) {
01389                 matrix[3][i] = posttrans[i];
01390         }
01391 //     transFinal = transPost +  Rotation * transPre;
01392 //   This will keep the old value of pretrans and change the value of posttrans and the total matrix
01393         if (flag==0) {
01394                 matrix[0][3] = matrix[3][0] + matrix[0][0]*preT[0] + matrix[0][1]*preT[1] + matrix[0][2]*preT[2]  ;
01395                 matrix[1][3] = matrix[3][1] + matrix[1][0]*preT[0] + matrix[1][1]*preT[1] + matrix[1][2]*preT[2]  ;
01396                 matrix[2][3] = matrix[3][2] + matrix[2][0]*preT[0] + matrix[2][1]*preT[1] + matrix[2][2]*preT[2]  ;
01397         }
01398 //   This will keep the old value of the total matrix, and c
01399         if (flag==1) { // Don't do anything
01400         }
01401 }
01402 
01403 
01404 
01405 
01406 void Transform3D::apply_scale(const float& scale)    // YYY
01407 {
01408         for (int i = 0; i < 3; i++) {
01409                 for (int j = 0; j < 4; j++) {
01410                         matrix[i][j] *= scale;
01411                 }
01412         }
01413         for (int j = 0; j < 3; j++) {
01414                 matrix[3][j] *= scale;
01415         }
01416 }
01417 
01418 void Transform3D::orthogonalize()  // YYY
01419 {
01420         //EulerType EMAN;
01421         float scale = get_scale() ;
01422         float inverseScale= 1/scale ;
01423         apply_scale(inverseScale);
01424 //      Dict angs = get_rotation(EMAN);
01425 //      set_Rotation(EMAN,angs);
01426 }
01427 
01428 
01429 void Transform3D::transpose()  // YYY
01430 {
01431         float tempij;
01432         for (int i = 0; i < 3; i++) {
01433                 for (int j = 0; j < i; j++) {
01434                         tempij= matrix[i][j];
01435                         matrix[i][j] = matrix[j][i];
01436                         matrix[j][i] = tempij;
01437                 }
01438         }
01439 }
01440 
01441 void Transform3D::set_scale(const float& scale)    // YYY
01442 {
01443         float OldScale= get_scale();
01444         float Scale2Apply = scale/OldScale;
01445         apply_scale(Scale2Apply);
01446 }
01447 
01448 float Transform3D::get_mag() const //
01449 {
01450         EulerType eulertype= SPIN ;
01451         Dict AA= get_rotation(eulertype);
01452         return AA["Omega"];
01453 }
01454 
01455 Vec3f Transform3D::get_finger() const //
01456 {
01457         EulerType eulertype= SPIN ;
01458         Dict AA= get_rotation(eulertype);
01459         return Vec3f(AA["n1"],AA["n2"],AA["n3"]);
01460 }
01461 
01462 Vec3f Transform3D::get_posttrans(int flag) const    //
01463 {
01464         if (flag==0){
01465                 return Vec3f(matrix[3][0], matrix[3][1], matrix[3][2]);
01466         }
01467         // otherwise as if all the translation was post
01468         return Vec3f(matrix[0][3], matrix[1][3], matrix[2][3]);
01469 }
01470 
01471 Vec3f Transform3D::get_total_posttrans() const {
01472         return get_posttrans(1);
01473 }
01474 
01475 Vec3f Transform3D::get_total_pretrans() const {
01476         return get_pretrans(1);
01477 }
01478 
01479 
01480 Vec3f Transform3D::get_pretrans(int flag) const    // Fix Me
01481 {
01482 //      The expression is R^T(v_total - v_post);
01483 
01484         Vec3f pretrans;
01485         Vec3f posttrans(matrix[3][0], matrix[3][1], matrix[3][2]);
01486         Vec3f tottrans(matrix[0][3], matrix[1][3], matrix[2][3]);
01487         Vec3f totminuspost;
01488 
01489         totminuspost = tottrans;
01490         if (flag==0) {
01491                 totminuspost = tottrans-posttrans;
01492         }
01493 
01494         Transform3D Rinv = inverse();
01495         for (int i=0; i<3; i++) {
01496                 float ptnow=0;
01497                 for (int j=0; j<3; j++) {
01498                         ptnow +=   Rinv.matrix[i][j]* totminuspost[j] ;
01499                 }
01500                 pretrans.set_value_at(i,ptnow) ;  //
01501         }
01502         return pretrans;
01503 }
01504 
01505 
01506  Vec3f Transform3D::get_center() const  // YYY
01507  {
01508         return Vec3f();
01509  }
01510 
01511 
01512 
01513 Vec3f Transform3D::get_matrix3_col(int i) const     // YYY
01514 {
01515         return Vec3f(matrix[0][i], matrix[1][i], matrix[2][i]);
01516 }
01517 
01518 
01519 Vec3f Transform3D::get_matrix3_row(int i) const     // YYY
01520 {
01521         return Vec3f(matrix[i][0], matrix[i][1], matrix[i][2]);
01522 }
01523 
01524 Vec3f Transform3D::transform(const Vec3f & v3f) const     // YYY
01525 {
01526 //      This is the transformation of a vector, v by a matrix M
01527         float x = matrix[0][0] * v3f[0] + matrix[0][1] * v3f[1] + matrix[0][2] * v3f[2] + matrix[0][3] ;
01528         float y = matrix[1][0] * v3f[0] + matrix[1][1] * v3f[1] + matrix[1][2] * v3f[2] + matrix[1][3] ;
01529         float z = matrix[2][0] * v3f[0] + matrix[2][1] * v3f[1] + matrix[2][2] * v3f[2] + matrix[2][3] ;
01530         return Vec3f(x, y, z);
01531 }
01532 
01533 
01534 Vec3f Transform3D::rotate(const Vec3f & v3f) const     // YYY
01535 {
01536 //      This is the rotation of a vector, v by a matrix M
01537         float x = matrix[0][0] * v3f[0] + matrix[0][1] * v3f[1] + matrix[0][2] * v3f[2]  ;
01538         float y = matrix[1][0] * v3f[0] + matrix[1][1] * v3f[1] + matrix[1][2] * v3f[2]  ;
01539         float z = matrix[2][0] * v3f[0] + matrix[2][1] * v3f[1] + matrix[2][2] * v3f[2]  ;
01540         return Vec3f(x, y, z);
01541 }
01542 
01543 
01544 Transform3D EMAN::operator*(const Transform3D & M2, const Transform3D & M1)     // YYY
01545 {
01546 //       This is the  left multiplication of a matrix M1 by a matrix M2; that is M2*M1
01547 //       It returns a new matrix
01548         Transform3D resultant;
01549         for (int i=0; i<3; i++) {
01550                 for (int j=0; j<4; j++) {
01551                         resultant[i][j] = M2[i][0] * M1[0][j] +  M2[i][1] * M1[1][j] + M2[i][2] * M1[2][j];
01552                 }
01553                 resultant[i][3] += M2[i][3];  // add on the new translation (not included above)
01554         }
01555 
01556         for (int j=0; j<3; j++) {
01557                 resultant[3][j] = M2[3][j];
01558         }
01559 
01560         return resultant; // This will have the post_trans of M2
01561 }
01562 
01563 /*             Here starts the pure rotation stuff */
01564 
01635 void Transform3D::set_rotation(const float& az, const float& alt, const float& phi )
01636 {
01637         EulerType euler_type=EMAN;
01638         Dict rot;
01639         rot["az"]  = az;
01640         rot["alt"] = alt;
01641         rot["phi"] = phi;
01642         set_rotation(euler_type, rot);
01643 }
01644 
01645 // This is where it all happens;
01646 void Transform3D::set_rotation(EulerType euler_type, const float& a1, const float& a2, const float& a3)
01647 {
01648         init();
01649         Dict rot;
01650         switch(euler_type) {
01651                 case EMAN:
01652                         rot["az"]  = a1;
01653                         rot["alt"] = a2;
01654                         rot["phi"] = a3;
01655                         break;
01656                 case SPIDER:
01657                         rot["phi"]   = a1;
01658                         rot["theta"] = a2;
01659                         rot["psi"]   = a3;
01660                         break;
01661                 case IMAGIC:
01662                         rot["alpha"]   = a1;
01663                         rot["beta"] = a2;
01664                         rot["gamma"]   = a3;
01665                         break;
01666                 case MRC:
01667                         rot["phi"]   = a1;
01668                         rot["theta"] = a2;
01669                         rot["omega"]   = a3;
01670                         break;
01671                 case XYZ:
01672                         rot["xtilt"]   = a1;
01673                         rot["ytilt"] = a2;
01674                         rot["ztilt"]   = a3;
01675                         break;
01676                 default:
01677                 throw InvalidValueException(euler_type, "cannot instantiate this Euler Type");
01678         }  // ends switch euler_type
01679         set_rotation(euler_type, rot);
01680 }
01681 
01682 // This is where it all happens;
01683 void Transform3D::set_rotation(EulerType euler_type, const float& a1, const float& a2, const float& a3, const float& a4)
01684 {
01685         init();
01686         Dict rot;
01687         switch(euler_type) {
01688                 case QUATERNION:
01689                         rot["e0"]  = a1;
01690                         rot["e1"] = a2;
01691                         rot["e2"] = a3;
01692                         rot["e3"] = a4;
01693                         break;
01694                 case SGIROT:
01695                         rot["q"]  = a1;
01696                         rot["n1"] = a2;
01697                         rot["n2"] = a3;
01698                         rot["n3"] = a4;
01699                 case SPIN:
01700                         rot["Omega"]  = a1;
01701                         rot["n1"] = a2;
01702                         rot["n2"] = a3;
01703                         rot["n3"] = a4;
01704                         break;
01705                 default:
01706                         throw InvalidValueException(euler_type, "cannot instantiate this Euler Type");
01707         }  // ends switch euler_type
01708         set_rotation(euler_type, rot);
01709 }
01710 
01711 void Transform3D::set_rotation(EulerType euler_type, const Dict& rotation)
01712 {
01713         float e0  = 0;float e1=0; float e2=0; float e3=0;
01714         float Omega=0;
01715         float az  = 0;
01716         float alt = 0;
01717         float phi = 0;
01718         float cxtilt = 0;
01719         float sxtilt = 0;
01720         float cytilt = 0;
01721         float sytilt = 0;
01722         bool is_quaternion = 0;
01723         bool is_matrix = 0;
01724 
01725         switch(euler_type) {
01726         case EMAN:
01727                 az  = (float)rotation["az"] ;
01728                 alt = (float)rotation["alt"]  ;
01729                 phi = (float)rotation["phi"] ;
01730                 break;
01731         case IMAGIC:
01732                 az  = (float)rotation["alpha"] ;
01733                 alt = (float)rotation["beta"]  ;
01734                 phi = (float)rotation["gamma"] ;
01735                 break;
01736 
01737         case SPIDER:
01738                 az =  (float)rotation["phi"]    + 90.0f;
01739                 alt = (float)rotation["theta"] ;
01740                 phi = (float)rotation["psi"]    - 90.0f;
01741                 break;
01742 
01743         case XYZ:
01744                 cxtilt = cos( (M_PI/180.0f)*(float)rotation["xtilt"]);
01745                 sxtilt = sin( (M_PI/180.0f)*(float)rotation["xtilt"]);
01746                 cytilt = cos( (M_PI/180.0f)*(float)rotation["ytilt"]);
01747                 sytilt = sin( (M_PI/180.0f)*(float)rotation["ytilt"]);
01748                 az =  (180.0f/M_PI)*atan2(-cytilt*sxtilt,sytilt)   + 90.0f ;
01749                 alt = (180.0f/M_PI)*acos(cytilt*cxtilt)  ;
01750                 phi = (float)rotation["ztilt"] +(180.0f/M_PI)*atan2(sxtilt,cxtilt*sytilt)   - 90.0f ;
01751                 break;
01752 
01753         case MRC:
01754                 az  = (float)rotation["phi"]   + 90.0f ;
01755                 alt = (float)rotation["theta"] ;
01756                 phi = (float)rotation["omega"] - 90.0f ;
01757                 break;
01758 
01759         case QUATERNION:
01760                 is_quaternion = 1;
01761                 e0 = (float)rotation["e0"];
01762                 e1 = (float)rotation["e1"];
01763                 e2 = (float)rotation["e2"];
01764                 e3 = (float)rotation["e3"];
01765                 break;
01766 
01767         case SPIN:
01768                 is_quaternion = 1;
01769                 Omega = (float)rotation["Omega"];
01770                 e0 = cos(Omega*M_PI/360.0f);
01771                 e1 = sin(Omega*M_PI/360.0f)* (float)rotation["n1"];
01772                 e2 = sin(Omega*M_PI/360.0f)* (float)rotation["n2"];
01773                 e3 = sin(Omega*M_PI/360.0f)* (float)rotation["n3"];
01774                 break;
01775 
01776         case SGIROT:
01777                 is_quaternion = 1;
01778                 Omega = (float)rotation["q"]  ;
01779                 e0 = cos(Omega*M_PI/360.0f);
01780                 e1 = sin(Omega*M_PI/360.0f)* (float)rotation["n1"];
01781                 e2 = sin(Omega*M_PI/360.0f)* (float)rotation["n2"];
01782                 e3 = sin(Omega*M_PI/360.0f)* (float)rotation["n3"];
01783                 break;
01784 
01785         case MATRIX:
01786                 is_matrix = 1;
01787                 matrix[0][0] = (float)rotation["m11"]  ;
01788                 matrix[0][1] = (float)rotation["m12"]  ;
01789                 matrix[0][2] = (float)rotation["m13"]  ;
01790                 matrix[1][0] = (float)rotation["m21"]  ;
01791                 matrix[1][1] = (float)rotation["m22"]  ;
01792                 matrix[1][2] = (float)rotation["m23"]  ;
01793                 matrix[2][0] = (float)rotation["m31"]  ;
01794                 matrix[2][1] = (float)rotation["m32"]  ;
01795                 matrix[2][2] = (float)rotation["m33"]  ;
01796                 break;
01797 
01798         default:
01799                 throw InvalidValueException(euler_type, "unknown Euler Type");
01800         }  // ends switch euler_type
01801 
01802 
01803         Vec3f postT  = get_posttrans( ) ;
01804         Vec3f preT   = get_pretrans( ) ;
01805 
01806 
01807         float azp  = fmod(az,360.0f)*M_PI/180.0f;
01808         float altp  = alt*M_PI/180.0f;
01809         float phip = fmod(phi,360.0f)*M_PI/180.0f;
01810 
01811         if (!is_quaternion && !is_matrix) {
01812                 matrix[0][0] =  cos(phip)*cos(azp) - cos(altp)*sin(azp)*sin(phip);
01813                 matrix[0][1] =  cos(phip)*sin(azp) + cos(altp)*cos(azp)*sin(phip);
01814                 matrix[0][2] =  sin(altp)*sin(phip);
01815                 matrix[1][0] = -sin(phip)*cos(azp) - cos(altp)*sin(azp)*cos(phip);
01816                 matrix[1][1] = -sin(phip)*sin(azp) + cos(altp)*cos(azp)*cos(phip);
01817                 matrix[1][2] =  sin(altp)*cos(phip);
01818                 matrix[2][0] =  sin(altp)*sin(azp);
01819                 matrix[2][1] = -sin(altp)*cos(azp);
01820                 matrix[2][2] =  cos(altp);
01821         }
01822         if (is_quaternion){
01823                 matrix[0][0] = e0 * e0 + e1 * e1 - e2 * e2 - e3 * e3;
01824                 matrix[0][1] = 2.0f * (e1 * e2 + e0 * e3);
01825                 matrix[0][2] = 2.0f * (e1 * e3 - e0 * e2);
01826                 matrix[1][0] = 2.0f * (e2 * e1 - e0 * e3);
01827                 matrix[1][1] = e0 * e0 - e1 * e1 + e2 * e2 - e3 * e3;
01828                 matrix[1][2] = 2.0f * (e2 * e3 + e0 * e1);
01829                 matrix[2][0] = 2.0f * (e3 * e1 + e0 * e2);
01830                 matrix[2][1] = 2.0f * (e3 * e2 - e0 * e1);
01831                 matrix[2][2] = e0 * e0 - e1 * e1 - e2 * e2 + e3 * e3;
01832                 // keep in mind matrix[0][2] is M13 gives an e0 e2 piece, etc
01833         }
01834         // Now do post and pretrans: vfinal = vpost + R vpre;
01835 
01836         matrix[0][3] = postT[0] + matrix[0][0]*preT[0] + matrix[0][1]*preT[1] + matrix[0][2]*preT[2]  ;
01837         matrix[1][3] = postT[1] + matrix[1][0]*preT[0] + matrix[1][1]*preT[1] + matrix[1][2]*preT[2]  ;
01838         matrix[2][3] = postT[2] + matrix[2][0]*preT[0] + matrix[2][1]*preT[1] + matrix[2][2]*preT[2]  ;
01839 }
01840 
01841 
01842 void Transform3D::set_rotation(const float& m11, const float& m12, const float& m13,
01843                                                            const float& m21, const float& m22, const float& m23,
01844                   const float& m31, const float& m32, const float& m33)
01845 {
01846         EulerType euler_type = MATRIX;
01847         Dict rot;
01848         rot["m11"]  = m11;
01849         rot["m12"]  = m12;
01850         rot["m13"]  = m13;
01851         rot["m21"]  = m21;
01852         rot["m22"]  = m22;
01853         rot["m23"]  = m23;
01854         rot["m31"]  = m31;
01855         rot["m32"]  = m32;
01856         rot["m33"]  = m33;
01857         set_rotation(euler_type, rot);  // Or should it be &rot ?
01858 }
01859 
01860 void Transform3D::set_rotation(const Vec3f & eahat, const Vec3f & ebhat,
01861                                     const Vec3f & eAhat, const Vec3f & eBhat)
01862 {// this rotation rotates unit vectors a,b into A,B;
01863 //    The program assumes a dot b must equal A dot B
01864         Vec3f eahatcp(eahat);
01865         Vec3f ebhatcp(ebhat);
01866         Vec3f eAhatcp(eAhat);
01867         Vec3f eBhatcp(eBhat);
01868 
01869         eahatcp.normalize();
01870         ebhatcp.normalize();
01871         eAhatcp.normalize();
01872         eBhatcp.normalize();
01873 
01874         Vec3f aMinusA(eahatcp);
01875         aMinusA  -= eAhatcp;
01876         Vec3f bMinusB(ebhatcp);
01877         bMinusB  -= eBhatcp;
01878 
01879 
01880         Vec3f  nhat;
01881         float aAlength = aMinusA.length();
01882         float bBlength = bMinusB.length();
01883         if (aAlength==0){
01884                 nhat=eahatcp;
01885         }else if (bBlength==0){
01886                 nhat=ebhatcp;
01887         }else{
01888                 nhat= aMinusA.cross(bMinusB);
01889                 nhat.normalize();
01890         }
01891 
01892 //              printf("nhat=%f,%f,%f \n",nhat[0],nhat[1],nhat[2]);
01893 
01894         Vec3f neahat  = eahatcp.cross(nhat);
01895         Vec3f nebhat  = ebhatcp.cross(nhat);
01896         Vec3f neAhat  = eAhatcp.cross(nhat);
01897         Vec3f neBhat  = eBhatcp.cross(nhat);
01898 
01899         float cosOmegaA = (neahat.dot(neAhat))  / (neahat.dot(neahat));
01900 //      float cosOmegaB = (nebhat.dot(neBhat))  / (nebhat.dot(nebhat));
01901         float sinOmegaA = (neahat.dot(eAhatcp)) / (neahat.dot(neahat));
01902 //      printf("cosOmegaA=%f \n",cosOmegaA);    printf("sinOmegaA=%f \n",sinOmegaA);
01903 
01904         float OmegaA = atan2(sinOmegaA,cosOmegaA);
01905 //      printf("OmegaA=%f \n",OmegaA*180/M_PI);
01906 
01907         EulerType euler_type=SPIN;
01908         Dict rotation;
01909         rotation["n1"]= nhat[0];
01910         rotation["n2"]= nhat[1];
01911         rotation["n3"]= nhat[2];
01912         rotation["Omega"] =OmegaA*180.0/M_PI;
01913         set_rotation(euler_type,  rotation);
01914 }
01915 
01916 
01917 float Transform3D::get_scale() const     // YYY
01918 {
01919         // Assumes uniform scaling, calculation uses Z only.
01920         float scale =0;
01921         for (int i=0; i<3; i++) {
01922                 for (int j=0; j<3; j++) {
01923                         scale = scale + matrix[i][j]*matrix[i][j];
01924                 }
01925         }
01926 
01927         return sqrt(scale/3);
01928 }
01929 
01930 
01931 
01932 Dict Transform3D::get_rotation(EulerType euler_type) const
01933 {
01934         Dict result;
01935 
01936         float max = 1 - ERR_LIMIT;
01937         float sca=get_scale();
01938         float cosalt=matrix[2][2]/sca;
01939 
01940 
01941         float az=0;
01942         float alt = 0;
01943         float phi=0;
01944         float phiS = 0; // like az   (but in SPIDER ZXZ)
01945         float psiS =0;  // like phi  (but in SPIDER ZYZ)
01946 
01947 
01948 // get alt, az, phi;  EMAN
01949 
01950         if (cosalt > max) {  // that is, alt close to 0
01951                 alt = 0;
01952                 az=0;
01953                 phi = (float)EMConsts::rad2deg*(float)atan2(matrix[0][1], matrix[0][0]);
01954         }
01955         else if (cosalt < -max) { // alt close to pi
01956                 alt = 180;
01957                 az=0;
01958                 phi=360.0f-(float)EMConsts::rad2deg*(float)atan2(matrix[0][1], matrix[0][0]);
01959         }
01960         else {
01961                 alt = (float)EMConsts::rad2deg*(float) acos(cosalt);
01962                 az  = 360.0f+(float)EMConsts::rad2deg*(float)atan2(matrix[2][0], -matrix[2][1]);
01963                 phi = 360.0f+(float)EMConsts::rad2deg*(float)atan2(matrix[0][2], matrix[1][2]);
01964         }
01965         az=fmod(az+180.0f,360.0f)-180.0f;
01966         phi=fmod(phi+180.0f,360.0f)-180.0f;
01967 
01968 //   get phiS, psiS ; SPIDER
01969         if (fabs(cosalt) > max) {  // that is, alt close to 0
01970                 phiS=0;
01971                 psiS = az+phi;
01972         }
01973         else {
01974                 phiS = az   - 90.0f;
01975                 psiS = phi  + 90.0f;
01976         }
01977         phiS = fmod((phiS   + 360.0f ), 360.0f) ;
01978         psiS = fmod((psiS   + 360.0f ), 360.0f) ;
01979 
01980 //   do some quaternionic stuff here
01981 
01982         float nphi = (az-phi)/2.0f;
01983     // The next is also e0
01984         float cosOover2 = (cos((az+phi)*M_PI/360) * cos(alt*M_PI/360)) ;
01985         float sinOover2 = sqrt(1 -cosOover2*cosOover2);
01986         float cosnTheta = sin((az+phi)*M_PI/360) * cos(alt*M_PI/360) / sqrt(1-cosOover2*cosOover2) ;
01987         float sinnTheta = sqrt(1-cosnTheta*cosnTheta);
01988         float n1 = sinnTheta*cos(nphi*M_PI/180);
01989         float n2 = sinnTheta*sin(nphi*M_PI/180);
01990         float n3 = cosnTheta;
01991         float xtilt = 0;
01992         float ytilt = 0;
01993         float ztilt = 0;
01994 
01995 
01996         if (cosOover2<0) {
01997                 cosOover2*=-1; n1 *=-1; n2*=-1; n3*=-1;
01998         }
01999 
02000 
02001         switch (euler_type) {
02002         case EMAN:
02003                 result["az"]  = az;
02004                 result["alt"] = alt;
02005                 result["phi"] = phi;
02006                 break;
02007 
02008         case IMAGIC:
02009                 result["alpha"] = az;
02010                 result["beta"] = alt;
02011                 result["gamma"] = phi;
02012                 break;
02013 
02014         case SPIDER:
02015                 result["phi"]   = phiS;  // The first Euler like az
02016                 result["theta"] = alt;
02017                 result["psi"]   = psiS;
02018                 break;
02019 
02020         case MRC:
02021                 result["phi"]   = phiS;
02022                 result["theta"] = alt;
02023                 result["omega"] = psiS;
02024                 break;
02025 
02026         case XYZ:
02027                 xtilt = atan2(-sin((M_PI/180.0f)*phiS)*sin((M_PI/180.0f)*alt),cos((M_PI/180.0f)*alt));
02028                 ytilt = asin(  cos((M_PI/180.0f)*phiS)*sin((M_PI/180.0f)*alt));
02029                 ztilt = psiS*M_PI/180.0f - atan2(sin(xtilt), cos(xtilt) *sin(ytilt));
02030 
02031                 xtilt=fmod(xtilt*180/M_PI+540.0f,360.0f) -180.0f;
02032                 ztilt=fmod(ztilt*180/M_PI+540.0f,360.0f) -180.0f;
02033 
02034                 result["xtilt"]  = xtilt;
02035                 result["ytilt"]  = ytilt*180/M_PI;
02036                 result["ztilt"]  = ztilt;
02037                 break;
02038 
02039         case QUATERNION:
02040                 result["e0"] = cosOover2 ;
02041                 result["e1"] = sinOover2 * n1 ;
02042                 result["e2"] = sinOover2 * n2;
02043                 result["e3"] = sinOover2 * n3;
02044                 break;
02045 
02046         case SPIN:
02047                 result["Omega"] =360.0f* acos(cosOover2)/ M_PI ;
02048                 result["n1"] = n1;
02049                 result["n2"] = n2;
02050                 result["n3"] = n3;
02051                 break;
02052 
02053         case SGIROT:
02054                 result["q"] = 360.0f*acos(cosOover2)/M_PI ;
02055                 result["n1"] = n1;
02056                 result["n2"] = n2;
02057                 result["n3"] = n3;
02058                 break;
02059 
02060         case MATRIX:
02061                 result["m11"] = matrix[0][0] ;
02062                 result["m12"] = matrix[0][1] ;
02063                 result["m13"] = matrix[0][2] ;
02064                 result["m21"] = matrix[1][0] ;
02065                 result["m22"] = matrix[1][1] ;
02066                 result["m23"] = matrix[1][2] ;
02067                 result["m31"] = matrix[2][0] ;
02068                 result["m32"] = matrix[2][1] ;
02069                 result["m33"] = matrix[2][2] ;
02070                 break;
02071 
02072         default:
02073                 throw InvalidValueException(euler_type, "unknown Euler Type");
02074         }
02075 
02076         return result;
02077 }
02078 
02079 Transform3D Transform3D::inverseUsingAngs() const    //   YYN need to test it for sure
02080 {
02081         // First Find the scale
02082         EulerType eE=EMAN;
02083 
02084 
02085         float scale   = get_scale();
02086         Vec3f preT   = get_pretrans( ) ;
02087         Vec3f postT   = get_posttrans( ) ;
02088         Dict angs     = get_rotation(eE);
02089         Dict invAngs  ;
02090 
02091         invAngs["phi"]   = 180.0f - (float) angs["az"] ;
02092         invAngs["az"]    = 180.0f - (float) angs["phi"] ;
02093         invAngs["alt"]   = angs["alt"] ;
02094 
02095 //    The inverse result
02096 //
02097 //           Z_phi   X_alt     Z_az
02098 //                 is
02099 //       Z_{pi-az}   X_alt  Z_{pi-phi}
02100 //      The reason for the extra pi's, is because one would like to keep alt positive
02101 
02102         float inverseScale= 1/scale ;
02103 
02104         Transform3D invM;
02105 
02106         invM.set_rotation(EMAN, invAngs);
02107         invM.apply_scale(inverseScale);
02108         invM.set_pretrans(-postT );
02109         invM.set_posttrans(-preT );
02110 
02111 
02112         return invM;
02113 
02114 }
02115 
02116 Transform3D Transform3D::inverse() const    //   YYN need to test it for sure
02117 {
02118         // This assumes the matrix is 4 by 4 and the last row reads [0 0 0 1]
02119 
02120         float m00 = matrix[0][0]; float m01=matrix[0][1]; float m02=matrix[0][2];
02121         float m10 = matrix[1][0]; float m11=matrix[1][1]; float m12=matrix[1][2];
02122         float m20 = matrix[2][0]; float m21=matrix[2][1]; float m22=matrix[2][2];
02123         float v0  = matrix[0][3]; float v1 =matrix[1][3]; float v2 =matrix[2][3];
02124 
02125     float cof00 = m11*m22-m12*m21;
02126     float cof11 = m22*m00-m20*m02;
02127     float cof22 = m00*m11-m01*m10;
02128     float cof01 = m10*m22-m20*m12;
02129     float cof02 = m10*m21-m20*m11;
02130     float cof12 = m00*m21-m01*m20;
02131     float cof10 = m01*m22-m02*m21;
02132     float cof20 = m01*m12-m02*m11;
02133     float cof21 = m00*m12-m10*m02;
02134 
02135     float Det = m00* cof00 + m02* cof02 -m01*cof01;
02136 
02137     Transform3D invM;
02138 
02139     invM.matrix[0][0] =   cof00/Det;
02140     invM.matrix[0][1] = - cof10/Det;
02141     invM.matrix[0][2] =   cof20/Det;
02142     invM.matrix[1][0] = - cof01/Det;
02143     invM.matrix[1][1] =   cof11/Det;
02144     invM.matrix[1][2] = - cof21/Det;
02145     invM.matrix[2][0] =   cof02/Det;
02146     invM.matrix[2][1] = - cof12/Det;
02147     invM.matrix[2][2] =   cof22/Det;
02148 
02149     invM.matrix[0][3] =  (- cof00*v0 + cof10*v1 - cof20*v2 )/Det;
02150     invM.matrix[1][3] =  (  cof01*v0 - cof11*v1 + cof21*v2 )/Det;
02151     invM.matrix[2][3] =  (- cof02*v0 + cof12*v1 - cof22*v2 )/Det;
02152 
02153         Vec3f postT   = get_posttrans( ) ;
02154         Vec3f invMpre   = - postT;
02155         Vec3f invMpost   ;
02156         for ( int i = 0; i < 3; i++) {
02157                 invMpost[i] = invM.matrix[i][3];
02158                 for ( int j = 0; j < 3; j++) {
02159                         invMpost[i] += - invM.matrix[i][j]*invMpre[j];
02160                 }
02161                 invM.matrix[3][i] = invMpost[i];
02162         }
02163 
02164         return invM;
02165 }
02166 
02167 
02168 
02169 // Symmetry Stuff
02170 
02171 Transform3D Transform3D::get_sym(const string & symname, int n) const
02172 {
02173         int nsym = get_nsym(symname);
02174 
02175 //      Transform3D invalid;
02176 //      invalid.set_rotation( -0.1f, -0.1f, -0.1f);
02177 
02178         // see www.math.utah.edu/~alfeld/math/polyhedra/polyhedra.html for pictures
02179         // By default we will put largest symmetry along z-axis.
02180 
02181         // Each Platonic Solid has 2E symmetry elements.
02182 
02183 
02184         // An icosahedron has   m=5, n=3, F=20 E=30=nF/2, V=12=nF/m,since vertices shared by 5 triangles;
02185         // It is composed of 20 triangles. E=3*20/2;
02186 
02187 
02188         // An dodecahedron has m=3, n=5   F=12 E=30  V=20
02189         // It is composed of 12 pentagons. E=5*12/2;   V= 5*12/3, since vertices shared by 3 pentagons;
02190 
02191 
02192 
02193     // The ICOS symmetry group has the face along z-axis
02194 
02195         float lvl0=0;                             //  there is one pentagon on top; five-fold along z
02196         float lvl1= 63.4349f; // that is atan(2)  // there are 5 pentagons with centers at this height (angle)
02197         float lvl2=116.5651f; //that is 180-lvl1  // there are 5 pentagons with centers at this height (angle)
02198         float lvl3=180.f;                           // there is one pentagon on the bottom
02199              // Notice that 63.439 is the angle between two faces of the dual object
02200 
02201         static double ICOS[180] = { // This is with a pentagon normal to z
02202                   0,lvl0,0,    0,lvl0,288,   0,lvl0,216,   0,lvl0,144,  0,lvl0,72,
02203                   0,lvl1,36,   0,lvl1,324,   0,lvl1,252,   0,lvl1,180,  0,lvl1,108,
02204                  72,lvl1,36,  72,lvl1,324,  72,lvl1,252,  72,lvl1,180,  72,lvl1,108,
02205                 144,lvl1,36, 144,lvl1,324, 144,lvl1,252, 144,lvl1,180, 144,lvl1,108,
02206                 216,lvl1,36, 216,lvl1,324, 216,lvl1,252, 216,lvl1,180, 216,lvl1,108,
02207                 288,lvl1,36, 288,lvl1,324, 288,lvl1,252, 288,lvl1,180, 288,lvl1,108,
02208                  36,lvl2,0,   36,lvl2,288,  36,lvl2,216,  36,lvl2,144,  36,lvl2,72,
02209                 108,lvl2,0,  108,lvl2,288, 108,lvl2,216, 108,lvl2,144, 108,lvl2,72,
02210                 180,lvl2,0,  180,lvl2,288, 180,lvl2,216, 180,lvl2,144, 180,lvl2,72,
02211                 252,lvl2,0,  252,lvl2,288, 252,lvl2,216, 252,lvl2,144, 252,lvl2,72,
02212                 324,lvl2,0,  324,lvl2,288, 324,lvl2,216, 324,lvl2,144, 324,lvl2,72,
02213                   0,lvl3,0,    0,lvl3,288,   0,lvl3,216,   0,lvl3,144,   0,lvl3,72
02214         };
02215 
02216 
02217         // A cube has   m=3, n=4, F=6 E=12=nF/2, V=8=nF/m,since vertices shared by 3 squares;
02218         // It is composed of 6 squares.
02219 
02220 
02221         // An octahedron has   m=4, n=3, F=8 E=12=nF/2, V=6=nF/m,since vertices shared by 4 triangles;
02222         // It is composed of 8 triangles.
02223 
02224     // We have placed the OCT symmetry group with a face along the z-axis
02225         lvl0=0;
02226         lvl1=90;
02227         lvl2=180;
02228 
02229         static float OCT[72] = {// This is with a face of a cube along z
02230                       0,lvl0,0,   0,lvl0,90,    0,lvl0,180,    0,lvl0,270,
02231                       0,lvl1,0,   0,lvl1,90,    0,lvl1,180,    0,lvl1,270,
02232                      90,lvl1,0,  90,lvl1,90,   90,lvl1,180,   90,lvl1,270,
02233                     180,lvl1,0, 180,lvl1,90,  180,lvl1,180,  180,lvl1,270,
02234                     270,lvl1,0, 270,lvl1,90,  270,lvl1,180,  270,lvl1,270,
02235                       0,lvl2,0,   0,lvl2,90,    0,lvl2,180,    0,lvl2,270
02236         };
02237         // B^4=A^3=1;  BABA=1; implies   AA=BAB, ABA=B^3 , AB^2A = BBBABBB and
02238         //   20 words with at most a single A
02239     //   1 B BB BBB A  BA AB BBA BAB ABB BBBA BBAB BABB ABBB BBBAB BBABB BABBB
02240     //                        BBBABB BBABBB BBBABBB
02241      // also     ABBBA is distinct yields 4 more words
02242      //    ABBBA   BABBBA BBABBBA BBBABBBA
02243      // for a total of 24 words
02244      // Note A BBB A BBB A  reduces to BBABB
02245      //  and  B A BBB A is the same as A BBB A BBB etc.
02246 
02247     // The TET symmetry group has a face along the z-axis
02248     // It has n=m=3; F=4, E=6=nF/2, V=4=nF/m
02249         lvl0=0;         // There is a face along z
02250         lvl1=109.4712f;  //  that is acos(-1/3)  // There  are 3 faces at this angle
02251 
02252         static float TET[36] = {// This is with the face along z
02253               0,lvl0,0,   0,lvl0,120,    0,lvl0,240,
02254               0,lvl1,60,   0,lvl1,180,    0,lvl1,300,
02255             120,lvl1,60, 120,lvl1,180,  120,lvl1,300,
02256             240,lvl1,60, 240,lvl1,180,  240,lvl1,300
02257         };
02258         // B^3=A^3=1;  BABA=1; implies   A^2=BAB, ABA=B^2 , AB^2A = B^2AB^2 and
02259         //   12 words with at most a single A
02260     //   1 B BB  A  BA AB BBA BAB ABB BBAB BABB BBABB
02261     // at most one A is necessary
02262 
02263         Transform3D ret;
02264         SymType type = get_sym_type(symname);
02265 
02266         switch (type) {
02267         case CSYM:
02268                 ret.set_rotation( n * 360.0f / nsym, 0, 0);
02269                 break;
02270         case DSYM:
02271                 if (n >= nsym / 2) {
02272                         ret.set_rotation((n - nsym/2) * 360.0f / (nsym / 2),180.0f, 0);
02273                 }
02274                 else {
02275                         ret.set_rotation( n * 360.0f / (nsym / 2),0, 0);
02276                 }
02277                 break;
02278         case ICOS_SYM:
02279                 ret.set_rotation((float)ICOS[n * 3 ],
02280                                  (float)ICOS[n * 3 + 1],
02281                                  (float)ICOS[n * 3 + 2] );
02282                 break;
02283         case OCT_SYM:
02284                 ret.set_rotation((float)OCT[n * 3],
02285                                  (float)OCT[n * 3 + 1],
02286                                  (float)OCT[n * 3 + 2] );
02287                 break;
02288         case TET_SYM:
02289                 ret.set_rotation((float)TET[n * 3 ],
02290                                  (float)TET[n * 3 + 1] ,
02291                                  (float)TET[n * 3 + 2] );
02292                 break;
02293         case ISYM:
02294                 ret.set_rotation(0, 0, 0);
02295                 break;
02296         default:
02297                 throw InvalidValueException(type, symname);
02298         }
02299 
02300         ret = (*this) * ret;
02301 
02302         return ret;
02303 }
02304 
02305 int Transform3D::get_nsym(const string & name)
02306 {
02307         string symname = name;
02308 
02309         for (size_t i = 0; i < name.size(); i++) {
02310                 if (isalpha(name[i])) {
02311                         symname[i] = (char)tolower(name[i]);
02312                 }
02313         }
02314 
02315         SymType type = get_sym_type(symname);
02316         int nsym = 0;
02317 
02318         switch (type) {
02319         case CSYM:
02320                 nsym = atoi(symname.c_str() + 1);
02321                 break;
02322         case DSYM:
02323                 nsym = atoi(symname.c_str() + 1) * 2;
02324                 break;
02325         case ICOS_SYM:
02326                 nsym = 60;
02327                 break;
02328         case OCT_SYM:
02329                 nsym = 24;
02330                 break;
02331         case TET_SYM:
02332                 nsym = 12;
02333                 break;
02334         case ISYM:
02335                 nsym = 1;
02336                 break;
02337         case UNKNOWN_SYM:
02338         default:
02339                 throw InvalidValueException(type, name);
02340         }
02341         return nsym;
02342 }
02343 
02344 
02345 
02346 Transform3D::SymType Transform3D::get_sym_type(const string & name)
02347 {
02348         SymType t = UNKNOWN_SYM;
02349 
02350         if (name[0] == 'c') {
02351                 t = CSYM;
02352         }
02353         else if (name[0] == 'd') {
02354                 t = DSYM;
02355         }
02356         else if (name == "icos") {
02357                 t = ICOS_SYM;
02358         }
02359         else if (name == "oct") {
02360                 t = OCT_SYM;
02361         }
02362         else if (name == "tet") {
02363                 t = TET_SYM;
02364         }
02365         else if (name == "i" || name == "") {
02366                 t = ISYM;
02367         }
02368         return t;
02369 }
02370 
02371 vector<Transform3D*>
02372 Transform3D::angles2tfvec(EulerType eulertype, const vector<float> ang) {
02373         int nangles = ang.size() / 3;
02374         vector<Transform3D*> tfvec;
02375         for (int i = 0; i < nangles; i++) {
02376                 tfvec.push_back(new Transform3D(eulertype,ang[3*i],ang[3*i+1],ang[3*i+2]));
02377         }
02378         return tfvec;
02379 }
02380 
02381 
02382 
02383 
02384 /* vim: set ts=4 noet: */

Generated on Thu Dec 9 13:45:51 2010 for EMAN2 by  doxygen 1.3.9.1