rs_quaternion.c revision 14c8aa51782107afb6f84187233f5ac8f6da0373 
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))
11extern 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
19extern 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
26extern 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
31extern 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
56extern 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
68extern 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
76extern 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
84extern 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
96extern 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
108extern 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
116extern 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
128extern 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