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