rs_quaternion.rsh Source File

- Overview
- Globals
- Structs

rs_quaternion.rsh
Go to the documentation of this file.

    00001 /*
    00002  * Copyright (C) 2011 The Android Open Source Project
    00003  *
    00004  * Licensed under the Apache License, Version 2.0 (the "License");
    00005  * you may not use this file except in compliance with the License.
    00006  * You may obtain a copy of the License at
    00007  *
    00008  *      http://www.apache.org/licenses/LICENSE-2.0
    00009  *
    00010  * Unless required by applicable law or agreed to in writing, software
    00011  * distributed under the License is distributed on an "AS IS" BASIS,
    00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    00013  * See the License for the specific language governing permissions and
    00014  * limitations under the License.
    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