git.mutantstargoat.com Git - gph-cmath/blob - src/cgmvec3.inl

   1  /*
   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_vsub(cgm_vec3 *a, const cgm_vec3 *b)
  24 {
  25         a->x -= b->x;
  26         a->y -= b->y;
  27         a->z -= b->z;
  28 }
  29
  30 static inline void cgm_vmul(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_vscale(cgm_vec3 *v, float s)
  38 {
  39         v->x *= s;
  40         v->y *= s;
  41         v->z *= s;
  42 }
  43
  44 static inline void cgm_vmul_m4v3(cgm_vec3 *v, const float *m)
  45 {
  46         float x = v->x * m[0] + v->y * m[4] + v->z * m[8] + m[12];
  47         float y = v->x * m[1] + v->y * m[5] + v->z * m[9] + m[13];
  48         v->z = v->x * m[2] + v->y * m[6] + v->z * m[10] + m[14];
  49         v->x = x;
  50         v->y = y;
  51 }
  52
  53 static inline void cgm_vmul_v3m4(cgm_vec3 *v, const float *m)
  54 {
  55         float x = v->x * m[0] + v->y * m[1] + v->z * m[2] + m[3];
  56         float y = v->x * m[4] + v->y * m[5] + v->z * m[6] + m[7];
  57         v->z = v->x * m[8] + v->y * m[9] + v->z * m[10] + m[11];
  58         v->x = x;
  59         v->y = y;
  60 }
  61
  62 static inline void cgm_vmul_m3v3(cgm_vec3 *v, const float *m)
  63 {
  64         float x = v->x * m[0] + v->y * m[4] + v->z * m[8];
  65         float y = v->x * m[1] + v->y * m[5] + v->z * m[9];
  66         v->z = v->x * m[2] + v->y * m[6] + v->z * m[10];
  67         v->x = x;
  68         v->y = y;
  69 }
  70
  71 static inline void cgm_vmul_v3m3(cgm_vec3 *v, const float *m)
  72 {
  73         float x = v->x * m[0] + v->y * m[1] + v->z * m[2];
  74         float y = v->x * m[4] + v->y * m[5] + v->z * m[6];
  75         v->z = v->x * m[8] + v->y * m[9] + v->z * m[10];
  76         v->x = x;
  77         v->y = y;
  78 }
  79
  80 static inline float cgm_vdot(const cgm_vec3 *a, const cgm_vec3 *b)
  81 {
  82         return a->x * b->x + a->y * b->y + a->z * b->z;
  83 }
  84
  85 static inline void cgm_vcross(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b)
  86 {
  87         res->x = a->y * b->z - a->z * b->y;
  88         res->y = a->z * b->x - a->x * b->z;
  89         res->z = a->x * b->y - a->y * b->x;
  90 }
  91
  92 static inline float cgm_vlength(const cgm_vec3 *v)
  93 {
  94         return sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
  95 }
  96
  97 static inline float cgm_vlength_sq(const cgm_vec3 *v)
  98 {
  99         return v->x * v->x + v->y * v->y + v->z * v->z;
 100 }
 101
 102 static inline float cgm_vdist(const cgm_vec3 *a, const cgm_vec3 *b)
 103 {
 104         float dx = a->x - b->x;
 105         float dy = a->y - b->y;
 106         float dz = a->z - b->z;
 107         return sqrt(dx * dx + dy * dy + dz * dz);
 108 }
 109
 110 static inline float cgm_vdist_sq(const cgm_vec3 *a, const cgm_vec3 *b)
 111 {
 112         float dx = a->x - b->x;
 113         float dy = a->y - b->y;
 114         float dz = a->z - b->z;
 115         return dx * dx + dy * dy + dz * dz;
 116 }
 117
 118 static inline void cgm_vnormalize(cgm_vec3 *v)
 119 {
 120         float len = cgm_vlength(v);
 121         if(len != 0.0f) {
 122                 float s = 1.0f / len;
 123                 v->x *= s;
 124                 v->y *= s;
 125                 v->z *= s;
 126         }
 127 }
 128
 129 static inline void cgm_vreflect(cgm_vec3 *v, const cgm_vec3 *n)
 130 {
 131         float ndotv2 = cgm_vdot(v, n) * 2.0f;
 132         v->x -= n->x * ndotv2;
 133         v->y -= n->y * ndotv2;
 134         v->z -= n->z * ndotv2;
 135 }
 136
 137 static inline void cgm_vrefract(cgm_vec3 *v, const cgm_vec3 *n, float ior)
 138 {
 139         float ndotv = cgm_vdot(v, n);
 140         float k = 1.0f - ior * ior * (1.0f - ndotv * ndotv);
 141         if(k < 0.0f) {
 142                 cgm_vreflect(v, n);     /* TIR */
 143         } else {
 144                 float sqrt_k = sqrt(k);
 145                 v->x = ior * v->x - (ior * ndotv + sqrt_k) * n->x;
 146                 v->y = ior * v->y - (ior * ndotv + sqrt_k) * n->y;
 147                 v->z = ior * v->z - (ior * ndotv + sqrt_k) * n->z;
 148         }
 149 }
 150
 151 static inline void cgm_vrotate_quat(cgm_vec3 *v, const cgm_quat *q)
 152 {
 153         cgm_quat vq, inv_q = *q, tmp_q = *q;
 154
 155         cgm_qcons(&vq, v->x, v->y, v->z, 0.0f);
 156         cgm_qinvert(&inv_q);
 157         cgm_qmul(&tmp_q, &vq);
 158         cgm_qmul(&vq, &inv_q);
 159         cgm_vcons(v, vq.x, vq.y, vq.z);
 160 }
 161
 162 static inline void cgm_vrotate_axis(cgm_vec3 *v, int axis, float angle)
 163 {
 164         float m[16];
 165         cgm_mrotation_axis(m, axis, angle);
 166         cgm_vmul_m3v3(v, m);
 167 }
 168
 169 static inline void cgm_vrotate(cgm_vec3 *v, float angle, float x, float y, float z)
 170 {
 171         float m[16];
 172         cgm_mrotation(m, angle, x, y, z);
 173         cgm_vmul_m3v3(v, m);
 174 }
 175
 176 static inline void cgm_vrotate_euler(cgm_vec3 *v, float a, float b, float c, enum cgm_euler_mode mode)
 177 {
 178         float m[16];
 179         cgm_mrotation_euler(m, a, b, c, mode);
 180         cgm_vmul_m3v3(v, m);
 181 }
 182
 183 static inline void cgm_vlerp(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b, float t)
 184 {
 185         res->x = a->x * (b->x - a->x) * t;
 186         res->y = a->y * (b->y - a->y) * t;
 187         res->z = a->z * (b->z - a->z) * t;
 188 }