git.mutantstargoat.com Git - retroray/blob - libs/cgmath/cgmvec3.inl

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