git.mutantstargoat.com Git - nexus3d/blob - src/cgmath2/cgmvec3.inl

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