initial commit
[deeprace] / libs / cgmath / cgmvec3.inl
1 /* gph-cmath - C graphics math library
2  * Copyright (C) 2018 John Tsiombikas <nuclear@member.fsf.org>
3  *
4  * This program is free software. Feel free to use, modify, and/or redistribute
5  * it under the terms of the MIT/X11 license. See LICENSE for details.
6  * If you intend to redistribute parts of the code without the LICENSE file
7  * replace this paragraph with the full contents of the LICENSE file.
8  */
9 static inline void cgm_vcons(cgm_vec3 *v, float x, float y, float z)
10 {
11         v->x = x;
12         v->y = y;
13         v->z = z;
14 }
15
16 static inline void cgm_vadd(cgm_vec3 *a, const cgm_vec3 *b)
17 {
18         a->x += b->x;
19         a->y += b->y;
20         a->z += b->z;
21 }
22
23 static inline void cgm_vadd_scaled(cgm_vec3 *a, const cgm_vec3 *b, float s)
24 {
25         a->x += b->x * s;
26         a->y += b->y * s;
27         a->z += b->z * s;
28 }
29
30 static inline void cgm_vsub(cgm_vec3 *a, const cgm_vec3 *b)
31 {
32         a->x -= b->x;
33         a->y -= b->y;
34         a->z -= b->z;
35 }
36
37 static inline void cgm_vsub_scaled(cgm_vec3 *a, const cgm_vec3 *b, float s)
38 {
39         a->x -= b->x * s;
40         a->y -= b->y * s;
41         a->z -= b->z * s;
42 }
43
44 static inline void cgm_vmul(cgm_vec3 *a, const cgm_vec3 *b)
45 {
46         a->x *= b->x;
47         a->y *= b->y;
48         a->z *= b->z;
49 }
50
51 static inline void cgm_vscale(cgm_vec3 *v, float s)
52 {
53         v->x *= s;
54         v->y *= s;
55         v->z *= s;
56 }
57
58 static inline void cgm_vmul_m4v3(cgm_vec3 *v, const float *m)
59 {
60         float x = v->x * m[0] + v->y * m[4] + v->z * m[8] + m[12];
61         float y = v->x * m[1] + v->y * m[5] + v->z * m[9] + m[13];
62         v->z = v->x * m[2] + v->y * m[6] + v->z * m[10] + m[14];
63         v->x = x;
64         v->y = y;
65 }
66
67 static inline void cgm_vmul_v3m4(cgm_vec3 *v, const float *m)
68 {
69         float x = v->x * m[0] + v->y * m[1] + v->z * m[2] + m[3];
70         float y = v->x * m[4] + v->y * m[5] + v->z * m[6] + m[7];
71         v->z = v->x * m[8] + v->y * m[9] + v->z * m[10] + m[11];
72         v->x = x;
73         v->y = y;
74 }
75
76 static inline void cgm_vmul_m3v3(cgm_vec3 *v, const float *m)
77 {
78         float x = v->x * m[0] + v->y * m[4] + v->z * m[8];
79         float y = v->x * m[1] + v->y * m[5] + v->z * m[9];
80         v->z = v->x * m[2] + v->y * m[6] + v->z * m[10];
81         v->x = x;
82         v->y = y;
83 }
84
85 static inline void cgm_vmul_v3m3(cgm_vec3 *v, const float *m)
86 {
87         float x = v->x * m[0] + v->y * m[1] + v->z * m[2];
88         float y = v->x * m[4] + v->y * m[5] + v->z * m[6];
89         v->z = v->x * m[8] + v->y * m[9] + v->z * m[10];
90         v->x = x;
91         v->y = y;
92 }
93
94 static inline float cgm_vdot(const cgm_vec3 *a, const cgm_vec3 *b)
95 {
96         return a->x * b->x + a->y * b->y + a->z * b->z;
97 }
98
99 static inline void cgm_vcross(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b)
100 {
101         res->x = a->y * b->z - a->z * b->y;
102         res->y = a->z * b->x - a->x * b->z;
103         res->z = a->x * b->y - a->y * b->x;
104 }
105
106 static inline float cgm_vlength(const cgm_vec3 *v)
107 {
108         return sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
109 }
110
111 static inline float cgm_vlength_sq(const cgm_vec3 *v)
112 {
113         return v->x * v->x + v->y * v->y + v->z * v->z;
114 }
115
116 static inline float cgm_vdist(const cgm_vec3 *a, const cgm_vec3 *b)
117 {
118         float dx = a->x - b->x;
119         float dy = a->y - b->y;
120         float dz = a->z - b->z;
121         return sqrt(dx * dx + dy * dy + dz * dz);
122 }
123
124 static inline float cgm_vdist_sq(const cgm_vec3 *a, const cgm_vec3 *b)
125 {
126         float dx = a->x - b->x;
127         float dy = a->y - b->y;
128         float dz = a->z - b->z;
129         return dx * dx + dy * dy + dz * dz;
130 }
131
132 static inline void cgm_vnormalize(cgm_vec3 *v)
133 {
134         float len = cgm_vlength(v);
135         if(len != 0.0f) {
136                 float s = 1.0f / len;
137                 v->x *= s;
138                 v->y *= s;
139                 v->z *= s;
140         }
141 }
142
143 static inline void cgm_vreflect(cgm_vec3 *v, const cgm_vec3 *n)
144 {
145         float ndotv2 = cgm_vdot(v, n) * 2.0f;
146         v->x -= n->x * ndotv2;
147         v->y -= n->y * ndotv2;
148         v->z -= n->z * ndotv2;
149 }
150
151 static inline void cgm_vrefract(cgm_vec3 *v, const cgm_vec3 *n, float ior)
152 {
153         float ndotv = cgm_vdot(v, n);
154         float k = 1.0f - ior * ior * (1.0f - ndotv * ndotv);
155         if(k < 0.0f) {
156                 cgm_vreflect(v, n);     /* TIR */
157         } else {
158                 float sqrt_k = sqrt(k);
159                 v->x = ior * v->x - (ior * ndotv + sqrt_k) * n->x;
160                 v->y = ior * v->y - (ior * ndotv + sqrt_k) * n->y;
161                 v->z = ior * v->z - (ior * ndotv + sqrt_k) * n->z;
162         }
163 }
164
165 static inline void cgm_vrotate_quat(cgm_vec3 *v, const cgm_quat *q)
166 {
167         cgm_quat vq, inv_q = *q, tmp_q = *q;
168
169         cgm_qcons(&vq, v->x, v->y, v->z, 0.0f);
170         cgm_qinvert(&inv_q);
171         cgm_qmul(&tmp_q, &vq);
172         cgm_qmul(&tmp_q, &inv_q);
173         cgm_vcons(v, tmp_q.x, tmp_q.y, tmp_q.z);
174 }
175
176 static inline void cgm_vrotate_axis(cgm_vec3 *v, int axis, float angle)
177 {
178         float m[16];
179         cgm_mrotation_axis(m, axis, angle);
180         cgm_vmul_m3v3(v, m);
181 }
182
183 static inline void cgm_vrotate(cgm_vec3 *v, float angle, float x, float y, float z)
184 {
185         float m[16];
186         cgm_mrotation(m, angle, x, y, z);
187         cgm_vmul_m3v3(v, m);
188 }
189
190 static inline void cgm_vrotate_euler(cgm_vec3 *v, float a, float b, float c, enum cgm_euler_mode mode)
191 {
192         float m[16];
193         cgm_mrotation_euler(m, a, b, c, mode);
194         cgm_vmul_m3v3(v, m);
195 }
196
197 static inline void cgm_vlerp(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b, float t)
198 {
199         res->x = a->x + (b->x - a->x) * t;
200         res->y = a->y + (b->y - a->y) * t;
201         res->z = a->z + (b->z - a->z) * t;
202 }