00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #ifndef CAL_QUATERNION_H
00012 #define CAL_QUATERNION_H
00013
00014
00015
00016
00017
00018 #include "cal3d/global.h"
00019 #include "cal3d/vector.h"
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00035 class CAL3D_API CalQuaternion
00036 {
00037
00038 public:
00039 float x;
00040 float y;
00041 float z;
00042 float w;
00043
00044
00045 public:
00046 inline CalQuaternion() : x(0.0f), y(0.0f), z(0.0f), w(1.0f){};
00047 inline CalQuaternion(const CalQuaternion& q): x(q.x), y(q.y), z(q.z), w(q.w) {};
00048 inline CalQuaternion(float qx, float qy, float qz, float qw): x(qx), y(qy), z(qz), w(qw) {};
00049 inline ~CalQuaternion() {};
00050
00051
00052 public:
00053 inline float& operator[](unsigned int index)
00054 {
00055 return (&x)[index];
00056 }
00057
00058 inline const float& operator[](unsigned int index) const
00059 {
00060 return (&x)[index];
00061 }
00062
00063 inline void operator=(const CalQuaternion& q)
00064 {
00065 x = q.x;
00066 y = q.y;
00067 z = q.z;
00068 w = q.w;
00069 }
00070
00071 inline void operator*=(const CalQuaternion& q)
00072 {
00073 float qx, qy, qz, qw;
00074 qx = x;
00075 qy = y;
00076 qz = z;
00077 qw = w;
00078
00079 x = qw * q.x + qx * q.w + qy * q.z - qz * q.y;
00080 y = qw * q.y - qx * q.z + qy * q.w + qz * q.x;
00081 z = qw * q.z + qx * q.y - qy * q.x + qz * q.w;
00082 w = qw * q.w - qx * q.x - qy * q.y - qz * q.z;
00083 }
00084
00085 inline void operator*=(const CalVector& v)
00086 {
00087 float qx, qy, qz, qw;
00088 qx = x;
00089 qy = y;
00090 qz = z;
00091 qw = w;
00092
00093 x = qw * v.x + qy * v.z - qz * v.y;
00094 y = qw * v.y - qx * v.z + qz * v.x;
00095 z = qw * v.z + qx * v.y - qy * v.x;
00096 w = - qx * v.x - qy * v.y - qz * v.z;
00097 }
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109 inline void blend(float d, const CalQuaternion& q)
00110 {
00111 float norm;
00112 norm = x * q.x + y * q.y + z * q.z + w * q.w;
00113
00114 bool bFlip;
00115 bFlip = false;
00116
00117 if(norm < 0.0f)
00118 {
00119 norm = -norm;
00120 bFlip = true;
00121 }
00122
00123 float inv_d;
00124 if(1.0f - norm < 0.000001f)
00125 {
00126 inv_d = 1.0f - d;
00127 }
00128 else
00129 {
00130 float theta;
00131 theta = (float) acos(norm);
00132
00133 float s;
00134 s = (float) (1.0f / sin(theta));
00135
00136 inv_d = (float) sin((1.0f - d) * theta) * s;
00137 d = (float) sin(d * theta) * s;
00138 }
00139
00140 if(bFlip)
00141 {
00142 d = -d;
00143 }
00144
00145 x = inv_d * x + d * q.x;
00146 y = inv_d * y + d * q.y;
00147 z = inv_d * z + d * q.z;
00148 w = inv_d * w + d * q.w;
00149 }
00150
00151 inline void clear()
00152 {
00153 x = 0.0f;
00154 y = 0.0f;
00155 z = 0.0f;
00156 w = 1.0f;
00157 }
00158 inline void conjugate()
00159 {
00160 x = -x;
00161 y = -y;
00162 z = -z;
00163 }
00164
00165 inline void invert()
00166 {
00167 conjugate();
00168 const float norm = (x*x) + (y*y) + (z*z) + (w*w);
00169
00170 if (norm == 0.0f) return;
00171
00172 const float inv_norm = 1 / norm;
00173 x *= inv_norm;
00174 y *= inv_norm;
00175 z *= inv_norm;
00176 w *= inv_norm;
00177 }
00178
00179 inline void set(float qx, float qy, float qz, float qw)
00180 {
00181 x = qx;
00182 y = qy;
00183 z = qz;
00184 w = qw;
00185 }
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203 };
00204
00205
00206 static inline CalQuaternion operator*(const CalQuaternion& q, const CalQuaternion& r)
00207 {
00208 return CalQuaternion(
00209 r.w * q.x + r.x * q.w + r.y * q.z - r.z * q.y,
00210 r.w * q.y - r.x * q.z + r.y * q.w + r.z * q.x,
00211 r.w * q.z + r.x * q.y - r.y * q.x + r.z * q.w,
00212 r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z
00213 );
00214 }
00215
00216 static inline CalQuaternion shortestArc( const CalVector& from, const CalVector& to )
00217 {
00218 CalVector cross = from % to;
00219 float dot = from * to ;
00220
00221 dot = (float) sqrt( 2*(dot+1) ) ;
00222
00223 cross /= dot ;
00224
00225
00226
00227 return CalQuaternion( cross[0], cross[1], cross[2], -dot/2 ) ;
00228
00229 }
00230
00231
00232 #endif
00233
00234