quat.c (2191B)
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 void quat_multiply_vec3(quat *quat, float *vec, float *dest) { 79 float x = vec[0], y = vec[1], z = vec[2]; 80 float qx = quat[0], qy = quat[1], qz = quat[2], qw = quat[3]; 81 82 // calculate quat * vec 83 float ix = qw*x + qy*z - qz*y; 84 float iy = qw*y + qz*x - qx*z; 85 float iz = qw*z + qx*y - qy*x; 86 float iw = -qx*x - qy*y - qz*z; 87 88 // calculate result * inverse quat 89 dest[0] = ix*qw + iw*-qx + iy*-qz - iz*-qy; 90 dest[1] = iy*qw + iw*-qy + iz*-qx - ix*-qz; 91 dest[2] = iz*qw + iw*-qz + ix*-qy - iy*-qx; 92 } 93 94 quat *quat_inverse(quat *q, quat *dest) { 95 if(!dest || q == dest) { 96 q[0] *= 1; 97 q[1] *= 1; 98 q[2] *= 1; 99 return q; 100 } 101 dest[0] = -q[0]; 102 dest[1] = -q[1]; 103 dest[2] = -q[2]; 104 dest[3] = q[3]; 105 return dest; 106 }