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

   1 static inline void cgm_vzero(cgm_vec3 *v)
   2 {
   3         v->x = v->y = v->z = 0.0f;
   4 }
   5
   6 static inline void cgm_vone(cgm_vec3 *v)
   7 {
   8         v->x = v->y = v->z = 1.0f;
   9 }
  10
  11 static inline void cgm_vadd(cgm_vec3 *a, const cgm_vec3 *b)
  12 {
  13         a->x += b->x;
  14         a->y += b->y;
  15         a->z += b->z;
  16 }
  17
  18 static inline void cgm_vsub(cgm_vec3 *a, const cgm_vec3 *b)
  19 {
  20         a->x -= b->x;
  21         a->y -= b->y;
  22         a->z -= b->z;
  23 }
  24
  25 static inline void cgm_vmul(cgm_vec3 *a, const cgm_vec3 *b)
  26 {
  27         a->x *= b->x;
  28         a->y *= b->y;
  29         a->z *= b->z;
  30 }
  31
  32 static inline void cgm_vscale(cgm_vec3 *v, float s)
  33 {
  34         v->x *= s;
  35         v->y *= s;
  36         v->z *= s;
  37 }
  38
  39 static inline float cgm_vdot(const cgm_vec3 *a, const cgm_vec3 *b)
  40 {
  41         return a->x * b->x + a->y * b->y + a->z * b->z;
  42 }
  43
  44 static inline void cgm_vcross(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b)
  45 {
  46         res->x = a->y * b->z - a->z * b->y;
  47         res->y = a->z * b->x - a->x * b->z;
  48         res->z = a->x * b->y - a->y * b->x;
  49 }
  50
  51 static inline float cgm_vlength(const cgm_vec3 *v)
  52 {
  53         return sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
  54 }
  55
  56 static inline float cgm_vlength_sq(const cgm_vec3 *v)
  57 {
  58         return v->x * v->x + v->y * v->y + v->z * v->z;
  59 }
  60
  61 static inline float cgm_vdist(const cgm_vec3 *a, const cgm_vec3 *b)
  62 {
  63         float dx = a->x - b->x;
  64         float dy = a->y - b->y;
  65         float dz = a->z - b->z;
  66         return sqrt(dx * dx + dy * dy + dz * dz);
  67 }
  68
  69 static inline float cgm_vdist_sq(const cgm_vec3 *a, const cgm_vec3 *b)
  70 {
  71         float dx = a->x - b->x;
  72         float dy = a->y - b->y;
  73         float dz = a->z - b->z;
  74         return dx * dx + dy * dy + dz * dz;
  75 }
  76
  77 static inline void cgm_vnormalize(cgm_vec3 *v)
  78 {
  79         float len = cgm_vlength(v);
  80         if(len != 0.0f) {
  81                 float s = 1.0f / len;
  82                 v->x *= s;
  83                 v->y *= s;
  84                 v->z *= s;
  85         }
  86 }
  87
  88 static inline void cgm_qident(cgm_quat *q)
  89 {
  90         q->x = q->y = q->z = 0.0f;
  91         q->w = 1.0f;
  92 }
  93
  94 static inline void cgm_qneg(cgm_quat *q)
  95 {
  96         q->x = -q->x;
  97         q->y = -q->y;
  98         q->z = -q->z;
  99         q->w = -q->w;
 100 }
 101
 102 static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b)
 103 {
 104         a->x += b->x;
 105         a->y += b->y;
 106         a->z += b->z;
 107         a->w += b->w;
 108 }
 109
 110 static inline void cgm_qsub(cgm_quat *a, const cgm_quat *b)
 111 {
 112         a->x -= b->x;
 113         a->y -= b->y;
 114         a->z -= b->z;
 115         a->w -= b->w;
 116 }
 117
 118 static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b)
 119 {
 120         float axb_x = a->y * b->z - a->z * b->y;
 121         float axb_y = a->z * b->x - a->x * b->z;
 122         float axb_z = a->x * b->y - a->y * b->x;
 123
 124         float im_x = a->w * b->x + b->w * a->x + axb_x;
 125         float im_y = a->w * b->y + b->w * a->y + axb_y;
 126         float im_z = a->w * b->z + b->w * a->z + axb_z;
 127
 128         a->w = a->w * b->w - (a->x * b->x + a->y * b->y + a->z * b->z);
 129         a->x = im_x;
 130         a->y = im_y;
 131         a->z = im_z;
 132 }
 133
 134 static inline float cgm_qlength(const cgm_quat *q)
 135 {
 136         return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
 137 }
 138
 139 static inline float cgm_qlength_sq(const cgm_quat *q)
 140 {
 141         return q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
 142 }
 143
 144 static inline void cgm_qnormalize(cgm_quat *q)
 145 {
 146         float len = cgm_qlength(q);
 147         if(len != 0.0f) {
 148                 float inv_len = 1.0f / len;
 149                 q->x *= inv_len;
 150                 q->y *= inv_len;
 151                 q->z *= inv_len;
 152                 q->w *= inv_len;
 153         }
 154 }
 155
 156 static inline void cgm_qconjugate(cgm_quat *q)
 157 {
 158         q->x = -q->x;
 159         q->y = -q->y;
 160         q->z = -q->z;
 161 }
 162
 163 static inline void cgm_qinvert(cgm_quat *q)
 164 {
 165         cgm_qconjugate(q);
 166         float len_sq = cgm_qlength_sq(q);
 167         if(len_sq != 0.0) {
 168                 float s = 1.0f / len_sq;
 169                 q->x *= s;
 170                 q->y *= s;
 171                 q->z *= s;
 172                 q->w *= s;
 173         }
 174 }
 175
 176 static inline void cgm_qrot_axis(cgm_quat *q, float angle, float x, float y, float z)
 177 {
 178         float half_angle = angle * 0.5f;
 179         float sin_ha = sin(half_angle);
 180         q->w = cos(half_angle);
 181         q->x = x * sin_ha;
 182         q->y = y * sin_ha;
 183         q->z = z * sin_ha;
 184 }
 185
 186 static inline void cgm_qrot_quat(cgm_quat *q, const cgm_quat *rq)
 187 {
 188         cgm_quat qrot = *rq;
 189         cgm_quat rqconj = *rq;
 190         cgm_qconjugate(&rqconj);
 191         cgm_qmul(&qrot, q);
 192         cgm_qmul(&qrot, &rqconj);
 193         *q = qrot;
 194 }
 195
 196 static inline void cgm_qmatrix(float *mat, const cgm_quat *q)
 197 {
 198         float xsq2 = 2.0f * q->x * q->x;
 199         float ysq2 = 2.0f * q->y * q->y;
 200         float zsq2 = 2.0f * q->z * q->z;
 201         float sx = 1.0f - ysq2 - zsq2;
 202         float sy = 1.0f - xsq2 - zsq2;
 203         float sz = 1.0f - xsq2 - ysq2;
 204
 205         mat[0] = sx;
 206         mat[1] = 2.0f * q->x * q->y + 2.0f * q->w * q->z;
 207         mat[2] = 2.0f * q->z * q->x - 2.0f * q->w * q->y;
 208         mat[3] = 0.0f;
 209
 210         mat[4] = 2.0f * q->x * q->y - 2.0f * q->w * q->z;
 211         mat[5] = sy;
 212         mat[6] = 2.0f * q->y * q->z + 2.0f * q->w * q->x;
 213         mat[7] = 0.0f;
 214
 215         mat[8] = 2.0f * q->z * q->x + 2.0f * q->w * q->y;
 216         mat[9] = 2.0f * q->y * q->z - 2.0f * q->w * q->x;
 217         mat[10] = sz;
 218         mat[11] = 0.0f;
 219
 220         mat[12] = mat[13] = mat[14] = 0.0f;
 221         mat[15] = 1.0f;
 222 }
 223
 224 static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t)
 225 {
 226         cgm_quat qa = *a;
 227         float dot = a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w;
 228         float angle, sin_angle, ta, tb;
 229
 230         if(dot < 0.0f) {
 231                 /* make sure we interpolate across the shortest arc */
 232                 qa.x = -qa.x;
 233                 qa.y = -qa.y;
 234                 qa.z = -qa.z;
 235                 qa.w = -qa.w;
 236                 dot = -dot;
 237         }
 238
 239         /* clamp dot to [-1, 1] in order to avoid domain errors in acos */
 240         if(dot < -1.0f) dot = -1.0f;
 241         if(dot > 1.0f) dot = 1.0f;
 242         angle = acos(dot);
 243         sin_angle = sin(angle);
 244
 245         if(sin_angle == 0.0f) {
 246                 /* use linear interpolation to avoid div/zero */
 247                 ta = 1.0f - t;
 248                 tb = t;
 249         } else {
 250                 ta = sin((1.0f - t) * angle) / sin_angle;
 251                 tb = sin(t * angle) / sin_angle;
 252         }
 253
 254         res->x = a->x * ta + b->x * tb;
 255         res->y = a->y * ta + b->y * tb;
 256         res->z = a->z * ta + b->z * tb;
 257         res->w = a->w * ta + b->w * tb;
 258 }