git.mutantstargoat.com Git - vrlugburz/blob - libs/cgmath/cgmquat.inl

   1 /* gph-cmath - C graphics math library
   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_qcons(cgm_quat *q, float x, float y, float z, float w)
  10 {
  11         q->x = x;
  12         q->y = y;
  13         q->z = z;
  14         q->w = w;
  15 }
  16
  17
  18 static inline void cgm_qneg(cgm_quat *q)
  19 {
  20         q->x = -q->x;
  21         q->y = -q->y;
  22         q->z = -q->z;
  23         q->w = -q->w;
  24 }
  25
  26 static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b)
  27 {
  28         a->x += b->x;
  29         a->y += b->y;
  30         a->z += b->z;
  31         a->w += b->w;
  32 }
  33
  34 static inline void cgm_qsub(cgm_quat *a, const cgm_quat *b)
  35 {
  36         a->x -= b->x;
  37         a->y -= b->y;
  38         a->z -= b->z;
  39         a->w -= b->w;
  40 }
  41
  42 static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b)
  43 {
  44         float x, y, z, dot;
  45         cgm_vec3 cross;
  46
  47         dot = a->x * b->x + a->y * b->y + a->z * b->z;
  48         cgm_vcross(&cross, (cgm_vec3*)a, (cgm_vec3*)b);
  49
  50         x = a->w * b->x + b->w * a->x + cross.x;
  51         y = a->w * b->y + b->w * a->y + cross.y;
  52         z = a->w * b->z + b->w * a->z + cross.z;
  53         a->w = a->w * b->w - dot;
  54         a->x = x;
  55         a->y = y;
  56         a->z = z;
  57 }
  58
  59 static inline float cgm_qlength(const cgm_quat *q)
  60 {
  61         return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
  62 }
  63
  64 static inline float cgm_qlength_sq(const cgm_quat *q)
  65 {
  66         return q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
  67 }
  68
  69 static inline void cgm_qnormalize(cgm_quat *q)
  70 {
  71         float len = cgm_qlength(q);
  72         if(len != 0.0f) {
  73                 float s = 1.0f / len;
  74                 q->x *= s;
  75                 q->y *= s;
  76                 q->z *= s;
  77                 q->w *= s;
  78         }
  79 }
  80
  81 static inline void cgm_qconjugate(cgm_quat *q)
  82 {
  83         q->x = -q->x;
  84         q->y = -q->y;
  85         q->z = -q->z;
  86 }
  87
  88 static inline void cgm_qinvert(cgm_quat *q)
  89 {
  90         float len_sq = cgm_qlength_sq(q);
  91         cgm_qconjugate(q);
  92         if(len_sq != 0.0f) {
  93                 float s = 1.0f / len_sq;
  94                 q->x *= s;
  95                 q->y *= s;
  96                 q->z *= s;
  97                 q->w *= s;
  98         }
  99 }
 100
 101 static inline void cgm_qrotation(cgm_quat *q, float angle, float x, float y, float z)
 102 {
 103         float hangle = angle * 0.5f;
 104         float sin_ha = sin(hangle);
 105         q->w = cos(hangle);
 106         q->x = x * sin_ha;
 107         q->y = y * sin_ha;
 108         q->z = z * sin_ha;
 109 }
 110
 111 static inline void cgm_qrotate(cgm_quat *q, float angle, float x, float y, float z)
 112 {
 113         cgm_quat qrot;
 114         cgm_qrotation(&qrot, angle, x, y, z);
 115         cgm_qmul(q, &qrot);
 116 }
 117
 118 static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *quat1, const cgm_quat *q2, float t)
 119 {
 120         float angle, dot, a, b, sin_angle;
 121         cgm_quat q1 = *quat1;
 122
 123         dot = quat1->x * q2->x + quat1->y * q2->y + quat1->z * q2->z + quat1->w * q2->w;
 124         if(dot < 0.0f) {
 125                 /* make sure we inteprolate across the shortest arc */
 126                 cgm_qneg(&q1);
 127                 dot = -dot;
 128         }
 129
 130         /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
 131          * floating point imprecisions
 132          */
 133         if(dot < -1.0f) dot = -1.0f;
 134         if(dot > 1.0f) dot = 1.0f;
 135         angle = acos(dot);
 136
 137         sin_angle = sin(angle);
 138         if(sin_angle == 0.0f) {
 139                 /* use linear interpolation to avoid div/zero */
 140                 a = 1.0f;
 141                 b = t;
 142         } else {
 143                 a = sin((1.0f - t) * angle) / sin_angle;
 144                 b = sin(t * angle) / sin_angle;
 145         }
 146
 147         res->x = q1.x * a + q2->x * b;
 148         res->y = q1.y * a + q2->y * b;
 149         res->z = q1.z * a + q2->z * b;
 150         res->w = q1.w * a + q2->w * b;
 151 }
 152
 153 static inline void cgm_qlerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t)
 154 {
 155         res->x = a->x + (b->x - a->x) * t;
 156         res->y = a->y + (b->y - a->y) * t;
 157         res->z = a->z + (b->z - a->z) * t;
 158         res->w = a->w + (b->w - a->w) * t;
 159 }