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