Home | History | Annotate | Download | only in scriptc
      1 /*
      2  * Copyright (C) 2011 The Android Open Source Project
      3  *
      4  * Licensed under the Apache License, Version 2.0 (the "License");
      5  * you may not use this file except in compliance with the License.
      6  * You may obtain a copy of the License at
      7  *
      8  *      http://www.apache.org/licenses/LICENSE-2.0
      9  *
     10  * Unless required by applicable law or agreed to in writing, software
     11  * distributed under the License is distributed on an "AS IS" BASIS,
     12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     13  * See the License for the specific language governing permissions and
     14  * limitations under the License.
     15  */
     16 
     17 /** @file rs_quaternion.rsh
     18  *  \brief Quaternion routines
     19  *
     20  *
     21  */
     22 
     23 #ifndef __RS_QUATERNION_RSH__
     24 #define __RS_QUATERNION_RSH__
     25 
     26 
     27 /**
     28  * Set the quaternion components
     29  * @param w component
     30  * @param x component
     31  * @param y component
     32  * @param z component
     33  */
     34 static void __attribute__((overloadable))
     35 rsQuaternionSet(rs_quaternion *q, float w, float x, float y, float z) {
     36     q->w = w;
     37     q->x = x;
     38     q->y = y;
     39     q->z = z;
     40 }
     41 
     42 /**
     43  * Set the quaternion from another quaternion
     44  * @param q destination quaternion
     45  * @param rhs source quaternion
     46  */
     47 static void __attribute__((overloadable))
     48 rsQuaternionSet(rs_quaternion *q, const rs_quaternion *rhs) {
     49     q->w = rhs->w;
     50     q->x = rhs->x;
     51     q->y = rhs->y;
     52     q->z = rhs->z;
     53 }
     54 
     55 /**
     56  * Multiply quaternion by a scalar
     57  * @param q quaternion to multiply
     58  * @param s scalar
     59  */
     60 static void __attribute__((overloadable))
     61 rsQuaternionMultiply(rs_quaternion *q, float s) {
     62     q->w *= s;
     63     q->x *= s;
     64     q->y *= s;
     65     q->z *= s;
     66 }
     67 
     68 /**
     69  * Add two quaternions
     70  * @param q destination quaternion to add to
     71  * @param rsh right hand side quaternion to add
     72  */
     73 static void
     74 rsQuaternionAdd(rs_quaternion *q, const rs_quaternion *rhs) {
     75     q->w *= rhs->w;
     76     q->x *= rhs->x;
     77     q->y *= rhs->y;
     78     q->z *= rhs->z;
     79 }
     80 
     81 /**
     82  * Loads a quaternion that represents a rotation about an arbitrary unit vector
     83  * @param q quaternion to set
     84  * @param rot angle to rotate by
     85  * @param x component of a vector
     86  * @param y component of a vector
     87  * @param x component of a vector
     88  */
     89 static void
     90 rsQuaternionLoadRotateUnit(rs_quaternion *q, float rot, float x, float y, float z) {
     91     rot *= (float)(M_PI / 180.0f) * 0.5f;
     92     float c = cos(rot);
     93     float s = sin(rot);
     94 
     95     q->w = c;
     96     q->x = x * s;
     97     q->y = y * s;
     98     q->z = z * s;
     99 }
    100 
    101 /**
    102  * Loads a quaternion that represents a rotation about an arbitrary vector
    103  * (doesn't have to be unit)
    104  * @param q quaternion to set
    105  * @param rot angle to rotate by
    106  * @param x component of a vector
    107  * @param y component of a vector
    108  * @param x component of a vector
    109  */
    110 static void
    111 rsQuaternionLoadRotate(rs_quaternion *q, float rot, float x, float y, float z) {
    112     const float len = x*x + y*y + z*z;
    113     if (len != 1) {
    114         const float recipLen = 1.f / sqrt(len);
    115         x *= recipLen;
    116         y *= recipLen;
    117         z *= recipLen;
    118     }
    119     rsQuaternionLoadRotateUnit(q, rot, x, y, z);
    120 }
    121 
    122 /**
    123  * Conjugates the quaternion
    124  * @param q quaternion to conjugate
    125  */
    126 static void
    127 rsQuaternionConjugate(rs_quaternion *q) {
    128     q->x = -q->x;
    129     q->y = -q->y;
    130     q->z = -q->z;
    131 }
    132 
    133 /**
    134  * Dot product of two quaternions
    135  * @param q0 first quaternion
    136  * @param q1 second quaternion
    137  * @return dot product between q0 and q1
    138  */
    139 static float
    140 rsQuaternionDot(const rs_quaternion *q0, const rs_quaternion *q1) {
    141     return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z;
    142 }
    143 
    144 /**
    145  * Normalizes the quaternion
    146  * @param q quaternion to normalize
    147  */
    148 static void
    149 rsQuaternionNormalize(rs_quaternion *q) {
    150     const float len = rsQuaternionDot(q, q);
    151     if (len != 1) {
    152         const float recipLen = 1.f / sqrt(len);
    153         rsQuaternionMultiply(q, recipLen);
    154     }
    155 }
    156 
    157 /**
    158  * Multiply quaternion by another quaternion
    159  * @param q destination quaternion
    160  * @param rhs right hand side quaternion to multiply by
    161  */
    162 static void __attribute__((overloadable))
    163 rsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) {
    164     rs_quaternion qtmp;
    165     rsQuaternionSet(&qtmp, q);
    166 
    167     q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z;
    168     q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y;
    169     q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z;
    170     q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x;
    171     rsQuaternionNormalize(q);
    172 }
    173 
    174 /**
    175  * Performs spherical linear interpolation between two quaternions
    176  * @param q result quaternion from interpolation
    177  * @param q0 first param
    178  * @param q1 second param
    179  * @param t how much to interpolate by
    180  */
    181 static void
    182 rsQuaternionSlerp(rs_quaternion *q, const rs_quaternion *q0, const rs_quaternion *q1, float t) {
    183     if (t <= 0.0f) {
    184         rsQuaternionSet(q, q0);
    185         return;
    186     }
    187     if (t >= 1.0f) {
    188         rsQuaternionSet(q, q1);
    189         return;
    190     }
    191 
    192     rs_quaternion tempq0, tempq1;
    193     rsQuaternionSet(&tempq0, q0);
    194     rsQuaternionSet(&tempq1, q1);
    195 
    196     float angle = rsQuaternionDot(q0, q1);
    197     if (angle < 0) {
    198         rsQuaternionMultiply(&tempq0, -1.0f);
    199         angle *= -1.0f;
    200     }
    201 
    202     float scale, invScale;
    203     if (angle + 1.0f > 0.05f) {
    204         if (1.0f - angle >= 0.05f) {
    205             float theta = acos(angle);
    206             float invSinTheta = 1.0f / sin(theta);
    207             scale = sin(theta * (1.0f - t)) * invSinTheta;
    208             invScale = sin(theta * t) * invSinTheta;
    209         } else {
    210             scale = 1.0f - t;
    211             invScale = t;
    212         }
    213     } else {
    214         rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w);
    215         scale = sin(M_PI * (0.5f - t));
    216         invScale = sin(M_PI * t);
    217     }
    218 
    219     rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale,
    220                         tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale);
    221 }
    222 
    223 /**
    224  * Computes rotation matrix from the normalized quaternion
    225  * @param m resulting matrix
    226  * @param p normalized quaternion
    227  */
    228 static void rsQuaternionGetMatrixUnit(rs_matrix4x4 *m, const rs_quaternion *q) {
    229     float xx = q->x * q->x;
    230     float xy = q->x * q->y;
    231     float xz = q->x * q->z;
    232     float xw = q->x * q->w;
    233     float yy = q->y * q->y;
    234     float yz = q->y * q->z;
    235     float yw = q->y * q->w;
    236     float zz = q->z * q->z;
    237     float zw = q->z * q->w;
    238 
    239     m->m[0]  = 1.0f - 2.0f * ( yy + zz );
    240     m->m[4]  =        2.0f * ( xy - zw );
    241     m->m[8]  =        2.0f * ( xz + yw );
    242     m->m[1]  =        2.0f * ( xy + zw );
    243     m->m[5]  = 1.0f - 2.0f * ( xx + zz );
    244     m->m[9]  =        2.0f * ( yz - xw );
    245     m->m[2]  =        2.0f * ( xz - yw );
    246     m->m[6]  =        2.0f * ( yz + xw );
    247     m->m[10] = 1.0f - 2.0f * ( xx + yy );
    248     m->m[3]  = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f;
    249     m->m[15] = 1.0f;
    250 }
    251 
    252 #endif
    253 
    254