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
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
00728 if (cosalt >= 1) {
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 {
00737
00738
00739
00740
00741
00742
00743
00744
00745 az = 360.0f+(float)EMConsts::rad2deg*(float)atan2(scale*matrix[2][0], -scale*matrix[2][1]);
00746
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 }
00752
00753
00754
00755
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
00766 if (fabs(cosalt) > 1) {
00767 phiS=0;
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
00777
00778 float nphi = (az-phi)/2.0f;
00779
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
00804 result["az"] = az;
00805 result["alt"] = alt;
00806 result["phi"] = phi;
00807 } else if (type == "imagic") {
00808
00809 result["alpha"] = az;
00810 result["beta"] = alt;
00811 result["gamma"] = phi;
00812 } else if (type == "spider") {
00813
00814 result["phi"] = phiS;
00815 result["theta"] = alt;
00816 result["psi"] = psiS;
00817 } else if (type == "mrc") {
00818
00819 result["phi"] = phiS;
00820 result["theta"] = alt;
00821 result["omega"] = psiS;
00822 } else if (type == "xyz") {
00823
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
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
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
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
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
00873
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
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
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
00931 return soln.get_trans_2d();
00932 }
00933
00934
00935
00936
00937
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
00946
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);
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
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
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;
01052 else {
01053
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)
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()
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
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
01237 Transform3D::Transform3D(const float& az, const float& alt, const float& phi, const Vec3f& posttrans )
01238 {
01239 init();
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
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
01267 Transform3D::Transform3D(EulerType euler_type, const Dict& rotation)
01268 {
01269 init();
01270 set_rotation(euler_type,rotation);
01271 }
01272
01273
01274
01275
01276 Transform3D::Transform3D( const Vec3f& pretrans, const float& az, const float& alt, const float& phi, const Vec3f& posttrans )
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
01296
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()
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)
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
01336
01337 void Transform3D::init()
01338 {
01339 to_identity();
01340 }
01341
01342
01343
01344 void Transform3D::set_pretrans(const float& dx, const float& dy, const float& dz)
01345 { set_pretrans( Vec3f(dx,dy,dz)); }
01346
01347
01348 void Transform3D::set_pretrans(const float& dx, const float& dy)
01349 { set_pretrans( Vec3f(dx,dy,0)); }
01350
01351 void Transform3D::set_pretrans(const Vec2f& pretrans)
01352 { set_pretrans( Vec3f(pretrans[0],pretrans[1],0)); }
01353
01354 void Transform3D::set_pretrans(const Vec3f & preT)
01355 {
01356 int flag=0;
01357
01358
01359
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
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)
01375 { set_posttrans( Vec3f(dx,dy,dz)); }
01376
01377
01378 void Transform3D::set_posttrans(const float& dx, const float& dy)
01379 { set_posttrans( Vec3f(dx,dy,0)); }
01380
01381 void Transform3D::set_posttrans(const Vec2f& posttrans)
01382 { set_pretrans( Vec3f(posttrans[0],posttrans[1],0)); }
01383
01384 void Transform3D::set_posttrans(const Vec3f & posttrans)
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
01392
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
01399 if (flag==1) {
01400 }
01401 }
01402
01403
01404
01405
01406 void Transform3D::apply_scale(const float& scale)
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()
01419 {
01420
01421 float scale = get_scale() ;
01422 float inverseScale= 1/scale ;
01423 apply_scale(inverseScale);
01424
01425
01426 }
01427
01428
01429 void Transform3D::transpose()
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)
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
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
01481 {
01482
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
01507 {
01508 return Vec3f();
01509 }
01510
01511
01512
01513 Vec3f Transform3D::get_matrix3_col(int i) const
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
01520 {
01521 return Vec3f(matrix[i][0], matrix[i][1], matrix[i][2]);
01522 }
01523
01524 Vec3f Transform3D::transform(const Vec3f & v3f) const
01525 {
01526
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
01535 {
01536
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)
01545 {
01546
01547
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];
01554 }
01555
01556 for (int j=0; j<3; j++) {
01557 resultant[3][j] = M2[3][j];
01558 }
01559
01560 return resultant;
01561 }
01562
01563
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
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 }
01679 set_rotation(euler_type, rot);
01680 }
01681
01682
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 }
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 }
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
01833 }
01834
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);
01858 }
01859
01860 void Transform3D::set_rotation(const Vec3f & eahat, const Vec3f & ebhat,
01861 const Vec3f & eAhat, const Vec3f & eBhat)
01862 {
01863
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
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
01901 float sinOmegaA = (neahat.dot(eAhatcp)) / (neahat.dot(neahat));
01902
01903
01904 float OmegaA = atan2(sinOmegaA,cosOmegaA);
01905
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
01918 {
01919
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;
01945 float psiS =0;
01946
01947
01948
01949
01950 if (cosalt > max) {
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) {
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
01969 if (fabs(cosalt) > max) {
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
01981
01982 float nphi = (az-phi)/2.0f;
01983
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;
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
02080 {
02081
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
02096
02097
02098
02099
02100
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
02117 {
02118
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
02170
02171 Transform3D Transform3D::get_sym(const string & symname, int n) const
02172 {
02173 int nsym = get_nsym(symname);
02174
02175
02176
02177
02178
02179
02180
02181
02182
02183
02184
02185
02186
02187
02188
02189
02190
02191
02192
02193
02194
02195 float lvl0=0;
02196 float lvl1= 63.4349f;
02197 float lvl2=116.5651f;
02198 float lvl3=180.f;
02199
02200
02201 static double ICOS[180] = {
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
02218
02219
02220
02221
02222
02223
02224
02225 lvl0=0;
02226 lvl1=90;
02227 lvl2=180;
02228
02229 static float OCT[72] = {
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
02238
02239
02240
02241
02242
02243
02244
02245
02246
02247
02248
02249 lvl0=0;
02250 lvl1=109.4712f;
02251
02252 static float TET[36] = {
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
02259
02260
02261
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