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 >= 24))
11 extern void __attribute__((overloadable))
rsQuaternionAdd(rs_quaternion * q,const rs_quaternion * rhs)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))
rsQuaternionConjugate(rs_quaternion * q)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))
rsQuaternionDot(const rs_quaternion * q0,const rs_quaternion * q1)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))
rsQuaternionGetMatrixUnit(rs_matrix4x4 * m,const rs_quaternion * q)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))
rsQuaternionLoadRotateUnit(rs_quaternion * q,float rot,float x,float y,float z)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))
rsQuaternionSet(rs_quaternion * q,float w,float x,float y,float z)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))
rsQuaternionSet(rs_quaternion * q,const rs_quaternion * rhs)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))
rsQuaternionLoadRotate(rs_quaternion * q,float rot,float x,float y,float z)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))
rsQuaternionNormalize(rs_quaternion * q)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))
rsQuaternionMultiply(rs_quaternion * q,float scalar)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))
rsQuaternionMultiply(rs_quaternion * q,const rs_quaternion * rhs)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))
rsQuaternionSlerp(rs_quaternion * q,const rs_quaternion * q0,const rs_quaternion * q1,float t)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 >= 24))
170