00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "stdafx.h"
00027 #include "quaternion.h"
00028 #include "transform.h"
00029
00030
00031 Transform Quaternion::ToTransform() const {
00032 float xx = v.x * v.x, yy = v.y * v.y, zz = v.z * v.z;
00033 float xy = v.x * v.y, xz = v.x * v.z, yz = v.y * v.z;
00034 float wx = v.x * w, wy = v.y * w, wz = v.z * w;
00035
00036 Matrix4x4 m;
00037 m.m[0][0] = 1.f - 2.f * (yy + zz);
00038 m.m[0][1] = 2.f * (xy + wz);
00039 m.m[0][2] = 2.f * (xz - wy);
00040 m.m[1][0] = 2.f * (xy - wz);
00041 m.m[1][1] = 1.f - 2.f * (xx + zz);
00042 m.m[1][2] = 2.f * (yz + wx);
00043 m.m[2][0] = 2.f * (xz + wy);
00044 m.m[2][1] = 2.f * (yz - wx);
00045 m.m[2][2] = 1.f - 2.f * (xx + yy);
00046
00047
00048 return Transform(Transpose(m), m);
00049 }
00050
00051
00052 Quaternion::Quaternion(const Transform &t) {
00053 const Matrix4x4 &m = t.m;
00054 float trace = m.m[0][0] + m.m[1][1] + m.m[2][2];
00055 if (trace > 0.f) {
00056
00057
00058 float s = sqrtf(trace + 1.0);
00059 w = s / 2.0f;
00060 s = 0.5f / s;
00061 v.x = (m.m[2][1] - m.m[1][2]) * s;
00062 v.y = (m.m[0][2] - m.m[2][0]) * s;
00063 v.z = (m.m[1][0] - m.m[0][1]) * s;
00064 }
00065 else {
00066
00067 const int nxt[3] = {1, 2, 0};
00068 float q[3];
00069 int i = 0;
00070 if (m.m[1][1] > m.m[0][0]) i = 1;
00071 if (m.m[2][2] > m.m[i][i]) i = 2;
00072 int j = nxt[i];
00073 int k = nxt[j];
00074 float s = sqrtf((m.m[i][i] - (m.m[j][j] + m.m[k][k])) + 1.0);
00075 q[i] = s * 0.5f;
00076 if (s != 0.f) s = 0.5f / s;
00077 w = (m.m[k][j] - m.m[j][k]) * s;
00078 q[j] = (m.m[j][i] + m.m[i][j]) * s;
00079 q[k] = (m.m[k][i] + m.m[i][k]) * s;
00080 v.x = q[0];
00081 v.y = q[1];
00082 v.z = q[2];
00083 }
00084 }
00085
00086
00087 Quaternion Slerp(float t, const Quaternion &q1,
00088 const Quaternion &q2) {
00089 float cosTheta = Dot(q1, q2);
00090 if (cosTheta > .9995f)
00091 return Normalize((1.f - t) * q1 + t * q2);
00092 else {
00093 float theta = acosf(Clamp(cosTheta, -1.f, 1.f));
00094 float thetap = theta * t;
00095 Quaternion qperp = Normalize(q2 - q1 * cosTheta);
00096 return q1 * cosf(thetap) + qperp * sinf(thetap);
00097 }
00098 }
00099
00100