foo
[vrlugburz] / libs / cgmath / cgmmisc.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 #include <stdlib.h>
10
11 static inline float cgm_deg_to_rad(float deg)
12 {
13         return M_PI * deg / 180.0f;
14 }
15
16 static inline float cgm_rad_to_deg(float rad)
17 {
18         return 180.0f * rad / M_PI;
19 }
20
21 static inline float cgm_smoothstep(float a, float b, float x)
22 {
23         if(x < a) return 0.0f;
24         if(x >= b) return 1.0f;
25
26         x = (x - a) / (b - a);
27         return x * x * (3.0f - 2.0f * x);
28 }
29
30 static inline float cgm_lerp(float a, float b, float t)
31 {
32         return a + (b - a) * t;
33 }
34
35 static inline float cgm_bezier(float a, float b, float c, float d, float t)
36 {
37         float omt, omt3, t3, f;
38         t3 = t * t * t;
39         omt = 1.0f - t;
40         omt3 = omt * omt * omt;
41         f = 3.0f * t * omt;
42
43         return (a * omt3) + (b * f * omt) + (c * f * t) + (d * t3);
44 }
45
46 static inline float cgm_bspline(float a, float b, float c, float d, float t)
47 {
48         static const float mat[] = {
49                 -1, 3, -3, 1,
50                 3, -6, 0, 4,
51                 -3, 3, 3, 1,
52                 1, 0, 0, 0
53         };
54         cgm_vec4 tmp, qfact;
55         float tsq = t * t;
56
57         cgm_wcons(&qfact, tsq * t, tsq, t, 1.0f);
58         cgm_wcons(&tmp, a, b, c, d);
59         cgm_wmul_m4v4(&tmp, mat);
60         cgm_wscale(&tmp, 1.0f / 6.0f);
61         return cgm_wdot(&tmp, &qfact);
62 }
63
64 static inline float cgm_spline(float a, float b, float c, float d, float t)
65 {
66         static const float mat[] = {
67                 -1, 2, -1, 0,
68                 3, -5, 0, 2,
69                 -3, 4, 1, 0,
70                 1, -1, 0, 0
71         };
72         cgm_vec4 tmp, qfact;
73         float tsq = t * t;
74
75         cgm_wcons(&qfact, tsq * t, tsq, t, 1.0f);
76         cgm_wcons(&tmp, a, b, c, d);
77         cgm_wmul_m4v4(&tmp, mat);
78         cgm_wscale(&tmp, 1.0f / 6.0f);
79         return cgm_wdot(&tmp, &qfact);
80 }
81
82 static inline void cgm_discrand(cgm_vec3 *pt, float rad)
83 {
84         float theta = 2.0f * M_PI * (float)rand() / RAND_MAX;
85         float r = sqrt((float)rand() / RAND_MAX) * rad;
86         pt->x = cos(theta) * r;
87         pt->y = sin(theta) * r;
88         pt->z = 0.0f;
89 }
90
91 static inline void cgm_sphrand(cgm_vec3 *pt, float rad)
92 {
93         float u, v, theta, phi;
94
95         u = (float)rand() / RAND_MAX;
96         v = (float)rand() / RAND_MAX;
97
98         theta = 2.0f * M_PI * u;
99         phi = acos(2.0f * v - 1.0f);
100
101         pt->x = cos(theta) * sin(phi) * rad;
102         pt->y = sin(theta) * sin(phi) * rad;
103         pt->z = cos(phi) * rad;
104 }
105
106 static inline void cgm_unproject(cgm_vec3 *res, const cgm_vec3 *norm_scrpos,
107                 const float *inv_viewproj)
108 {
109         cgm_vec4 pos;
110
111         pos.x = 2.0f * norm_scrpos->x - 1.0f;
112         pos.y = 2.0f * norm_scrpos->y - 1.0f;
113         pos.z = 2.0f * norm_scrpos->z - 1.0f;
114         pos.w = 1.0f;
115
116         cgm_wmul_m4v4(&pos, inv_viewproj);
117
118         res->x = pos.x / pos.w;
119         res->y = pos.y / pos.w;
120         res->z = pos.z / pos.w;
121 }
122
123 static inline void cgm_glu_unproject(float winx, float winy, float winz,
124                 const float *view, const float *proj, const int *vp,
125                 float *objx, float *objy, float *objz)
126 {
127         cgm_vec3 npos, res;
128         float inv_pv[16];
129
130         cgm_mcopy(inv_pv, proj);
131         cgm_mmul(inv_pv, view);
132
133         npos.x = (winx - vp[0]) / vp[2];
134         npos.y = (winy - vp[1]) / vp[4];
135         npos.z = winz;
136
137         cgm_unproject(&res, &npos, inv_pv);
138
139         *objx = res.x;
140         *objy = res.y;
141         *objz = res.z;
142 }
143
144 static inline void cgm_pick_ray(cgm_ray *ray, float nx, float ny,
145                 const float *viewmat, const float *projmat)
146 {
147         cgm_vec3 npos, farpt;
148         float inv_pv[16];
149
150         cgm_mcopy(inv_pv, projmat);
151         cgm_mmul(inv_pv, viewmat);
152
153         cgm_vcons(&npos, nx, ny, 0.0f);
154         cgm_unproject(&ray->origin, &npos, inv_pv);
155         npos.z = 1.0f;
156         cgm_unproject(&farpt, &npos, inv_pv);
157
158         ray->dir.x = farpt.x - ray->origin.x;
159         ray->dir.y = farpt.y - ray->origin.y;
160         ray->dir.z = farpt.z - ray->origin.z;
161 }
162
163 static inline void cgm_raypos(cgm_vec3 *p, const cgm_ray *ray, float t)
164 {
165         p->x = ray->origin.x + ray->dir.x * t;
166         p->y = ray->origin.y + ray->dir.y * t;
167         p->z = ray->origin.z + ray->dir.z * t;
168 }
169
170 static inline void cgm_bary(cgm_vec3 *bary, const cgm_vec3 *a,
171                 const cgm_vec3 *b, const cgm_vec3 *c, const cgm_vec3 *pt)
172 {
173         float d00, d01, d11, d20, d21, denom;
174         cgm_vec3 v0 = *b, v1 = *c, v2 = *pt;
175
176         cgm_vsub(&v0, a);
177         cgm_vsub(&v1, a);
178         cgm_vsub(&v2, a);
179
180         d00 = cgm_vdot(&v0, &v0);
181         d01 = cgm_vdot(&v0, &v1);
182         d11 = cgm_vdot(&v1, &v1);
183         d20 = cgm_vdot(&v2, &v0);
184         d21 = cgm_vdot(&v2, &v1);
185         denom = d00 * d11 - d01 * d01;
186
187         bary->y = (d11 * d20 - d01 * d21) / denom;
188         bary->z = (d00 * d21 - d01 * d20) / denom;
189         bary->x = 1.0f - bary->y - bary->z;
190 }
191
192 static inline void cgm_uvec_to_sph(float *theta, float *phi, const cgm_vec3 *v)
193 {
194         *theta = atan2(v->z, v->x);
195         *phi = acos(v->y);
196 }
197
198 static inline void cgm_sph_to_uvec(cgm_vec3 *v, float theta, float phi)
199 {
200         v->x = sin(theta) * cos(phi);
201         v->y = sin(phi);
202         v->z = cos(theta) * cos(phi);
203 }