quaternion.c (5705B)
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 <draw.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; 62 q.r=0.; 63 q.i=0.; 64 q.j=0.; 65 q.k=1.; 66 if((t=.25*(1+mat[0][0]+mat[1][1]+mat[2][2]))>EPS){ 67 q.r=sqrt(t); 68 t=4*q.r; 69 q.i=(mat[1][2]-mat[2][1])/t; 70 q.j=(mat[2][0]-mat[0][2])/t; 71 q.k=(mat[0][1]-mat[1][0])/t; 72 } 73 else if((t=-.5*(mat[1][1]+mat[2][2]))>EPS){ 74 q.i=sqrt(t); 75 t=2*q.i; 76 q.j=mat[0][1]/t; 77 q.k=mat[0][2]/t; 78 } 79 else if((t=.5*(1-mat[2][2]))>EPS){ 80 q.j=sqrt(t); 81 q.k=mat[1][2]/(2*q.j); 82 } 83 return q; 84 #else 85 /* 86 * Transcribed from Ken Shoemake's new code -- not known to work 87 */ 88 /* This algorithm avoids near-zero divides by looking for a large 89 * component -- first r, then i, j, or k. When the trace is greater than zero, 90 * |r| is greater than 1/2, which is as small as a largest component can be. 91 * Otherwise, the largest diagonal entry corresponds to the largest of |i|, 92 * |j|, or |k|, one of which must be larger than |r|, and at least 1/2. 93 */ 94 Quaternion qu; 95 double tr, s; 96 97 tr = mat[0][0] + mat[1][1] + mat[2][2]; 98 if (tr >= 0.0) { 99 s = sqrt(tr + mat[3][3]); 100 qu.r = s*0.5; 101 s = 0.5 / s; 102 qu.i = (mat[2][1] - mat[1][2]) * s; 103 qu.j = (mat[0][2] - mat[2][0]) * s; 104 qu.k = (mat[1][0] - mat[0][1]) * s; 105 } 106 else { 107 int i = 0; 108 if (mat[1][1] > mat[0][0]) i = 1; 109 if (mat[2][2] > mat[i][i]) i = 2; 110 switch(i){ 111 case 0: 112 s = sqrt( (mat[0][0] - (mat[1][1]+mat[2][2])) + mat[3][3] ); 113 qu.i = s*0.5; 114 s = 0.5 / s; 115 qu.j = (mat[0][1] + mat[1][0]) * s; 116 qu.k = (mat[2][0] + mat[0][2]) * s; 117 qu.r = (mat[2][1] - mat[1][2]) * s; 118 break; 119 case 1: 120 s = sqrt( (mat[1][1] - (mat[2][2]+mat[0][0])) + mat[3][3] ); 121 qu.j = s*0.5; 122 s = 0.5 / s; 123 qu.k = (mat[1][2] + mat[2][1]) * s; 124 qu.i = (mat[0][1] + mat[1][0]) * s; 125 qu.r = (mat[0][2] - mat[2][0]) * s; 126 break; 127 case 2: 128 s = sqrt( (mat[2][2] - (mat[0][0]+mat[1][1])) + mat[3][3] ); 129 qu.k = s*0.5; 130 s = 0.5 / s; 131 qu.i = (mat[2][0] + mat[0][2]) * s; 132 qu.j = (mat[1][2] + mat[2][1]) * s; 133 qu.r = (mat[1][0] - mat[0][1]) * s; 134 break; 135 } 136 } 137 if (mat[3][3] != 1.0){ 138 s=1/sqrt(mat[3][3]); 139 qu.r*=s; 140 qu.i*=s; 141 qu.j*=s; 142 qu.k*=s; 143 } 144 return (qu); 145 #endif 146 } 147 Quaternion qadd(Quaternion q, Quaternion r){ 148 q.r+=r.r; 149 q.i+=r.i; 150 q.j+=r.j; 151 q.k+=r.k; 152 return q; 153 } 154 Quaternion qsub(Quaternion q, Quaternion r){ 155 q.r-=r.r; 156 q.i-=r.i; 157 q.j-=r.j; 158 q.k-=r.k; 159 return q; 160 } 161 Quaternion qneg(Quaternion q){ 162 q.r=-q.r; 163 q.i=-q.i; 164 q.j=-q.j; 165 q.k=-q.k; 166 return q; 167 } 168 Quaternion qmul(Quaternion q, Quaternion r){ 169 Quaternion s; 170 s.r=q.r*r.r-q.i*r.i-q.j*r.j-q.k*r.k; 171 s.i=q.r*r.i+r.r*q.i+q.j*r.k-q.k*r.j; 172 s.j=q.r*r.j+r.r*q.j+q.k*r.i-q.i*r.k; 173 s.k=q.r*r.k+r.r*q.k+q.i*r.j-q.j*r.i; 174 return s; 175 } 176 Quaternion qdiv(Quaternion q, Quaternion r){ 177 return qmul(q, qinv(r)); 178 } 179 Quaternion qunit(Quaternion q){ 180 double l=qlen(q); 181 q.r/=l; 182 q.i/=l; 183 q.j/=l; 184 q.k/=l; 185 return q; 186 } 187 /* 188 * Bug?: takes no action on divide check 189 */ 190 Quaternion qinv(Quaternion q){ 191 double l=q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k; 192 q.r/=l; 193 q.i=-q.i/l; 194 q.j=-q.j/l; 195 q.k=-q.k/l; 196 return q; 197 } 198 double qlen(Quaternion p){ 199 return sqrt(p.r*p.r+p.i*p.i+p.j*p.j+p.k*p.k); 200 } 201 Quaternion slerp(Quaternion q, Quaternion r, double a){ 202 double u, v, ang, s; 203 double dot=q.r*r.r+q.i*r.i+q.j*r.j+q.k*r.k; 204 ang=dot<-1?PI:dot>1?0:acos(dot); /* acos gives NaN for dot slightly out of range */ 205 s=sin(ang); 206 if(s==0) return ang<PI/2?q:r; 207 u=sin((1-a)*ang)/s; 208 v=sin(a*ang)/s; 209 q.r=u*q.r+v*r.r; 210 q.i=u*q.i+v*r.i; 211 q.j=u*q.j+v*r.j; 212 q.k=u*q.k+v*r.k; 213 return q; 214 } 215 /* 216 * Only works if qlen(q)==qlen(r)==1 217 */ 218 Quaternion qmid(Quaternion q, Quaternion r){ 219 double l; 220 q=qadd(q, r); 221 l=qlen(q); 222 if(l<1e-12){ 223 q.r=r.i; 224 q.i=-r.r; 225 q.j=r.k; 226 q.k=-r.j; 227 } 228 else{ 229 q.r/=l; 230 q.i/=l; 231 q.j/=l; 232 q.k/=l; 233 } 234 return q; 235 } 236 /* 237 * Only works if qlen(q)==1 238 */ 239 static Quaternion qident={1,0,0,0}; 240 Quaternion qsqrt(Quaternion q){ 241 return qmid(q, qident); 242 }