00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "quaternion.h"
00025 #include "geometry/matrix4x4.h"
00026
00027 namespace lux
00028 {
00029
00030 void orthoNormalize(float m[4][4])
00031 {
00032 float len, temp[3][3];
00033 for (int i = 0; i <3; i++)
00034 for (int j = 0; j < 3; j++) {
00035 temp[i][j] = m[i][j];
00036 }
00037
00038
00039 len = sqrtf (temp[0][0]*temp[0][0] + temp[0][1]*temp[0][1] + temp[0][2]*temp[0][2]);
00040 len = (len == 0.0f) ? 1.0f : 1.0f/len;
00041 temp[0][0] *= len; temp[0][1] *= len; temp[0][2] *= len;
00042
00043
00044 temp[2][0] = (temp[0][1]*temp[1][2] - temp[0][2]*temp[1][1]);
00045 temp[2][1] = (temp[0][2]*temp[1][0] - temp[0][0]*temp[1][2]);
00046 temp[2][2] = (temp[0][0]*temp[1][1] - temp[0][1]*temp[1][0]);
00047
00048
00049 len = sqrtf (temp[2][0]*temp[2][0] + temp[2][1]*temp[2][1] + temp[2][2]*temp[2][2]);
00050 len = (len == 0.0f) ? 1.0f : 1.0f/len;
00051 temp[2][0] *= len; temp[2][1] *= len; temp[2][2] *= len;
00052
00053
00054 temp[1][0] = (temp[2][1]*temp[0][2] - temp[2][2]*temp[0][1]);
00055 temp[1][1] = (temp[2][2]*temp[0][0] - temp[2][0]*temp[0][2]);
00056 temp[1][2] = (temp[2][0]*temp[0][1] - temp[2][1]*temp[0][0]);
00057
00058
00059 len = sqrtf (temp[1][0]*temp[1][0] + temp[1][1]*temp[1][1] + temp[1][2]*temp[1][2]);
00060 len = (len == 0.0f) ? 1.0f : 1.0f/len;
00061 temp[1][0] *= len; temp[1][1] *= len; temp[1][2] *= len;
00062
00063
00064 m[0][0] = temp[0][0]; m[0][1] = temp[0][1]; m[0][2] = temp[0][2];
00065 m[1][0] = temp[1][0]; m[1][1] = temp[1][1]; m[1][2] = temp[1][2];
00066 m[2][0] = temp[2][0]; m[2][1] = temp[2][1]; m[2][2] = temp[2][2];
00067 }
00068
00069
00070 Quaternion::Quaternion(const boost::shared_ptr<Matrix4x4> m) {
00071 float ortho[4][4];
00072 memcpy(ortho, m->m, sizeof(float) * 16);
00073 orthoNormalize(ortho);
00074
00075 float trace = ortho[0][0] + ortho[1][1] + ortho[2][2] + 1.0;
00076
00077 if (trace > 1e-6) {
00078 float s = sqrtf(trace) * 2;
00079 v.x = ( ortho[1][2] - ortho[2][1] ) / s;
00080 v.y = ( ortho[2][0] - ortho[0][2] ) / s;
00081 v.z = ( ortho[0][1] - ortho[1][0] ) / s;
00082 w = 0.25 * s;
00083 } else {
00084 if ( ortho[0][0] > ortho[1][1] && ortho[0][0] > ortho[2][2] ) {
00085 float s = sqrtf( 1.0 + ortho[0][0] - ortho[1][1] - ortho[2][2] ) * 2;
00086 v.x = 0.25 * s;
00087 v.y = (ortho[0][1] + ortho[1][0] ) / s;
00088 v.z = (ortho[2][0] + ortho[0][2] ) / s;
00089 w = (ortho[1][2] - ortho[2][1] ) / s;
00090 } else if ( ortho[1][1] > ortho[2][2] ) {
00091 float s = sqrtf( 1.0 + ortho[1][1] - ortho[0][0] - ortho[2][2] ) * 2;
00092 v.x = (ortho[0][1] + ortho[1][0] ) / s;
00093 v.y = 0.25 * s;
00094 v.z = (ortho[1][2] + ortho[2][1] ) / s;
00095 w = (ortho[2][0] - ortho[0][2] ) / s;
00096 } else {
00097 float s = sqrtf( 1.0 + ortho[2][2] - ortho[0][0] - ortho[1][1] ) * 2;
00098 v.x = (ortho[2][0] + ortho[0][2] ) / s;
00099 v.y = (ortho[1][2] + ortho[2][1] ) / s;
00100 v.z = 0.25 * s;
00101 w = (ortho[0][1] - ortho[1][0] ) / s;
00102 }
00103 }
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140 }
00141
00142 Quaternion::Quaternion(const Quaternion &q) {
00143 w = q.w;
00144 v = q.v;
00145 }
00146
00147 Quaternion::Quaternion() {
00148 w = 1.0;
00149 }
00150
00151 Quaternion Quaternion::Slerp(float t, const Quaternion &q1, const Quaternion &q2) {
00152
00153 float cos_phi = Dot(q1, q2);
00154 float sign = (cos_phi > 0) ? 1 : -1;
00155
00156 cos_phi *= sign;
00157
00158 float f1, f2;
00159 if (1 - cos_phi > 1e-6) {
00160
00161 float phi = acosf(cos_phi);
00162 float sin_phi = sinf(phi);
00163 f1 = sin((1-t)*phi) / sin_phi;
00164 f2 = sin(t*phi) / sin_phi;
00165 } else {
00166
00167
00168 f1 = 1-t;
00169 f2 = t;
00170 }
00171
00172 return f1 * q1 + (sign*f2) * q2;
00173 }
00174
00175
00176 void Quaternion::ToMatrix(float m[4][4]) const {
00177 const float xx = v.x * v.x;
00178 const float yy = v.y * v.y;
00179 const float zz = v.z * v.z;
00180 const float xy = v.x * v.y;
00181 const float xz = v.x * v.z;
00182 const float yz = v.y * v.z;
00183 const float xw = v.x * w;
00184 const float yw = v.y * w;
00185 const float zw = v.z * w;
00186
00187 m[0][0] = 1 - 2 * ( yy + zz );
00188 m[1][0] = 2 * ( xy - zw );
00189 m[2][0] = 2 * ( xz + yw );
00190 m[0][1] = 2 * ( xy + zw );
00191 m[1][1] = 1 - 2 * ( xx + zz );
00192 m[2][1] = 2 * ( yz - xw );
00193 m[0][2] = 2 * ( xz - yw );
00194 m[1][2] = 2 * ( yz + xw );
00195 m[2][2] = 1 - 2 * ( xx + yy );
00196
00197
00198 m[0][3] = m[1][3] = m[2][3] = 0;
00199 m[3][0] = m[3][1] = m[3][2] = 0;
00200 m[3][3] = 1;
00201 }
00202
00203 float Dot(const Quaternion &q1, const Quaternion &q2) {
00204 return q1.w * q2.w + Dot(q1.v, q2.v);
00205 }
00206
00207 }
00208