rs_core.rsh revision 95333f998fd4a983f89e5128e85f6f710d200bd9 
1#ifndef __RS_CORE_RSH__
2#define __RS_CORE_RSH__
3
4static void __attribute__((overloadable)) rsDebug(const char *s, float2 v) {
5    rsDebug(s, v.x, v.y);
6}
7static void __attribute__((overloadable)) rsDebug(const char *s, float3 v) {
8    rsDebug(s, v.x, v.y, v.z);
9}
10static void __attribute__((overloadable)) rsDebug(const char *s, float4 v) {
11    rsDebug(s, v.x, v.y, v.z, v.w);
12}
13
14static uchar4 __attribute__((overloadable)) rsPackColorTo8888(float r, float g, float b)
15{
16    uchar4 c;
17    c.x = (uchar)(r * 255.f);
18    c.y = (uchar)(g * 255.f);
19    c.z = (uchar)(b * 255.f);
20    c.w = 255;
21    return c;
22}
23
24static uchar4 __attribute__((overloadable)) rsPackColorTo8888(float r, float g, float b, float a)
25{
26    uchar4 c;
27    c.x = (uchar)(r * 255.f);
28    c.y = (uchar)(g * 255.f);
29    c.z = (uchar)(b * 255.f);
30    c.w = (uchar)(a * 255.f);
31    return c;
32}
33
34static uchar4 __attribute__((overloadable)) rsPackColorTo8888(float3 color)
35{
36    color *= 255.f;
37    uchar4 c = {color.x, color.y, color.z, 255};
38    return c;
39}
40
41static uchar4 __attribute__((overloadable)) rsPackColorTo8888(float4 color)
42{
43    color *= 255.f;
44    uchar4 c = {color.x, color.y, color.z, color.w};
45    return c;
46}
47
48static float4 rsUnpackColor8888(uchar4 c)
49{
50    float4 ret = (float4)0.0039156862745f;
51    ret *= convert_float4(c);
52    return ret;
53}
54
55//extern uchar4 __attribute__((overloadable)) rsPackColorTo565(float r, float g, float b);
56//extern uchar4 __attribute__((overloadable)) rsPackColorTo565(float3);
57//extern float4 rsUnpackColor565(uchar4);
58
59
60/////////////////////////////////////////////////////
61// Matrix ops
62/////////////////////////////////////////////////////
63
64static void __attribute__((overloadable))
65rsMatrixSet(rs_matrix4x4 *m, uint32_t row, uint32_t col, float v) {
66    m->m[row * 4 + col] = v;
67}
68
69static float __attribute__((overloadable))
70rsMatrixGet(const rs_matrix4x4 *m, uint32_t row, uint32_t col) {
71    return m->m[row * 4 + col];
72}
73
74static void __attribute__((overloadable))
75rsMatrixSet(rs_matrix3x3 *m, uint32_t row, uint32_t col, float v) {
76    m->m[row * 3 + col] = v;
77}
78
79static float __attribute__((overloadable))
80rsMatrixGet(const rs_matrix3x3 *m, uint32_t row, uint32_t col) {
81    return m->m[row * 3 + col];
82}
83
84static void __attribute__((overloadable))
85rsMatrixSet(rs_matrix2x2 *m, uint32_t row, uint32_t col, float v) {
86    m->m[row * 2 + col] = v;
87}
88
89static float __attribute__((overloadable))
90rsMatrixGet(const rs_matrix2x2 *m, uint32_t row, uint32_t col) {
91    return m->m[row * 2 + col];
92}
93
94static void __attribute__((overloadable))
95rsMatrixLoadIdentity(rs_matrix4x4 *m) {
96    m->m[0] = 1.f;
97    m->m[1] = 0.f;
98    m->m[2] = 0.f;
99    m->m[3] = 0.f;
100    m->m[4] = 0.f;
101    m->m[5] = 1.f;
102    m->m[6] = 0.f;
103    m->m[7] = 0.f;
104    m->m[8] = 0.f;
105    m->m[9] = 0.f;
106    m->m[10] = 1.f;
107    m->m[11] = 0.f;
108    m->m[12] = 0.f;
109    m->m[13] = 0.f;
110    m->m[14] = 0.f;
111    m->m[15] = 1.f;
112}
113
114static void __attribute__((overloadable))
115rsMatrixLoadIdentity(rs_matrix3x3 *m) {
116    m->m[0] = 1.f;
117    m->m[1] = 0.f;
118    m->m[2] = 0.f;
119    m->m[3] = 0.f;
120    m->m[4] = 1.f;
121    m->m[5] = 0.f;
122    m->m[6] = 0.f;
123    m->m[7] = 0.f;
124    m->m[8] = 1.f;
125}
126
127static void __attribute__((overloadable))
128rsMatrixLoadIdentity(rs_matrix2x2 *m) {
129    m->m[0] = 1.f;
130    m->m[1] = 0.f;
131    m->m[2] = 0.f;
132    m->m[3] = 1.f;
133}
134
135static void __attribute__((overloadable))
136rsMatrixLoad(rs_matrix4x4 *m, const float *v) {
137    m->m[0] = v[0];
138    m->m[1] = v[1];
139    m->m[2] = v[2];
140    m->m[3] = v[3];
141    m->m[4] = v[4];
142    m->m[5] = v[5];
143    m->m[6] = v[6];
144    m->m[7] = v[7];
145    m->m[8] = v[8];
146    m->m[9] = v[9];
147    m->m[10] = v[10];
148    m->m[11] = v[11];
149    m->m[12] = v[12];
150    m->m[13] = v[13];
151    m->m[14] = v[14];
152    m->m[15] = v[15];
153}
154
155static void __attribute__((overloadable))
156rsMatrixLoad(rs_matrix3x3 *m, const float *v) {
157    m->m[0] = v[0];
158    m->m[1] = v[1];
159    m->m[2] = v[2];
160    m->m[3] = v[3];
161    m->m[4] = v[4];
162    m->m[5] = v[5];
163    m->m[6] = v[6];
164    m->m[7] = v[7];
165    m->m[8] = v[8];
166}
167
168static void __attribute__((overloadable))
169rsMatrixLoad(rs_matrix2x2 *m, const float *v) {
170    m->m[0] = v[0];
171    m->m[1] = v[1];
172    m->m[2] = v[2];
173    m->m[3] = v[3];
174}
175
176static void __attribute__((overloadable))
177rsMatrixLoad(rs_matrix4x4 *m, const rs_matrix4x4 *v) {
178    m->m[0] = v->m[0];
179    m->m[1] = v->m[1];
180    m->m[2] = v->m[2];
181    m->m[3] = v->m[3];
182    m->m[4] = v->m[4];
183    m->m[5] = v->m[5];
184    m->m[6] = v->m[6];
185    m->m[7] = v->m[7];
186    m->m[8] = v->m[8];
187    m->m[9] = v->m[9];
188    m->m[10] = v->m[10];
189    m->m[11] = v->m[11];
190    m->m[12] = v->m[12];
191    m->m[13] = v->m[13];
192    m->m[14] = v->m[14];
193    m->m[15] = v->m[15];
194}
195
196static void __attribute__((overloadable))
197rsMatrixLoad(rs_matrix4x4 *m, const rs_matrix3x3 *v) {
198    m->m[0] = v->m[0];
199    m->m[1] = v->m[1];
200    m->m[2] = v->m[2];
201    m->m[3] = 0.f;
202    m->m[4] = v->m[3];
203    m->m[5] = v->m[4];
204    m->m[6] = v->m[5];
205    m->m[7] = 0.f;
206    m->m[8] = v->m[6];
207    m->m[9] = v->m[7];
208    m->m[10] = v->m[8];
209    m->m[11] = 0.f;
210    m->m[12] = 0.f;
211    m->m[13] = 0.f;
212    m->m[14] = 0.f;
213    m->m[15] = 1.f;
214}
215
216static void __attribute__((overloadable))
217rsMatrixLoad(rs_matrix4x4 *m, const rs_matrix2x2 *v) {
218    m->m[0] = v->m[0];
219    m->m[1] = v->m[1];
220    m->m[2] = 0.f;
221    m->m[3] = 0.f;
222    m->m[4] = v->m[3];
223    m->m[5] = v->m[4];
224    m->m[6] = 0.f;
225    m->m[7] = 0.f;
226    m->m[8] = v->m[6];
227    m->m[9] = v->m[7];
228    m->m[10] = 1.f;
229    m->m[11] = 0.f;
230    m->m[12] = 0.f;
231    m->m[13] = 0.f;
232    m->m[14] = 0.f;
233    m->m[15] = 1.f;
234}
235
236static void __attribute__((overloadable))
237rsMatrixLoad(rs_matrix3x3 *m, const rs_matrix3x3 *v) {
238    m->m[0] = v->m[0];
239    m->m[1] = v->m[1];
240    m->m[2] = v->m[2];
241    m->m[3] = v->m[3];
242    m->m[4] = v->m[4];
243    m->m[5] = v->m[5];
244    m->m[6] = v->m[6];
245    m->m[7] = v->m[7];
246    m->m[8] = v->m[8];
247}
248
249static void __attribute__((overloadable))
250rsMatrixLoad(rs_matrix2x2 *m, const rs_matrix2x2 *v) {
251    m->m[0] = v->m[0];
252    m->m[1] = v->m[1];
253    m->m[2] = v->m[2];
254    m->m[3] = v->m[3];
255}
256
257static void __attribute__((overloadable))
258rsMatrixLoadRotate(rs_matrix4x4 *m, float rot, float x, float y, float z) {
259    float c, s;
260    m->m[3] = 0;
261    m->m[7] = 0;
262    m->m[11]= 0;
263    m->m[12]= 0;
264    m->m[13]= 0;
265    m->m[14]= 0;
266    m->m[15]= 1;
267    rot *= (float)(M_PI / 180.0f);
268    c = cos(rot);
269    s = sin(rot);
270
271    const float len = x*x + y*y + z*z;
272    if (len != 1) {
273        const float recipLen = 1.f / sqrt(len);
274        x *= recipLen;
275        y *= recipLen;
276        z *= recipLen;
277    }
278    const float nc = 1.0f - c;
279    const float xy = x * y;
280    const float yz = y * z;
281    const float zx = z * x;
282    const float xs = x * s;
283    const float ys = y * s;
284    const float zs = z * s;
285    m->m[ 0] = x*x*nc +  c;
286    m->m[ 4] =  xy*nc - zs;
287    m->m[ 8] =  zx*nc + ys;
288    m->m[ 1] =  xy*nc + zs;
289    m->m[ 5] = y*y*nc +  c;
290    m->m[ 9] =  yz*nc - xs;
291    m->m[ 2] =  zx*nc - ys;
292    m->m[ 6] =  yz*nc + xs;
293    m->m[10] = z*z*nc +  c;
294}
295
296static void __attribute__((overloadable))
297rsMatrixLoadScale(rs_matrix4x4 *m, float x, float y, float z) {
298    rsMatrixLoadIdentity(m);
299    m->m[0] = x;
300    m->m[5] = y;
301    m->m[10] = z;
302}
303
304static void __attribute__((overloadable))
305rsMatrixLoadTranslate(rs_matrix4x4 *m, float x, float y, float z) {
306    rsMatrixLoadIdentity(m);
307    m->m[12] = x;
308    m->m[13] = y;
309    m->m[14] = z;
310}
311
312static void __attribute__((overloadable))
313rsMatrixLoadMultiply(rs_matrix4x4 *m, const rs_matrix4x4 *lhs, const rs_matrix4x4 *rhs) {
314    for (int i=0 ; i<4 ; i++) {
315        float ri0 = 0;
316        float ri1 = 0;
317        float ri2 = 0;
318        float ri3 = 0;
319        for (int j=0 ; j<4 ; j++) {
320            const float rhs_ij = rsMatrixGet(rhs, i,j);
321            ri0 += rsMatrixGet(lhs, j, 0) * rhs_ij;
322            ri1 += rsMatrixGet(lhs, j, 1) * rhs_ij;
323            ri2 += rsMatrixGet(lhs, j, 2) * rhs_ij;
324            ri3 += rsMatrixGet(lhs, j, 3) * rhs_ij;
325        }
326        rsMatrixSet(m, i, 0, ri0);
327        rsMatrixSet(m, i, 1, ri1);
328        rsMatrixSet(m, i, 2, ri2);
329        rsMatrixSet(m, i, 3, ri3);
330    }
331}
332
333static void __attribute__((overloadable))
334rsMatrixMultiply(rs_matrix4x4 *m, const rs_matrix4x4 *rhs) {
335    rs_matrix4x4 mt;
336    rsMatrixLoadMultiply(&mt, m, rhs);
337    rsMatrixLoad(m, &mt);
338}
339
340static void __attribute__((overloadable))
341rsMatrixLoadMultiply(rs_matrix3x3 *m, const rs_matrix3x3 *lhs, const rs_matrix3x3 *rhs) {
342    for (int i=0 ; i<3 ; i++) {
343        float ri0 = 0;
344        float ri1 = 0;
345        float ri2 = 0;
346        for (int j=0 ; j<3 ; j++) {
347            const float rhs_ij = rsMatrixGet(rhs, i,j);
348            ri0 += rsMatrixGet(lhs, j, 0) * rhs_ij;
349            ri1 += rsMatrixGet(lhs, j, 1) * rhs_ij;
350            ri2 += rsMatrixGet(lhs, j, 2) * rhs_ij;
351        }
352        rsMatrixSet(m, i, 0, ri0);
353        rsMatrixSet(m, i, 1, ri1);
354        rsMatrixSet(m, i, 2, ri2);
355    }
356}
357
358static void __attribute__((overloadable))
359rsMatrixMultiply(rs_matrix3x3 *m, const rs_matrix3x3 *rhs) {
360    rs_matrix3x3 mt;
361    rsMatrixLoadMultiply(&mt, m, rhs);
362    rsMatrixLoad(m, &mt);
363}
364
365static void __attribute__((overloadable))
366rsMatrixLoadMultiply(rs_matrix2x2 *m, const rs_matrix2x2 *lhs, const rs_matrix2x2 *rhs) {
367    for (int i=0 ; i<2 ; i++) {
368        float ri0 = 0;
369        float ri1 = 0;
370        for (int j=0 ; j<2 ; j++) {
371            const float rhs_ij = rsMatrixGet(rhs, i,j);
372            ri0 += rsMatrixGet(lhs, j, 0) * rhs_ij;
373            ri1 += rsMatrixGet(lhs, j, 1) * rhs_ij;
374        }
375        rsMatrixSet(m, i, 0, ri0);
376        rsMatrixSet(m, i, 1, ri1);
377    }
378}
379
380static void __attribute__((overloadable))
381rsMatrixMultiply(rs_matrix2x2 *m, const rs_matrix2x2 *rhs) {
382    rs_matrix2x2 mt;
383    rsMatrixLoadMultiply(&mt, m, rhs);
384    rsMatrixLoad(m, &mt);
385}
386
387static void __attribute__((overloadable))
388rsMatrixRotate(rs_matrix4x4 *m, float rot, float x, float y, float z) {
389    rs_matrix4x4 m1;
390    rsMatrixLoadRotate(&m1, rot, x, y, z);
391    rsMatrixMultiply(m, &m1);
392}
393
394static void __attribute__((overloadable))
395rsMatrixScale(rs_matrix4x4 *m, float x, float y, float z) {
396    rs_matrix4x4 m1;
397    rsMatrixLoadScale(&m1, x, y, z);
398    rsMatrixMultiply(m, &m1);
399}
400
401static void __attribute__((overloadable))
402rsMatrixTranslate(rs_matrix4x4 *m, float x, float y, float z) {
403    rs_matrix4x4 m1;
404    rsMatrixLoadTranslate(&m1, x, y, z);
405    rsMatrixMultiply(m, &m1);
406}
407
408static void __attribute__((overloadable))
409rsMatrixLoadOrtho(rs_matrix4x4 *m, float left, float right, float bottom, float top, float near, float far) {
410    rsMatrixLoadIdentity(m);
411    m->m[0] = 2.f / (right - left);
412    m->m[5] = 2.f / (top - bottom);
413    m->m[10]= -2.f / (far - near);
414    m->m[12]= -(right + left) / (right - left);
415    m->m[13]= -(top + bottom) / (top - bottom);
416    m->m[14]= -(far + near) / (far - near);
417}
418
419static void __attribute__((overloadable))
420rsMatrixLoadFrustum(rs_matrix4x4 *m, float left, float right, float bottom, float top, float near, float far) {
421    rsMatrixLoadIdentity(m);
422    m->m[0] = 2.f * near / (right - left);
423    m->m[5] = 2.f * near / (top - bottom);
424    m->m[8] = (right + left) / (right - left);
425    m->m[9] = (top + bottom) / (top - bottom);
426    m->m[10]= -(far + near) / (far - near);
427    m->m[11]= -1.f;
428    m->m[14]= -2.f * far * near / (far - near);
429    m->m[15]= 0.f;
430}
431
432static float4 __attribute__((overloadable))
433rsMatrixMultiply(rs_matrix4x4 *m, float4 in) {
434    float4 ret;
435    ret.x = (m->m[0] * in.x) + (m->m[4] * in.y) + (m->m[8] * in.z) + (m->m[12] * in.w);
436    ret.y = (m->m[1] * in.x) + (m->m[5] * in.y) + (m->m[9] * in.z) + (m->m[13] * in.w);
437    ret.z = (m->m[2] * in.x) + (m->m[6] * in.y) + (m->m[10] * in.z) + (m->m[14] * in.w);
438    ret.w = (m->m[3] * in.x) + (m->m[7] * in.y) + (m->m[11] * in.z) + (m->m[15] * in.w);
439    return ret;
440}
441
442static float4 __attribute__((overloadable))
443rsMatrixMultiply(rs_matrix4x4 *m, float3 in) {
444    float4 ret;
445    ret.x = (m->m[0] * in.x) + (m->m[4] * in.y) + (m->m[8] * in.z) + m->m[12];
446    ret.y = (m->m[1] * in.x) + (m->m[5] * in.y) + (m->m[9] * in.z) + m->m[13];
447    ret.z = (m->m[2] * in.x) + (m->m[6] * in.y) + (m->m[10] * in.z) + m->m[14];
448    ret.w = (m->m[3] * in.x) + (m->m[7] * in.y) + (m->m[11] * in.z) + m->m[15];
449    return ret;
450}
451
452static float4 __attribute__((overloadable))
453rsMatrixMultiply(rs_matrix4x4 *m, float2 in) {
454    float4 ret;
455    ret.x = (m->m[0] * in.x) + (m->m[4] * in.y) + m->m[12];
456    ret.y = (m->m[1] * in.x) + (m->m[5] * in.y) + m->m[13];
457    ret.z = (m->m[2] * in.x) + (m->m[6] * in.y) + m->m[14];
458    ret.w = (m->m[3] * in.x) + (m->m[7] * in.y) + m->m[15];
459    return ret;
460}
461
462static float3 __attribute__((overloadable))
463rsMatrixMultiply(rs_matrix3x3 *m, float3 in) {
464    float3 ret;
465    ret.x = (m->m[0] * in.x) + (m->m[3] * in.y) + (m->m[6] * in.z);
466    ret.y = (m->m[1] * in.x) + (m->m[4] * in.y) + (m->m[7] * in.z);
467    ret.z = (m->m[2] * in.x) + (m->m[5] * in.y) + (m->m[8] * in.z);
468    return ret;
469}
470
471static float3 __attribute__((overloadable))
472rsMatrixMultiply(rs_matrix3x3 *m, float2 in) {
473    float3 ret;
474    ret.x = (m->m[0] * in.x) + (m->m[3] * in.y);
475    ret.y = (m->m[1] * in.x) + (m->m[4] * in.y);
476    ret.z = (m->m[2] * in.x) + (m->m[5] * in.y);
477    return ret;
478}
479
480static float2 __attribute__((overloadable))
481rsMatrixMultiply(rs_matrix2x2 *m, float2 in) {
482    float2 ret;
483    ret.x = (m->m[0] * in.x) + (m->m[2] * in.y);
484    ret.y = (m->m[1] * in.x) + (m->m[3] * in.y);
485    return ret;
486}
487
488// Returns true if the matrix was successfully inversed
489static bool __attribute__((overloadable))
490rsMatrixInverse(rs_matrix4x4 *m) {
491    rs_matrix4x4 result;
492
493    int i, j;
494    for (i = 0; i < 4; ++i) {
495        for (j = 0; j < 4; ++j) {
496            // computeCofactor for int i, int j
497            int c0 = (i+1) % 4;
498            int c1 = (i+2) % 4;
499            int c2 = (i+3) % 4;
500            int r0 = (j+1) % 4;
501            int r1 = (j+2) % 4;
502            int r2 = (j+3) % 4;
503
504            float minor = (m->m[c0 + 4*r0] * (m->m[c1 + 4*r1] * m->m[c2 + 4*r2] - m->m[c1 + 4*r2] * m->m[c2 + 4*r1]))
505                         - (m->m[c0 + 4*r1] * (m->m[c1 + 4*r0] * m->m[c2 + 4*r2] - m->m[c1 + 4*r2] * m->m[c2 + 4*r0]))
506                         + (m->m[c0 + 4*r2] * (m->m[c1 + 4*r0] * m->m[c2 + 4*r1] - m->m[c1 + 4*r1] * m->m[c2 + 4*r0]));
507
508            float cofactor = (i+j) & 1 ? -minor : minor;
509
510            result.m[4*i + j] = cofactor;
511        }
512    }
513
514    // Dot product of 0th column of source and 0th row of result
515    float det = m->m[0]*result.m[0] + m->m[4]*result.m[1] +
516                 m->m[8]*result.m[2] + m->m[12]*result.m[3];
517
518    if (fabs(det) < 1e-6) {
519        return false;
520    }
521
522    det = 1.0f / det;
523    for (i = 0; i < 16; ++i) {
524        m->m[i] = result.m[i] * det;
525    }
526
527    return true;
528}
529
530// Returns true if the matrix was successfully inversed
531static bool __attribute__((overloadable))
532rsMatrixInverseTranspose(rs_matrix4x4 *m) {
533    rs_matrix4x4 result;
534
535    int i, j;
536    for (i = 0; i < 4; ++i) {
537        for (j = 0; j < 4; ++j) {
538            // computeCofactor for int i, int j
539            int c0 = (i+1) % 4;
540            int c1 = (i+2) % 4;
541            int c2 = (i+3) % 4;
542            int r0 = (j+1) % 4;
543            int r1 = (j+2) % 4;
544            int r2 = (j+3) % 4;
545
546            float minor = (m->m[c0 + 4*r0] * (m->m[c1 + 4*r1] * m->m[c2 + 4*r2] - m->m[c1 + 4*r2] * m->m[c2 + 4*r1]))
547                         - (m->m[c0 + 4*r1] * (m->m[c1 + 4*r0] * m->m[c2 + 4*r2] - m->m[c1 + 4*r2] * m->m[c2 + 4*r0]))
548                         + (m->m[c0 + 4*r2] * (m->m[c1 + 4*r0] * m->m[c2 + 4*r1] - m->m[c1 + 4*r1] * m->m[c2 + 4*r0]));
549
550            float cofactor = (i+j) & 1 ? -minor : minor;
551
552            result.m[4*j + i] = cofactor;
553        }
554    }
555
556    // Dot product of 0th column of source and 0th column of result
557    float det = m->m[0]*result.m[0] + m->m[4]*result.m[4] +
558                 m->m[8]*result.m[8] + m->m[12]*result.m[12];
559
560    if (fabs(det) < 1e-6) {
561        return false;
562    }
563
564    det = 1.0f / det;
565    for (i = 0; i < 16; ++i) {
566        m->m[i] = result.m[i] * det;
567    }
568
569    return true;
570}
571
572static void __attribute__((overloadable))
573rsMatrixTranspose(rs_matrix4x4 *m) {
574    int i, j;
575    float temp;
576    for (i = 0; i < 3; ++i) {
577        for (j = i + 1; j < 4; ++j) {
578            temp = m->m[i*4 + j];
579            m->m[i*4 + j] = m->m[j*4 + i];
580            m->m[j*4 + i] = temp;
581        }
582    }
583}
584
585static void __attribute__((overloadable))
586rsMatrixTranspose(rs_matrix3x3 *m) {
587    int i, j;
588    float temp;
589    for (i = 0; i < 2; ++i) {
590        for (j = i + 1; j < 3; ++j) {
591            temp = m->m[i*3 + j];
592            m->m[i*3 + j] = m->m[j*4 + i];
593            m->m[j*3 + i] = temp;
594        }
595    }
596}
597
598static void __attribute__((overloadable))
599rsMatrixTranspose(rs_matrix2x2 *m) {
600    float temp = m->m[1];
601    m->m[1] = m->m[2];
602    m->m[2] = temp;
603}
604
605/////////////////////////////////////////////////////
606// quaternion ops
607/////////////////////////////////////////////////////
608
609static void __attribute__((overloadable))
610rsQuaternionSet(rs_quaternion *q, float w, float x, float y, float z) {
611    q->w = w;
612    q->x = x;
613    q->y = y;
614    q->z = z;
615}
616
617static void __attribute__((overloadable))
618rsQuaternionSet(rs_quaternion *q, const rs_quaternion *rhs) {
619    q->w = rhs->w;
620    q->x = rhs->x;
621    q->y = rhs->y;
622    q->z = rhs->z;
623}
624
625static void __attribute__((overloadable))
626rsQuaternionMultiply(rs_quaternion *q, float s) {
627    q->w *= s;
628    q->x *= s;
629    q->y *= s;
630    q->z *= s;
631}
632
633static void __attribute__((overloadable))
634rsQuaternionMultiply(rs_quaternion *q, const rs_quaternion *rhs) {
635    q->w = -q->x*rhs->x - q->y*rhs->y - q->z*rhs->z + q->w*rhs->w;
636    q->x =  q->x*rhs->w + q->y*rhs->z - q->z*rhs->y + q->w*rhs->x;
637    q->y = -q->x*rhs->z + q->y*rhs->w + q->z*rhs->z + q->w*rhs->y;
638    q->z =  q->x*rhs->y - q->y*rhs->x + q->z*rhs->w + q->w*rhs->z;
639}
640
641static void
642rsQuaternionAdd(rs_quaternion *q, const rs_quaternion *rhs) {
643    q->w *= rhs->w;
644    q->x *= rhs->x;
645    q->y *= rhs->y;
646    q->z *= rhs->z;
647}
648
649static void
650rsQuaternionLoadRotateUnit(rs_quaternion *q, float rot, float x, float y, float z) {
651    rot *= (float)(M_PI / 180.0f) * 0.5f;
652    float c = cos(rot);
653    float s = sin(rot);
654
655    q->w = c;
656    q->x = x * s;
657    q->y = y * s;
658    q->z = z * s;
659}
660
661static void
662rsQuaternionLoadRotate(rs_quaternion *q, float rot, float x, float y, float z) {
663    const float len = x*x + y*y + z*z;
664    if (len != 1) {
665        const float recipLen = 1.f / sqrt(len);
666        x *= recipLen;
667        y *= recipLen;
668        z *= recipLen;
669    }
670    rsQuaternionLoadRotateUnit(q, rot, x, y, z);
671}
672
673static void
674rsQuaternionConjugate(rs_quaternion *q) {
675    q->x = -q->x;
676    q->y = -q->y;
677    q->z = -q->z;
678}
679
680static float
681rsQuaternionDot(const rs_quaternion *q0, const rs_quaternion *q1) {
682    return q0->w*q1->w + q0->x*q1->x + q0->y*q1->y + q0->z*q1->z;
683}
684
685static void
686rsQuaternionNormalize(rs_quaternion *q) {
687    const float len = rsQuaternionDot(q, q);
688    if (len != 1) {
689        const float recipLen = 1.f / sqrt(len);
690        rsQuaternionMultiply(q, recipLen);
691    }
692}
693
694static void
695rsQuaternionSlerp(rs_quaternion *q, const rs_quaternion *q0, const rs_quaternion *q1, float t) {
696    if(t <= 0.0f) {
697        rsQuaternionSet(q, q0);
698        return;
699    }
700    if(t >= 1.0f) {
701        rsQuaternionSet(q, q1);
702        return;
703    }
704
705    rs_quaternion tempq0, tempq1;
706    rsQuaternionSet(&tempq0, q0);
707    rsQuaternionSet(&tempq1, q1);
708
709    float angle = rsQuaternionDot(q0, q1);
710    if(angle < 0) {
711        rsQuaternionMultiply(&tempq0, -1.0f);
712        angle *= -1.0f;
713    }
714
715    float scale, invScale;
716    if (angle + 1.0f > 0.05f) {
717        if (1.0f - angle >= 0.05f) {
718            float theta = acos(angle);
719            float invSinTheta = 1.0f / sin(theta);
720            scale = sin(theta * (1.0f - t)) * invSinTheta;
721            invScale = sin(theta * t) * invSinTheta;
722        }
723        else {
724            scale = 1.0f - t;
725            invScale = t;
726        }
727    }
728    else {
729        rsQuaternionSet(&tempq1, tempq0.z, -tempq0.y, tempq0.x, -tempq0.w);
730        scale = sin(M_PI * (0.5f - t));
731        invScale = sin(M_PI * t);
732    }
733
734    rsQuaternionSet(q, tempq0.w*scale + tempq1.w*invScale, tempq0.x*scale + tempq1.x*invScale,
735                        tempq0.y*scale + tempq1.y*invScale, tempq0.z*scale + tempq1.z*invScale);
736}
737
738static void rsQuaternionGetMatrixUnit(rs_matrix4x4 *m, const rs_quaternion *q) {
739    float x2 = 2.0f * q->x * q->x;
740    float y2 = 2.0f * q->y * q->y;
741    float z2 = 2.0f * q->z * q->z;
742    float xy = 2.0f * q->x * q->y;
743    float wz = 2.0f * q->w * q->z;
744    float xz = 2.0f * q->x * q->z;
745    float wy = 2.0f * q->w * q->y;
746    float wx = 2.0f * q->w * q->x;
747    float yz = 2.0f * q->y * q->z;
748
749    m->m[0] = 1.0f - y2 - z2;
750    m->m[1] = xy - wz;
751    m->m[2] = xz + wy;
752    m->m[3] = 0.0f;
753
754    m->m[4] = xy + wz;
755    m->m[5] = 1.0f - x2 - z2;
756    m->m[6] = yz - wx;
757    m->m[7] = 0.0f;
758
759    m->m[8] = xz - wy;
760    m->m[9] = yz - wx;
761    m->m[10] = 1.0f - x2 - y2;
762    m->m[11] = 0.0f;
763
764    m->m[12] = 0.0f;
765    m->m[13] = 0.0f;
766    m->m[14] = 0.0f;
767    m->m[15] = 1.0f;
768}
769
770/////////////////////////////////////////////////////
771// utility funcs
772/////////////////////////////////////////////////////
773void __attribute__((overloadable))
774rsExtractFrustumPlanes(const rs_matrix4x4 *modelViewProj,
775                         float4 *left, float4 *right,
776                         float4 *top, float4 *bottom,
777                         float4 *near, float4 *far) {
778    // x y z w = a b c d in the plane equation
779    left->x = modelViewProj->m[3] + modelViewProj->m[0];
780    left->y = modelViewProj->m[7] + modelViewProj->m[4];
781    left->z = modelViewProj->m[11] + modelViewProj->m[8];
782    left->w = modelViewProj->m[15] + modelViewProj->m[12];
783
784    right->x = modelViewProj->m[3] - modelViewProj->m[0];
785    right->y = modelViewProj->m[7] - modelViewProj->m[4];
786    right->z = modelViewProj->m[11] - modelViewProj->m[8];
787    right->w = modelViewProj->m[15] - modelViewProj->m[12];
788
789    top->x = modelViewProj->m[3] - modelViewProj->m[1];
790    top->y = modelViewProj->m[7] - modelViewProj->m[5];
791    top->z = modelViewProj->m[11] - modelViewProj->m[9];
792    top->w = modelViewProj->m[15] - modelViewProj->m[13];
793
794    bottom->x = modelViewProj->m[3] + modelViewProj->m[1];
795    bottom->y = modelViewProj->m[7] + modelViewProj->m[5];
796    bottom->z = modelViewProj->m[11] + modelViewProj->m[9];
797    bottom->w = modelViewProj->m[15] + modelViewProj->m[13];
798
799    near->x = modelViewProj->m[3] + modelViewProj->m[2];
800    near->y = modelViewProj->m[7] + modelViewProj->m[6];
801    near->z = modelViewProj->m[11] + modelViewProj->m[10];
802    near->w = modelViewProj->m[15] + modelViewProj->m[14];
803
804    far->x = modelViewProj->m[3] - modelViewProj->m[2];
805    far->y = modelViewProj->m[7] - modelViewProj->m[6];
806    far->z = modelViewProj->m[11] - modelViewProj->m[10];
807    far->w = modelViewProj->m[15] - modelViewProj->m[14];
808
809    float len = length(left->xyz);
810    *left /= len;
811    len = length(right->xyz);
812    *right /= len;
813    len = length(top->xyz);
814    *top /= len;
815    len = length(bottom->xyz);
816    *bottom /= len;
817    len = length(near->xyz);
818    *near /= len;
819    len = length(far->xyz);
820    *far /= len;
821}
822
823bool __attribute__((overloadable))
824rsIsSphereInFrustum(float4 *sphere,
825                      float4 *left, float4 *right,
826                      float4 *top, float4 *bottom,
827                      float4 *near, float4 *far) {
828
829    float distToCenter = dot(left->xyz, sphere->xyz) + left->w;
830    if(distToCenter < -sphere->w) {
831        return false;
832    }
833    distToCenter = dot(right->xyz, sphere->xyz) + right->w;
834    if(distToCenter < -sphere->w) {
835        return false;
836    }
837    distToCenter = dot(top->xyz, sphere->xyz) + top->w;
838    if(distToCenter < -sphere->w) {
839        return false;
840    }
841    distToCenter = dot(bottom->xyz, sphere->xyz) + bottom->w;
842    if(distToCenter < -sphere->w) {
843        return false;
844    }
845    distToCenter = dot(near->xyz, sphere->xyz) + near->w;
846    if(distToCenter < -sphere->w) {
847        return false;
848    }
849    distToCenter = dot(far->xyz, sphere->xyz) + far->w;
850    if(distToCenter < -sphere->w) {
851        return false;
852    }
853    return true;
854}
855
856
857/////////////////////////////////////////////////////
858// int ops
859/////////////////////////////////////////////////////
860
861__inline__ static uint __attribute__((overloadable, always_inline)) rsClamp(uint amount, uint low, uint high) {
862    return amount < low ? low : (amount > high ? high : amount);
863}
864__inline__ static int __attribute__((overloadable, always_inline)) rsClamp(int amount, int low, int high) {
865    return amount < low ? low : (amount > high ? high : amount);
866}
867__inline__ static ushort __attribute__((overloadable, always_inline)) rsClamp(ushort amount, ushort low, ushort high) {
868    return amount < low ? low : (amount > high ? high : amount);
869}
870__inline__ static short __attribute__((overloadable, always_inline)) rsClamp(short amount, short low, short high) {
871    return amount < low ? low : (amount > high ? high : amount);
872}
873__inline__ static uchar __attribute__((overloadable, always_inline)) rsClamp(uchar amount, uchar low, uchar high) {
874    return amount < low ? low : (amount > high ? high : amount);
875}
876__inline__ static char __attribute__((overloadable, always_inline)) rsClamp(char amount, char low, char high) {
877    return amount < low ? low : (amount > high ? high : amount);
878}
879
880
881
882#endif
883
884