src/lib/math_quaternion.c

   1 /************************************************************************
   2 * math_quaternion.c
   3 * voxelands - 3d voxel world sandbox game
   4 * Copyright (C) Lisa 'darkrose' Milne 2016 <lisa@ltmnet.com>
   5 *
   6 * This program is free software: you can redistribute it and/or modify
   7 * it under the terms of the GNU General Public License as published by
   8 * the Free Software Foundation, either version 3 of the License, or
   9 * (at your option) any later version.
  10 *
  11 * This program is distributed in the hope that it will be useful, but
  12 * WITHOUT ANY WARRANTY; without even the implied warranty of
  13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  14 * See the GNU General Public License for more details.
  15 *
  16 * You should have received a copy of the GNU General Public License
  17 * along with this program.  If not, see <http://www.gnu.org/licenses/>
  18 ************************************************************************/
  19
  20 #include "common.h"
  21
  22 #include <math.h>
  23 #include <stdlib.h>
  24 #include <stdio.h>
  25
  26 /* create a new quaternion */
  27 quaternion_t *quat_create_quat(float x, float y, float z, float w)
  28 {
  29         quaternion_t *q = malloc(sizeof(quaternion_t));
  30
  31         quat_init_quat(q,x,y,z,w);
  32
  33         return q;
  34 }
  35
  36 /* create a new quaternion from euler values */
  37 quaternion_t *quat_create_euler(float x, float y, float z)
  38 {
  39         quaternion_t *q = malloc(sizeof(quaternion_t));
  40
  41         quat_init_euler(q,x,y,z);
  42
  43         return q;
  44
  45 }
  46
  47 /* create a new quaternion from axis values */
  48 quaternion_t *quat_create_axis(v3_t *v, float angle)
  49 {
  50         quaternion_t *q = malloc(sizeof(quaternion_t));
  51
  52         quat_init_axis(q,v,angle);
  53
  54         return q;
  55
  56 }
  57
  58 /* init a quaternion */
  59 void quat_init_quat(quaternion_t *q, float x, float y, float z, float w)
  60 {
  61         q->x = x;
  62         q->y = y;
  63         q->z = z;
  64
  65         q->w = w;
  66
  67         quat_normalise(q);
  68 }
  69
  70 /* init a quaternion from euler values */
  71 void quat_init_euler(quaternion_t *q, float x, float y, float z)
  72 {
  73         float poa = M_PI/180.0;
  74         float rx = y * poa / 2.0;
  75         float ry = z * poa / 2.0;
  76         float rz = x * poa / 2.0;
  77
  78         float sinp = sin(rx);
  79         float siny = sin(ry);
  80         float sinr = sin(rz);
  81         float cosp = cos(rx);
  82         float cosy = cos(ry);
  83         float cosr = cos(rz);
  84
  85         q->x = sinr * cosp * cosy - cosr * sinp * siny;
  86         q->y = cosr * sinp * cosy + sinr * cosp * siny;
  87         q->z = cosr * cosp * siny - sinr * sinp * cosy;
  88         q->w = cosr * cosp * cosy + sinr * sinp * siny;
  89
  90         quat_normalise(q);
  91 }
  92
  93 /* init a quaternion from axis values */
  94 void quat_init_axis(quaternion_t *q, v3_t *v, float angle)
  95 {
  96         float rad;
  97         float scale;
  98
  99         if (sqrt((v->x*v->x)+(v->y*v->y)+(v->z*v->z)) == 0.0) {
 100                 q->w = 1.0f;
 101                 q->x = 0.0f;
 102                 q->y = 0.0f;
 103                 q->z = 0.0f;
 104
 105                 return;
 106         }
 107
 108         rad = angle / 2.0;
 109         q->w = cosf(rad);
 110
 111         scale = sinf(rad);
 112
 113         q->x = (v->x*scale);
 114         q->y = (v->y*scale);
 115         q->z = (v->z*scale);
 116
 117         quat_normalise(q);
 118 }
 119
 120 /* normalise a quaternion */
 121 void quat_normalise(quaternion_t *q)
 122 {
 123         float mag = sqrt((q->x*q->x)+(q->y*q->y)+(q->z*q->z)+(q->w*q->w));
 124
 125         /* Check for bogus length, to protect against divide by zero */
 126         if (mag != 0.0) {
 127                 q->w /= mag;
 128                 q->x /= mag;
 129                 q->y /= mag;
 130                 q->z /= mag;
 131         }
 132 }
 133
 134 /* get axis values from a quaternion */
 135 void quat_get_axis(quaternion_t *q, v3_t *v, float *angle)
 136 {
 137         double scale;
 138
 139         *angle = acos(q->w)*2.0;
 140
 141         scale = sqrt((q->x*q->x)+(q->y*q->y)+(q->z*q->z));
 142
 143         if (!scale) {
 144                 *angle = 0.0f;
 145
 146                 v->x = 0.0f;
 147                 v->y = 0.0f;
 148                 v->z = 1.0f;
 149         }else{
 150                 v->x = q->x/scale;
 151                 v->y = q->y/scale;
 152                 v->z = q->z/scale;
 153         }
 154
 155         vect_normalise(v);
 156 }
 157
 158 /* multiply a quaternion by a vector */
 159 void quat_multiply_vector(quaternion_t *q, quaternion_t *rq, v3_t *v)
 160 {
 161         rq->w = -((q->x*v->x)-(q->y*v->y)-(q->z*v->z));
 162         rq->x = (q->w*v->x)+(q->y*v->z)-(q->z*v->y);
 163         rq->y = (q->w*v->y)+(q->z*v->x)-(q->x*v->z);
 164         rq->z = (q->w*v->z)+(q->x*v->y)-(q->y*v->x);
 165 }
 166
 167 /* multiply a quaternion by a quaternion */
 168 void quat_multiply(quaternion_t *q1, quaternion_t *q2, quaternion_t *rq)
 169 {
 170         rq->w = (q1->w*q2->w)-(q1->x*q2->x)-(q1->y*q2->y)-(q1->z*q2->z);
 171         rq->x = (q1->w*q2->x)+(q1->x*q2->w)+(q1->y*q2->z)-(q1->z*q2->y);
 172         rq->y = (q1->w*q2->y)+(q1->y*q2->w)+(q1->z*q2->x)-(q1->x*q2->z);
 173         rq->z = (q1->w*q2->z)+(q1->z*q2->w)+(q1->x*q2->y)-(q1->y*q2->x);
 174 }
 175
 176 /* print the values of a quaternion */
 177 void quat_print(quaternion_t *q)
 178 {
 179         printf("w:%f, x:%f, y:%f, z:%f",q->w,q->x,q->y,q->z);
 180 }
 181
 182
 183 /* compute the w value of a quaternion */
 184 void quat_computew(quaternion_t *q)
 185 {
 186         float t = 1.0-(q->x*q->x)-(q->y*q->y)-(q->z*q->z);
 187
 188         if (t < 0.0) {
 189                 q->w = 0.0;
 190         }else{
 191                 q->w = -sqrt(t);
 192         }
 193 }
 194
 195 /* get the world position of a vertex, from a joint/weight */
 196 void quat_rotate(quaternion_t *q, v3_t *in, v3_t *out)
 197 {
 198         quaternion_t tmp;
 199         quaternion_t inv;
 200         quaternion_t qin;
 201
 202         inv.x = -q->x;
 203         inv.y = -q->y;
 204         inv.z = -q->z;
 205         inv.w =  q->w;
 206
 207         qin.x = in->x;
 208         qin.y = in->y;
 209         qin.z = in->z;
 210         qin.w = 0;
 211
 212         quat_multiply(q, &qin, &tmp);
 213         quat_multiply(&tmp, &inv, &qin);
 214
 215         out->x = qin.x;
 216         out->y = qin.y;
 217         out->z = qin.z;
 218 }