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

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