1 /* 2 * Quaternion arithmetic: 3 * qadd(q, r) returns q+r 4 * qsub(q, r) returns q-r 5 * qneg(q) returns -q 6 * qmul(q, r) returns q*r 7 * qdiv(q, r) returns q/r, can divide check. 8 * qinv(q) returns 1/q, can divide check. 9 * double qlen(p) returns modulus of p 10 * qunit(q) returns a unit quaternion parallel to q 11 * The following only work on unit quaternions and rotation matrices: 12 * slerp(q, r, a) returns q*(r*q^-1)^a 13 * qmid(q, r) slerp(q, r, .5) 14 * qsqrt(q) qmid(q, (Quaternion){1,0,0,0}) 15 * qtom(m, q) converts a unit quaternion q into a rotation matrix m 16 * mtoq(m) returns a quaternion equivalent to a rotation matrix m 17 */ 18 #include <u.h> 19 #include <libc.h> 20 #include <libg.h> 21 #include <geometry.h> 22 void qtom(Matrix m, Quaternion q){ 23 #ifndef new 24 m[0][0]=1-2*(q.j*q.j+q.k*q.k); 25 m[0][1]=2*(q.i*q.j+q.r*q.k); 26 m[0][2]=2*(q.i*q.k-q.r*q.j); 27 m[0][3]=0; 28 m[1][0]=2*(q.i*q.j-q.r*q.k); 29 m[1][1]=1-2*(q.i*q.i+q.k*q.k); 30 m[1][2]=2*(q.j*q.k+q.r*q.i); 31 m[1][3]=0; 32 m[2][0]=2*(q.i*q.k+q.r*q.j); 33 m[2][1]=2*(q.j*q.k-q.r*q.i); 34 m[2][2]=1-2*(q.i*q.i+q.j*q.j); 35 m[2][3]=0; 36 m[3][0]=0; 37 m[3][1]=0; 38 m[3][2]=0; 39 m[3][3]=1; 40 #else 41 /* 42 * Transcribed from Ken Shoemake's new code -- not known to work 43 */ 44 double Nq = q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k; 45 double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0; 46 double xs = q.i*s, ys = q.j*s, zs = q.k*s; 47 double wx = q.r*xs, wy = q.r*ys, wz = q.r*zs; 48 double xx = q.i*xs, xy = q.i*ys, xz = q.i*zs; 49 double yy = q.j*ys, yz = q.j*zs, zz = q.k*zs; 50 m[0][0] = 1.0 - (yy + zz); m[1][0] = xy + wz; m[2][0] = xz - wy; 51 m[0][1] = xy - wz; m[1][1] = 1.0 - (xx + zz); m[2][1] = yz + wx; 52 m[0][2] = xz + wy; m[1][2] = yz - wx; m[2][2] = 1.0 - (xx + yy); 53 m[0][3] = m[1][3] = m[2][3] = m[3][0] = m[3][1] = m[3][2] = 0.0; 54 m[3][3] = 1.0; 55 #endif 56 } 57 Quaternion mtoq(Matrix mat){ 58 #ifndef new 59 #define EPS 1.387778780781445675529539585113525e-17 /* 2^-56 */ 60 double t; 61 Quaternion q=(Quaternion){0.,0.,0.,1.}; 62 if((t=.25*(1+mat[0][0]+mat[1][1]+mat[2][2]))>EPS){ 63 q.r=sqrt(t); 64 t=4*q.r; 65 q.i=(mat[1][2]-mat[2][1])/t; 66 q.j=(mat[2][0]-mat[0][2])/t; 67 q.k=(mat[0][1]-mat[1][0])/t; 68 } 69 else if((t=-.5*(mat[1][1]+mat[2][2]))>EPS){ 70 q.i=sqrt(t); 71 t=2*q.i; 72 q.j=mat[0][1]/t; 73 q.k=mat[0][2]/t; 74 } 75 else if((t=.5*(1-mat[2][2]))>EPS){ 76 q.j=sqrt(t); 77 q.k=mat[1][2]/(2*q.j); 78 } 79 return q; 80 #else 81 /* 82 * Transcribed from Ken Shoemake's new code -- not known to work 83 */ 84 /* This algorithm avoids near-zero divides by looking for a large 85 * component -- first r, then i, j, or k. When the trace is greater than zero, 86 * |r| is greater than 1/2, which is as small as a largest component can be. 87 * Otherwise, the largest diagonal entry corresponds to the largest of |i|, 88 * |j|, or |k|, one of which must be larger than |r|, and at least 1/2. 89 */ 90 Quaternion qu; 91 double tr, s; 92 93 tr = mat[0][0] + mat[1][1] + mat[2][2]; 94 if (tr >= 0.0) { 95 s = sqrt(tr + mat[3][3]); 96 qu.r = s*0.5; 97 s = 0.5 / s; 98 qu.i = (mat[2][1] - mat[1][2]) * s; 99 qu.j = (mat[0][2] - mat[2][0]) * s; 100 qu.k = (mat[1][0] - mat[0][1]) * s; 101 } 102 else { 103 int i = 0; 104 if (mat[1][1] > mat[0][0]) i = 1; 105 if (mat[2][2] > mat[i][i]) i = 2; 106 switch(i){ 107 case 0: 108 s = sqrt( (mat[0][0] - (mat[1][1]+mat[2][2])) + mat[3][3] ); 109 qu.i = s*0.5; 110 s = 0.5 / s; 111 qu.j = (mat[0][1] + mat[1][0]) * s; 112 qu.k = (mat[2][0] + mat[0][2]) * s; 113 qu.r = (mat[2][1] - mat[1][2]) * s; 114 break; 115 case 1: 116 s = sqrt( (mat[1][1] - (mat[2][2]+mat[0][0])) + mat[3][3] ); 117 qu.j = s*0.5; 118 s = 0.5 / s; 119 qu.k = (mat[1][2] + mat[2][1]) * s; 120 qu.i = (mat[0][1] + mat[1][0]) * s; 121 qu.r = (mat[0][2] - mat[2][0]) * s; 122 break; 123 case 2: 124 s = sqrt( (mat[2][2] - (mat[0][0]+mat[1][1])) + mat[3][3] ); 125 qu.k = s*0.5; 126 s = 0.5 / s; 127 qu.i = (mat[2][0] + mat[0][2]) * s; 128 qu.j = (mat[1][2] + mat[2][1]) * s; 129 qu.r = (mat[1][0] - mat[0][1]) * s; 130 break; 131 } 132 } 133 if (mat[3][3] != 1.0){ 134 s=1/sqrt(mat[3][3]); 135 qu.r*=s; 136 qu.i*=s; 137 qu.j*=s; 138 qu.k*=s; 139 } 140 return (qu); 141 #endif 142 } 143 Quaternion qadd(Quaternion q, Quaternion r){ 144 return (Quaternion){q.r+r.r, q.i+r.i, q.j+r.j, q.k+r.k}; 145 } 146 Quaternion qsub(Quaternion q, Quaternion r){ 147 return (Quaternion){q.r-r.r, q.i-r.i, q.j-r.j, q.k-r.k}; 148 } 149 Quaternion qneg(Quaternion q){ 150 return (Quaternion){-q.r, -q.i, -q.j, -q.k}; 151 } 152 Quaternion qmul(Quaternion q, Quaternion r){ 153 return (Quaternion){q.r*r.r-q.i*r.i-q.j*r.j-q.k*r.k, 154 q.r*r.i+r.r*q.i+q.j*r.k-q.k*r.j, 155 q.r*r.j+r.r*q.j+q.k*r.i-q.i*r.k, 156 q.r*r.k+r.r*q.k+q.i*r.j-q.j*r.i}; 157 } 158 Quaternion qdiv(Quaternion q, Quaternion r){ 159 return qmul(q, qinv(r)); 160 } 161 Quaternion qunit(Quaternion q){ 162 double l=qlen(q); 163 return (Quaternion){q.r/l, q.i/l, q.j/l, q.k/l}; 164 } 165 /* 166 * Bug?: takes no action on divide check 167 */ 168 Quaternion qinv(Quaternion q){ 169 double l=q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k; 170 return (Quaternion){q.r/l, -q.i/l, -q.j/l, -q.k/l}; 171 } 172 double qlen(Quaternion p){ 173 return sqrt(p.r*p.r+p.i*p.i+p.j*p.j+p.k*p.k); 174 } 175 Quaternion slerp(Quaternion q, Quaternion r, double a){ 176 double u, v, ang, s; 177 double dot=q.r*r.r+q.i*r.i+q.j*r.j+q.k*r.k; 178 ang=dot<-1?PI:dot>1?0:acos(dot); /* acos gives NaN for dot slightly out of range */ 179 s=sin(ang); 180 if(s==0) return ang<PI/2?q:r; 181 u=sin((1-a)*ang)/s; 182 v=sin(a*ang)/s; 183 return (Quaternion){u*q.r+v*r.r, u*q.i+v*r.i, u*q.j+v*r.j, u*q.k+v*r.k}; 184 } 185 /* 186 * Only works if qlen(q)==qlen(r)==1 187 */ 188 Quaternion qmid(Quaternion q, Quaternion r){ 189 double l; 190 q=qadd(q, r); 191 l=qlen(q); 192 if(l<1e-12) return (Quaternion){r.i,-r.r,r.k,-r.j}; 193 return (Quaternion){q.r/l, q.i/l, q.j/l, q.k/l}; 194 } 195 /* 196 * Only works if qlen(q)==1 197 */ 198 Quaternion qsqrt(Quaternion q){ 199 return qmid(q, (Quaternion){1,0,0,0}); 200 } 201