Home | History | Annotate | Download | only in runtime
      1 #include "rs_core.rsh"
      2 
      3 /* Implementation of Core Runtime */
      4 
      5 
      6 /////////////////////////////////////////////////////
      7 // Quaternion ops
      8 /////////////////////////////////////////////////////
      9 
     10 #if (defined(RS_VERSION) && (RS_VERSION >= UNRELEASED))
     11 extern void __attribute__((overloadable))
     12     rsQuaternionAdd(rs_quaternion* q, const rs_quaternion* rhs) {
     13     q->w += rhs->w;
     14     q->x += rhs->x;
     15     q->y += rhs->y;
     16     q->z += rhs->z;
     17 }
     18 
     19 extern void __attribute__((overloadable))
     20     rsQuaternionConjugate(rs_quaternion* q) {
     21     q->x = -q->x;
     22     q->y = -q->y;
     23     q->z = -q->z;
     24 }
     25 
     26 extern float __attribute__((overloadable))
     27     rsQuaternionDot(const rs_quaternion* q0, const rs_quaternion* q1) {
     28     return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z;
     29 }
     30 
     31 extern void __attribute__((overloadable))
     32     rsQuaternionGetMatrixUnit(rs_matrix4x4* m, const rs_quaternion* q) {
     33     float xx = q->x * q->x;
     34     float xy = q->x * q->y;
     35     float xz = q->x * q->z;
     36     float xw = q->x * q->w;
     37     float yy = q->y * q->y;
     38     float yz = q->y * q->z;
     39     float yw = q->y * q->w;
     40     float zz = q->z * q->z;
     41     float zw = q->z * q->w;
     42 
     43     m->m[0]  = 1.0f - 2.0f * ( yy + zz );
     44     m->m[4]  =        2.0f * ( xy - zw );
     45     m->m[8]  =        2.0f * ( xz + yw );
     46     m->m[1]  =        2.0f * ( xy + zw );
     47     m->m[5]  = 1.0f - 2.0f * ( xx + zz );
     48     m->m[9]  =        2.0f * ( yz - xw );
     49     m->m[2]  =        2.0f * ( xz - yw );
     50     m->m[6]  =        2.0f * ( yz + xw );
     51     m->m[10] = 1.0f - 2.0f * ( xx + yy );
     52     m->m[3]  = m->m[7] = m->m[11] = m->m[12] = m->m[13] = m->m[14] = 0.0f;
     53     m->m[15] = 1.0f;
     54 }
     55 
     56 extern void __attribute__((overloadable))
     57     rsQuaternionLoadRotateUnit(rs_quaternion* q, float rot, float x, float y, float z) {
     58     rot *= (float)(M_PI / 180.0f) * 0.5f;
     59     float c = cos(rot);
     60     float s = sin(rot);
     61 
     62     q->w = c;
     63     q->x = x * s;
     64     q->y = y * s;
     65     q->z = z * s;
     66 }
     67 
     68 extern void __attribute__((overloadable))
     69     rsQuaternionSet(rs_quaternion* q, float w, float x, float y, float z) {
     70     q->w = w;
     71     q->x = x;
     72     q->y = y;
     73     q->z = z;
     74 }
     75 
     76 extern void __attribute__((overloadable))
     77     rsQuaternionSet(rs_quaternion* q, const rs_quaternion* rhs) {
     78     q->w = rhs->w;
     79     q->x = rhs->x;
     80     q->y = rhs->y;
     81     q->z = rhs->z;
     82 }
     83 
     84 extern void __attribute__((overloadable))
     85     rsQuaternionLoadRotate(rs_quaternion* q, float rot, float x, float y, float z) {
     86     const float len = x*x + y*y + z*z;
     87     if (len != 1) {
     88         const float recipLen = 1.f / sqrt(len);
     89         x *= recipLen;
     90         y *= recipLen;
     91         z *= recipLen;
     92     }
     93     rsQuaternionLoadRotateUnit(q, rot, x, y, z);
     94 }
     95 
     96 extern void __attribute__((overloadable))
     97     rsQuaternionNormalize(rs_quaternion* q) {
     98     const float len = rsQuaternionDot(q, q);
     99     if (len != 1) {
    100         const float recipLen = 1.f / sqrt(len);
    101         q->w *= recipLen;
    102         q->x *= recipLen;
    103         q->y *= recipLen;
    104         q->z *= recipLen;
    105     }
    106 }
    107 
    108 extern void __attribute__((overloadable))
    109     rsQuaternionMultiply(rs_quaternion* q, float scalar) {
    110     q->w *= scalar;
    111     q->x *= scalar;
    112     q->y *= scalar;
    113     q->z *= scalar;
    114 }
    115 
    116 extern void __attribute__((overloadable))
    117     rsQuaternionMultiply(rs_quaternion* q, const rs_quaternion* rhs) {
    118     rs_quaternion qtmp;
    119     rsQuaternionSet(&qtmp, q);
    120 
    121     q->w = qtmp.w*rhs->w - qtmp.x*rhs->x - qtmp.y*rhs->y - qtmp.z*rhs->z;
    122     q->x = qtmp.w*rhs->x + qtmp.x*rhs->w + qtmp.y*rhs->z - qtmp.z*rhs->y;
    123     q->y = qtmp.w*rhs->y + qtmp.y*rhs->w + qtmp.z*rhs->x - qtmp.x*rhs->z;
    124     q->z = qtmp.w*rhs->z + qtmp.z*rhs->w + qtmp.x*rhs->y - qtmp.y*rhs->x;
    125     rsQuaternionNormalize(q);
    126 }
    127 
    128 extern void __attribute__((overloadable))
    129     rsQuaternionSlerp(rs_quaternion* q, const rs_quaternion* q0, const rs_quaternion* q1, float t) {
    130     if (t <= 0.0f) {
    131         rsQuaternionSet(q, q0);
    132         return;
    133     }
    134     if (t >= 1.0f) {
    135         rsQuaternionSet(q, q1);
    136         return;
    137     }
    138 
    139     rs_quaternion tempq0, tempq1;
    140     rsQuaternionSet(&tempq0, q0);
    141     rsQuaternionSet(&tempq1, q1);
    142 
    143     float angle = rsQuaternionDot(q0, q1);
    144     if (angle < 0) {
    145         rsQuaternionMultiply(&tempq0, -1.0f);
    146         angle *= -1.0f;
    147     }
    148 
    149     float scale, invScale;
    150     if (angle + 1.0f > 0.05f) {
    151         if (1.0f - angle >= 0.05f) {
    152             float theta = acos(angle);
    153             float invSinTheta = 1.0f / sin(theta);
    154             scale = sin(theta * (1.0f - t)) * invSinTheta;
    155             invScale = sin(theta * t) * invSinTheta;
    156         } else {
    157             scale = 1.0f - t;
    158             invScale = t;
    159         }
    160     } else {
    161         rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w);
    162         scale = sin(M_PI * (0.5f - t));
    163         invScale = sin(M_PI * t);
    164     }
    165 
    166     rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale,
    167                         tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale);
    168 }
    169 #endif // (defined(RS_VERSION) && (RS_VERSION >= UNRELEASED))
    170