sys/src/libgeometry/quaternion.c

   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 }