00001
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "transform.h"
00037 #include "util.h"
00038 #include "emobject.h"
00039 #include <cctype>
00040 #include <cstring>
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>
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;
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;
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
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
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
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
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"));
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
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
00516 az = (float)rotation["az"] ;
00517 alt = (float)rotation["alt"] ;
00518 phi = (float)rotation["phi"] ;
00519 } else if ( type == "imagic" ) {
00520
00521 az = (float)rotation["alpha"] ;
00522 alt = (float)rotation["beta"] ;
00523 phi = (float)rotation["gamma"] ;
00524 } else if ( type == "spider" ) {
00525
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
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
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
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
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
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
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
00608 }
00609
00610
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
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
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]);
00646 float psi = atan2(v1[1],-v1[0]);
00647
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);
00680 ret.set_rotation(rot);
00681
00682 Vec3f trans = get_trans();
00683 trans[0] = -trans[0];
00684 ret.set_trans(trans);
00685
00686
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;
00724 float psiS =0;
00725
00726
00727 if (cosalt > max) {
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) {
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
00743
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
00754 if (fabs(cosalt) > max) {
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
00766
00767 float nphi = (az-phi)/2.0f;
00768
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
00793 result["az"] = az;
00794 result["alt"] = alt;
00795 result["phi"] = phi;
00796 } else if (type == "imagic") {
00797
00798 result["alpha"] = az;
00799 result["beta"] = alt;
00800 result["gamma"] = phi;
00801 } else if (type == "spider") {
00802
00803 result["phi"] = phiS;
00804 result["theta"] = alt;
00805 result["psi"] = psiS;
00806 } else if (type == "mrc") {
00807
00808 result["phi"] = phiS;
00809 result["theta"] = alt;
00810 result["omega"] = psiS;
00811 } else if (type == "xyz") {
00812
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
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
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
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
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
00862
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
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
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
00920 return soln.get_trans_2d();
00921 }
00922
00923
00924
00925
00926
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
00935
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);
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
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
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;
01041 else {
01042
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)
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()
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
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
01225 Transform3D::Transform3D(const float& az, const float& alt, const float& phi, const Vec3f& posttrans )
01226 {
01227 init();
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
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
01255 Transform3D::Transform3D(EulerType euler_type, const Dict& rotation)
01256 {
01257 init();
01258 set_rotation(euler_type,rotation);
01259 }
01260
01261
01262
01263
01264 Transform3D::Transform3D( const Vec3f& pretrans, const float& az, const float& alt, const float& phi, const Vec3f& posttrans )
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
01284
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()
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)
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
01324
01325 void Transform3D::init()
01326 {
01327 to_identity();
01328 }
01329
01330
01331
01332 void Transform3D::set_pretrans(const float& dx, const float& dy, const float& dz)
01333 { set_pretrans( Vec3f(dx,dy,dz)); }
01334
01335
01336 void Transform3D::set_pretrans(const float& dx, const float& dy)
01337 { set_pretrans( Vec3f(dx,dy,0)); }
01338
01339 void Transform3D::set_pretrans(const Vec2f& pretrans)
01340 { set_pretrans( Vec3f(pretrans[0],pretrans[1],0)); }
01341
01342 void Transform3D::set_pretrans(const Vec3f & preT)
01343 {
01344 int flag=0;
01345
01346
01347
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
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)
01363 { set_posttrans( Vec3f(dx,dy,dz)); }
01364
01365
01366 void Transform3D::set_posttrans(const float& dx, const float& dy)
01367 { set_posttrans( Vec3f(dx,dy,0)); }
01368
01369 void Transform3D::set_posttrans(const Vec2f& posttrans)
01370 { set_pretrans( Vec3f(posttrans[0],posttrans[1],0)); }
01371
01372 void Transform3D::set_posttrans(const Vec3f & posttrans)
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
01380
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
01387 if (flag==1) {
01388 }
01389 }
01390
01391
01392
01393
01394 void Transform3D::apply_scale(const float& scale)
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()
01407 {
01408
01409 float scale = get_scale() ;
01410 float inverseScale= 1/scale ;
01411 apply_scale(inverseScale);
01412
01413
01414 }
01415
01416
01417 void Transform3D::transpose()
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)
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
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
01469 {
01470
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
01495 {
01496 return Vec3f();
01497 }
01498
01499
01500
01501 Vec3f Transform3D::get_matrix3_col(int i) const
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
01508 {
01509 return Vec3f(matrix[i][0], matrix[i][1], matrix[i][2]);
01510 }
01511
01512 Vec3f Transform3D::transform(const Vec3f & v3f) const
01513 {
01514
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
01523 {
01524
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)
01533 {
01534
01535
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];
01542 }
01543
01544 for (int j=0; j<3; j++) {
01545 resultant[3][j] = M2[3][j];
01546 }
01547
01548 return resultant;
01549 }
01550
01551
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
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 }
01667 set_rotation(euler_type, rot);
01668 }
01669
01670
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 }
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 }
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
01821 }
01822
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);
01846 }
01847
01848 void Transform3D::set_rotation(const Vec3f & eahat, const Vec3f & ebhat,
01849 const Vec3f & eAhat, const Vec3f & eBhat)
01850 {
01851
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
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
01889 float sinOmegaA = (neahat.dot(eAhatcp)) / (neahat.dot(neahat));
01890
01891
01892 float OmegaA = atan2(sinOmegaA,cosOmegaA);
01893
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
01906 {
01907
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;
01933 float psiS =0;
01934
01935
01936
01937
01938 if (cosalt > max) {
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) {
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
01957 if (fabs(cosalt) > max) {
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
01969
01970 float nphi = (az-phi)/2.0f;
01971
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;
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
02068 {
02069
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
02084
02085
02086
02087
02088
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
02105 {
02106
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
02158
02159 Transform3D Transform3D::get_sym(const string & symname, int n) const
02160 {
02161 int nsym = get_nsym(symname);
02162
02163
02164
02165
02166
02167
02168
02169
02170
02171
02172
02173
02174
02175
02176
02177
02178
02179
02180
02181
02182
02183 float lvl0=0;
02184 float lvl1= 63.4349f;
02185 float lvl2=116.5651f;
02186 float lvl3=180.f;
02187
02188
02189 static double ICOS[180] = {
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
02206
02207
02208
02209
02210
02211
02212
02213 lvl0=0;
02214 lvl1=90;
02215 lvl2=180;
02216
02217 static float OCT[72] = {
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
02226
02227
02228
02229
02230
02231
02232
02233
02234
02235
02236
02237 lvl0=0;
02238 lvl1=109.4712f;
02239
02240 static float TET[36] = {
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
02247
02248
02249
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