polyadvent

A game engine from scratch in C
git clone git://jb55.com/polyadvent
Log | Files | Refs | README

quat.c (2253B)


      1 
      2 #include "quat.h"
      3 #include <math.h>
      4 
      5 void quat_id(quat *dest) {
      6   dest[0] = 0;
      7   dest[1] = 0;
      8   dest[2] = 0;
      9   dest[3] = 1;
     10 }
     11 
     12 void quat_multiply(quat *a, quat *b, quat *dest) {
     13   float ax = a[0];
     14   float ay = a[1];
     15   float az = a[2];
     16   float aw = a[3];
     17 
     18   float bx = b[0];
     19   float by = b[1];
     20   float bz = b[2];
     21   float bw = b[3];
     22 
     23   dest[0] = ax * bw + aw * bx + ay * bz - az * by;
     24   dest[1] = ay * bw + aw * by + az * bx - ax * bz;
     25   dest[2] = az * bw + aw * bz + ax * by - ay * bx;
     26   dest[3] = aw * bw - ax * bx - ay * by - az * bz;
     27 }
     28 
     29 /* void quat_from_vec3(float *vec3, quat *dest) { */
     30 /*     float angle = atan2(vec3[0], vec3[2]); */
     31 /*     dest[0] = 0; */
     32 /*     dest[1] = 1 * sin(angle/2); */
     33 /* } */
     34 
     35 void quat_axis_angle(float *axis, float angle, quat *dest) {
     36   float half_angle = angle / 2.0;
     37   float s = sin(half_angle);
     38 
     39   dest[0] = axis[0] * s;
     40   dest[1] = axis[1] * s;
     41   dest[2] = axis[2] * s;
     42   dest[3] = cos(half_angle);
     43 }
     44 
     45 void quat_to_mat3(quat *quat, float *dest) {
     46 	float x = quat[0], y = quat[1], z = quat[2], w = quat[3];
     47 
     48 	float x2 = x + x;
     49 	float y2 = y + y;
     50 	float z2 = z + z;
     51 
     52 	float xx = x*x2;
     53 	float xy = x*y2;
     54 	float xz = x*z2;
     55 
     56 	float yy = y*y2;
     57 	float yz = y*z2;
     58 	float zz = z*z2;
     59 
     60 	float wx = w*x2;
     61 	float wy = w*y2;
     62 	float wz = w*z2;
     63 
     64 	dest[0] = 1.0 - (yy + zz);
     65 	dest[1] = xy - wz;
     66 	dest[2] = xz + wy;
     67 
     68 	dest[3] = xy + wz;
     69 	dest[4] = 1.0 - (xx + zz);
     70 	dest[5] = yz - wx;
     71 
     72 	dest[6] = xz - wy;
     73 	dest[7] = yz + wx;
     74 	dest[8] = 1.0 - (xx + yy);
     75 }
     76 
     77 
     78 float quat_yaw(quat *q) {
     79     return atan2(q[2],q[3])*2.0;
     80 }
     81 
     82 void quat_multiply_vec3(quat *quat, float *vec, float *dest) {
     83 	float x  = vec[0], y = vec[1], z = vec[2];
     84 	float qx = quat[0], qy = quat[1], qz = quat[2], qw = quat[3];
     85 
     86 	// calculate quat * vec
     87 	float ix = qw*x + qy*z - qz*y;
     88 	float iy = qw*y + qz*x - qx*z;
     89 	float iz = qw*z + qx*y - qy*x;
     90 	float iw = -qx*x - qy*y - qz*z;
     91 
     92 	// calculate result * inverse quat
     93 	dest[0] = ix*qw + iw*-qx + iy*-qz - iz*-qy;
     94 	dest[1] = iy*qw + iw*-qy + iz*-qx - ix*-qz;
     95 	dest[2] = iz*qw + iw*-qz + ix*-qy - iy*-qx;
     96 }
     97 
     98 quat *quat_inverse(quat *q, quat *dest) {
     99 	if(!dest || q == dest) {
    100 		q[0] *= 1;
    101 		q[1] *= 1;
    102 		q[2] *= 1;
    103 		return q;
    104 	}
    105 	dest[0] = -q[0];
    106 	dest[1] = -q[1];
    107 	dest[2] = -q[2];
    108 	dest[3] = q[3];
    109 	return dest;
    110 }