Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00023 #ifndef __RS_QUATERNION_RSH__
00024 #define __RS_QUATERNION_RSH__
00025
00026
00034 static void __attribute__((overloadable))
00035 rsQuaternionSet(rs_quaternion *q, float w, float x, float y, float z) {
00036 q->w = w;
00037 q->x = x;
00038 q->y = y;
00039 q->z = z;
00040 }
00041
00047 static void __attribute__((overloadable))
00048 rsQuaternionSet(rs_quaternion *q, const rs_quaternion *rhs) {
00049 q->w = rhs->w;
00050 q->x = rhs->x;
00051 q->y = rhs->y;
00052 q->z = rhs->z;
00053 }
00054
00060 static void __attribute__((overloadable))
00061 rsQuaternionMultiply(rs_quaternion *q, float s) {
00062 q->w *= s;
00063 q->x *= s;
00064 q->y *= s;
00065 q->z *= s;
00066 }
00067
00073 static void
00074 rsQuaternionAdd(rs_quaternion *q, const rs_quaternion *rhs) {
00075 q->w *= rhs->w;
00076 q->x *= rhs->x;
00077 q->y *= rhs->y;
00078 q->z *= rhs->z;
00079 }
00080
00089 static void
00090 rsQuaternionLoadRotateUnit(rs_quaternion *q, float rot, float x, float y, float z) {
00091 rot *= (float)(M_PI / 180.0f) * 0.5f;
00092 float c = cos(rot);
00093 float s = sin(rot);
00094
00095 q->w = c;
00096 q->x = x * s;
00097 q->y = y * s;
00098 q->z = z * s;
00099 }
00100
00110 static void
00111 rsQuaternionLoadRotate(rs_quaternion *q, float rot, float x, float y, float z) {
00112 const float len = x*x + y*y + z*z;
00113 if (len != 1) {
00114 const float recipLen = 1.f / sqrt(len);
00115 x *= recipLen;
00116 y *= recipLen;
00117 z *= recipLen;
00118 }
00119 rsQuaternionLoadRotateUnit(q, rot, x, y, z);
00120 }
00121
00126 static void
00127 rsQuaternionConjugate(rs_quaternion *q) {
00128 q->x = -q->x;
00129 q->y = -q->y;
00130 q->z = -q->z;
00131 }
00132
00139 static float
00140 rsQuaternionDot(const rs_quaternion *q0, const rs_quaternion *q1) {
00141 return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z;
00142 }
00143
00148 static void
00149 rsQuaternionNormalize(rs_quaternion *q) {
00150 const float len = rsQuaternionDot(q, q);
00151 if (len != 1) {
00152 const float recipLen = 1.f / sqrt(len);
00153 rsQuaternionMultiply(q, recipLen);
00154 }
00155 }
00156
00162 static void __attribute__((overloadable))
00163 rsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) {
00164 rs_quaternion qtmp;
00165 rsQuaternionSet(&qtmp, q);
00166
00167 q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z;
00168 q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y;
00169 q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z;
00170 q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x;
00171 rsQuaternionNormalize(q);
00172 }
00173
00181 static void
00182 rsQuaternionSlerp(rs_quaternion *q, const rs_quaternion *q0, const rs_quaternion *q1, float t) {
00183 if (t <= 0.0f) {
00184 rsQuaternionSet(q, q0);
00185 return;
00186 }
00187 if (t >= 1.0f) {
00188 rsQuaternionSet(q, q1);
00189 return;
00190 }
00191
00192 rs_quaternion tempq0, tempq1;
00193 rsQuaternionSet(&tempq0, q0);
00194 rsQuaternionSet(&tempq1, q1);
00195
00196 float angle = rsQuaternionDot(q0, q1);
00197 if (angle < 0) {
00198 rsQuaternionMultiply(&tempq0, -1.0f);
00199 angle *= -1.0f;
00200 }
00201
00202 float scale, invScale;
00203 if (angle + 1.0f > 0.05f) {
00204 if (1.0f - angle >= 0.05f) {
00205 float theta = acos(angle);
00206 float invSinTheta = 1.0f / sin(theta);
00207 scale = sin(theta * (1.0f - t)) * invSinTheta;
00208 invScale = sin(theta * t) * invSinTheta;
00209 } else {
00210 scale = 1.0f - t;
00211 invScale = t;
00212 }
00213 } else {
00214 rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w);
00215 scale = sin(M_PI * (0.5f - t));
00216 invScale = sin(M_PI * t);
00217 }
00218
00219 rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale,
00220 tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale);
00221 }
00222
00228 static void rsQuaternionGetMatrixUnit(rs_matrix4x4 *m, const rs_quaternion *q) {
00229 float xx = q->x * q->x;
00230 float xy = q->x * q->y;
00231 float xz = q->x * q->z;
00232 float xw = q->x * q->w;
00233 float yy = q->y * q->y;
00234 float yz = q->y * q->z;
00235 float yw = q->y * q->w;
00236 float zz = q->z * q->z;
00237 float zw = q->z * q->w;
00238
00239 m->m[0] = 1.0f - 2.0f * ( yy + zz );
00240 m->m[4] = 2.0f * ( xy - zw );
00241 m->m[8] = 2.0f * ( xz + yw );
00242 m->m[1] = 2.0f * ( xy + zw );
00243 m->m[5] = 1.0f - 2.0f * ( xx + zz );
00244 m->m[9] = 2.0f * ( yz - xw );
00245 m->m[2] = 2.0f * ( xz - yw );
00246 m->m[6] = 2.0f * ( yz + xw );
00247 m->m[10] = 1.0f - 2.0f * ( xx + yy );
00248 m->m[3] = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f;
00249 m->m[15] = 1.0f;
00250 }
00251
00252 #endif
00253