EMAN2
quaternion.cpp
Go to the documentation of this file.
00001 
00005 /*
00006  * Author: Steven Ludtke, 04/10/2003 (sludtke@bcm.edu)
00007  * Copyright (c) 2000-2006 Baylor College of Medicine
00008  * 
00009  * This software is issued under a joint BSD/GNU license. You may use the
00010  * source code in this file under either license. However, note that the
00011  * complete EMAN2 and SPARX software packages have some GPL dependencies,
00012  * so you are responsible for compliance with the licenses of these packages
00013  * if you opt to use BSD licensing. The warranty disclaimer below holds
00014  * in either instance.
00015  * 
00016  * This complete copyright notice must be included in any revised version of the
00017  * source code. Additional authorship citations may be added, but existing
00018  * author citations must be preserved.
00019  * 
00020  * This program is free software; you can redistribute it and/or modify
00021  * it under the terms of the GNU General Public License as published by
00022  * the Free Software Foundation; either version 2 of the License, or
00023  * (at your option) any later version.
00024  * 
00025  * This program is distributed in the hope that it will be useful,
00026  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00028  * GNU General Public License for more details.
00029  * 
00030  * You should have received a copy of the GNU General Public License
00031  * along with this program; if not, write to the Free Software
00032  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033  * 
00034  * */
00035 
00036 #include "quaternion.h"
00037 
00038 using namespace EMAN;
00039 
00040 Quaternion::Quaternion()
00041 :       e0(0), e1(0), e2(0), e3(0)
00042 {
00043 }
00044 
00045 Quaternion::Quaternion(float radians, const Vec3f &axis)
00046 {
00047         Vec3f q = axis;
00048         //normalize();
00049 
00050         q *= sin(radians / 2.0f);
00051         e0 = cos(radians / 2.0f);
00052 
00053         e1 = q[0];
00054         e2 = q[1];
00055         e3 = q[2];
00056 }
00057 
00058 Quaternion::Quaternion(const Vec3f &axis, float radians)
00059 {
00060         Vec3f q = axis;
00061         //normalize();
00062 
00063         q *= sin(radians / 2.0f);
00064         e0 = cos(radians / 2.0f);
00065 
00066         e1 = q[0];
00067         e2 = q[1];
00068         e3 = q[2];
00069 }
00070 
00071 Quaternion::Quaternion(float ee0, float ee1, float ee2, float ee3)
00072 :e0(ee0), e1(ee1), e2(ee2), e3(ee3)
00073 {
00074         //normalize();
00075 }
00076 
00077 Quaternion::Quaternion(const vector<float> & m)
00078 {
00079         int i = 0;
00080 
00081         if (m[0] > m[4]) {
00082                 if (m[0] > m[8]) {
00083                         i = 0;
00084                 }
00085                 else {
00086                         i = 2;
00087                 }
00088         }
00089         else {
00090                 if (m[4] > m[8]) {
00091                         i = 1;
00092                 }
00093                 else {
00094                         i = 2;
00095                 }
00096         }
00097 
00098         if (m[0] + m[4] + m[8] > m[i*3+i]) {
00099                 e0 = (float) (sqrt(m[0] + m[4] + m[8] + 1) / 2.0);
00100                 e1 = (float) ((m[5] - m[7]) / (4 * e0));
00101                 e2 = (float) ((m[6] - m[2]) / (4 * e0));
00102                 e3 = (float) ((m[1] - m[3]) / (4 * e0));
00103         }
00104         else {
00105                 float quat[3];
00106                 int j = (i + 1) % 3;
00107                 int k = (i + 2) % 3;
00108 
00109                 quat[i] = (float) (sqrt(m[i*3+i] - m[j*3+j] - m[k*3+k] + 1) / 2.0);
00110                 quat[j] = (float) ((m[i*3+j] + m[j*3+i]) / (4 * quat[i]));
00111                 quat[k] = (float) ((m[i*3+k] + m[k*3+i]) / (4 * quat[i]));
00112 
00113                 e0 = (float) ((m[j*3+k] - m[k*3+j]) / (4 * quat[i]));
00114                 e1 = quat[0];
00115                 e2 = quat[1];
00116                 e3 = quat[2];
00117         }
00118 
00119         //normalize();
00120 }
00121 
00122 
00123 void Quaternion::normalize()
00124 {
00125         float dist = 1.0f / sqrt(norm());
00126         e0 *= dist;
00127         e1 *= dist;
00128         e2 *= dist;
00129         e3 *= dist;
00130 }
00131 
00132 Quaternion & Quaternion::inverse()
00133 {
00134         float f = 1.0f / norm();
00135         e0 *= f;
00136         e1 *= -f;
00137         e2 *= -f;
00138         e3 *= -f;
00139         return (*this);
00140 }
00141 
00142 Quaternion Quaternion::create_inverse() const
00143 {
00144         Quaternion q = *this;
00145         return q.inverse();
00146 }
00147 
00148 
00149 Vec3f Quaternion::rotate(const Vec3f &v) const
00150 {
00151         Vec3f i(e1, e2, e3);
00152         Vec3f v1 = i.cross(v) * (2 * e0);
00153         Vec3f v2 = v1.cross(i) * (float) 2;
00154         Vec3f rotated = v + v1 - v2;
00155 
00156         return rotated;
00157 }
00158 
00159 
00160 float Quaternion::to_angle() const
00161 {
00162         Vec3f q(e1, e2, e3);
00163         float len = q.length();
00164         float radians = 0;
00165 
00166         if (len > 0.00001f) {
00167                 radians = 2.0f * acos(e0);
00168         }
00169         else {
00170                 radians = 0;
00171         }
00172         return radians;
00173 }
00174 
00175 Vec3f Quaternion::to_axis() const
00176 {
00177         Vec3f q(e1, e2, e3);
00178         float len = q.length();
00179         Vec3f axis;
00180 
00181         if (len > 0.00001f) {
00182                 axis = q * ((float) (1.0f / len));
00183         }
00184         else {
00185                 axis.set_value(0.0f, 0.0f, 1.0f);
00186         }
00187         return axis;
00188 }
00189 
00190 
00191 vector<float> Quaternion::to_matrix3() const
00192 {
00193         vector < float >m(9);
00194 
00195         m[0] = e0 * e0 + e1 * e1 - e2 * e2 - e3 * e3;
00196         m[1] = 2.0f * (e1 * e2 + e0 * e3);
00197         m[2] = 2.0f * (e1 * e3 - e0 * e2);
00198 
00199         m[3] = 2.0f * (e1 * e2 - e0 * e3);
00200         m[4] = e0 * e0 + e1 * e1 + e2 * e2 - e3 * e3;
00201         m[5] = 2.0f * (e2 * e3 + e0 * e1);
00202 
00203         m[6] = 2.0f * (e1 * e3 + e0 * e2);
00204         m[7] = 2.0f * (e2 * e3 - e0 * e1);
00205         m[8] = e0 * e0 - e1 * e1 - e2 * e2 + e3 * e3;
00206 
00207         return m;
00208 }
00209 
00210 
00211 
00212 float Quaternion::real() const
00213 {
00214         return e0;
00215 }
00216 
00217 Vec3f Quaternion::unreal() const
00218 {
00219         return Vec3f(e1, e2, e3);
00220 }
00221 
00222 vector < float >Quaternion::as_list() const
00223 {
00224         vector < float >v(4);
00225         v[0] = e0;
00226         v[1] = e1;
00227         v[2] = e2;
00228         v[3] = e3;
00229 
00230         return v;
00231 }
00232 
00233 Quaternion & Quaternion::operator+=(const Quaternion & q)
00234 {
00235         e0 += q.e0;
00236         e1 += q.e1;
00237         e2 += q.e2;
00238         e3 += q.e3;
00239         return *this;
00240 }
00241 
00242 Quaternion & Quaternion::operator-=(const Quaternion & q)
00243 {
00244         e0 -= q.e0;
00245         e1 -= q.e1;
00246         e2 -= q.e2;
00247         e3 -= q.e3;
00248         return *this;
00249 }
00250 
00251 Quaternion & Quaternion::operator*=(const Quaternion & q)
00252 {
00253         float a = e0 * q.e0 - e1 * q.e1 - e2 * q.e2 - e3 * q.e3;
00254         float b = e0 * q.e1 + e1 * q.e0 + e2 * q.e3 - e3 * q.e2;
00255         float c = e0 * q.e2 - e1 * q.e3 + e2 * q.e0 + e3 * q.e1;
00256         float d = e0 * q.e3 + e1 * q.e2 - e2 * q.e1 + e3 * q.e0;
00257 
00258         e0 = a;
00259         e1 = b;
00260         e2 = c;
00261         e3 = d;
00262 
00263 
00264         // normalize();
00265 
00266         return (*this);
00267 }
00268 
00269 Quaternion & Quaternion::operator*=(float s)
00270 {
00271         e0 *= s;
00272         e1 *= s;
00273         e2 *= s;
00274         e3 *= s;
00275         return (*this);
00276 }
00277 
00278 Quaternion & Quaternion::operator/=(const Quaternion & q)
00279 {
00280         float qn = q.norm();
00281 
00282         float a = (+e0 * q.e0 + e1 * q.e1 + e2 * q.e2 + e3 * q.e3) / qn;
00283         float b = (-e0 * q.e1 + e1 * q.e0 - e2 * q.e3 + e3 * q.e2) / qn;
00284         float c = (-e0 * q.e2 + e1 * q.e3 + e2 * q.e0 - e3 * q.e1) / qn;
00285         float d = (-e0 * q.e3 - e1 * q.e2 + e2 * q.e1 + e3 * q.e0) / qn;
00286 
00287         e0 = a;
00288         e1 = b;
00289         e2 = c;
00290         e3 = d;
00291 
00292         return (*this);
00293 }
00294 
00295 Quaternion & Quaternion::operator/=(float s)
00296 {
00297         if (s != 0) {
00298                 e0 /= s;
00299                 e1 /= s;
00300                 e2 /= s;
00301                 e3 /= s;
00302         }
00303 
00304         return (*this);
00305 }
00306 
00307 
00308 Quaternion EMAN::operator+(const Quaternion & q1, const Quaternion & q2)
00309 {
00310         Quaternion q = q1;
00311         q += q2;
00312         return q;
00313 }
00314 
00315 Quaternion EMAN::operator-(const Quaternion & q1, const Quaternion & q2)
00316 {
00317         Quaternion q = q1;
00318         q -= q2;
00319         return q;
00320 }
00321 
00322 
00323 Quaternion EMAN::operator*(const Quaternion & q1, const Quaternion & q2)
00324 {
00325         Quaternion q = q1;
00326         q *= q2;
00327         return q;
00328 }
00329 
00330 Quaternion EMAN::operator*(const Quaternion & q, float s)
00331 {
00332         Quaternion q1 = q;
00333         q1 *= s;
00334         return q1;
00335 }
00336 
00337 Quaternion EMAN::operator*(float s, const Quaternion & q)
00338 {
00339         Quaternion q1 = q;
00340         q1 *= s;
00341         return q1;
00342 }
00343 
00344 Quaternion EMAN::operator/(const Quaternion & q1, const Quaternion & q2)
00345 {
00346         Quaternion q = q1;
00347         q /= q2;
00348         return q;
00349 }
00350 
00351 
00352 bool EMAN::operator==(const Quaternion & q1, const Quaternion & q2)
00353 {
00354         bool result = true;
00355         const float err_limit = 0.00001f;
00356         
00357         vector < float >v1 = q1.as_list();
00358         vector < float >v2 = q2.as_list();
00359 
00360         for (size_t i = 0; i < v1.size(); i++) {
00361                 if (fabs(v1[i] - v2[i]) > err_limit) {
00362                         result = false;
00363                         break;
00364                 }
00365         }
00366 
00367         return result;
00368 }
00369 
00370 bool EMAN::operator!=(const Quaternion & q1, const Quaternion & q2)
00371 {
00372         return (!(q1 == q2));
00373 }
00374 
00375 
00376 Quaternion Quaternion::interpolate(const Quaternion & from,
00377                                                                    const Quaternion & to, float t)
00378 {
00379         const double epsilon = 0.00001;
00380         double cosom = from.e1 * to.e1 + from.e2 * to.e2 + from.e3 * to.e3 + from.e0 * to.e0;
00381 
00382         Quaternion q;
00383         if (cosom < 0) {
00384                 cosom = -cosom;
00385                 q = q - to;
00386         }
00387         else {
00388                 q = to;
00389         }
00390 
00391         double scale0 = 1 - t;
00392         double scale1 = t;
00393 
00394         if ((1 - cosom) > epsilon) {
00395                 double omega = acos(cosom);
00396                 double sinom = sin(omega);
00397                 scale0 = sin((1 - t) * omega) / sinom;
00398                 scale1 = sin(t * omega) / sinom;
00399         }
00400 
00401         float scale0f = (float) scale0;
00402         float scale1f = (float) scale1;
00403         
00404         return (scale0f * from + scale1f * q);
00405 }