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 "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
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
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
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
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
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 }