/*
* Quaternion arithmetic:
*      qadd(q, r)      returns q+r
*      qsub(q, r)      returns q-r
*      qneg(q)         returns -q
*      qmul(q, r)      returns q*r
*      qdiv(q, r)      returns q/r, can divide check.
*      qinv(q)         returns 1/q, can divide check.
*      double qlen(p)  returns modulus of p
*      qunit(q)        returns a unit quaternion parallel to q
* The following only work on unit quaternions and rotation matrices:
*      slerp(q, r, a)  returns q*(r*q^-1)^a
*      qmid(q, r)      slerp(q, r, .5)
*      qsqrt(q)        qmid(q, (Quaternion){1,0,0,0})
*      qtom(m, q)      converts a unit quaternion q into a rotation matrix m
*      mtoq(m)         returns a quaternion equivalent to a rotation matrix m
*/
#include <u.h>
#include <libc.h>
#include <draw.h>
#include <geometry.h>
void qtom(Matrix m, Quaternion q){
#ifndef new
       m[0][0]=1-2*(q.j*q.j+q.k*q.k);
       m[0][1]=2*(q.i*q.j+q.r*q.k);
       m[0][2]=2*(q.i*q.k-q.r*q.j);
       m[0][3]=0;
       m[1][0]=2*(q.i*q.j-q.r*q.k);
       m[1][1]=1-2*(q.i*q.i+q.k*q.k);
       m[1][2]=2*(q.j*q.k+q.r*q.i);
       m[1][3]=0;
       m[2][0]=2*(q.i*q.k+q.r*q.j);
       m[2][1]=2*(q.j*q.k-q.r*q.i);
       m[2][2]=1-2*(q.i*q.i+q.j*q.j);
       m[2][3]=0;
       m[3][0]=0;
       m[3][1]=0;
       m[3][2]=0;
       m[3][3]=1;
#else
       /*
        * Transcribed from Ken Shoemake's new code -- not known to work
        */
       double Nq = q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k;
       double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
       double xs = q.i*s,              ys = q.j*s,             zs = q.k*s;
       double wx = q.r*xs,             wy = q.r*ys,            wz = q.r*zs;
       double xx = q.i*xs,             xy = q.i*ys,            xz = q.i*zs;
       double yy = q.j*ys,             yz = q.j*zs,            zz = q.k*zs;
       m[0][0] = 1.0 - (yy + zz); m[1][0] = xy + wz;         m[2][0] = xz - wy;
       m[0][1] = xy - wz;         m[1][1] = 1.0 - (xx + zz); m[2][1] = yz + wx;
       m[0][2] = xz + wy;         m[1][2] = yz - wx;         m[2][2] = 1.0 - (xx + yy);
       m[0][3] = m[1][3] = m[2][3] = m[3][0] = m[3][1] = m[3][2] = 0.0;
       m[3][3] = 1.0;
#endif
}
Quaternion mtoq(Matrix mat){
#ifndef new
#define EPS     1.387778780781445675529539585113525e-17 /* 2^-56 */
       double t;
       Quaternion q;
       q.r=0.;
       q.i=0.;
       q.j=0.;
       q.k=1.;
       if((t=.25*(1+mat[0][0]+mat[1][1]+mat[2][2]))>EPS){
               q.r=sqrt(t);
               t=4*q.r;
               q.i=(mat[1][2]-mat[2][1])/t;
               q.j=(mat[2][0]-mat[0][2])/t;
               q.k=(mat[0][1]-mat[1][0])/t;
       }
       else if((t=-.5*(mat[1][1]+mat[2][2]))>EPS){
               q.i=sqrt(t);
               t=2*q.i;
               q.j=mat[0][1]/t;
               q.k=mat[0][2]/t;
       }
       else if((t=.5*(1-mat[2][2]))>EPS){
               q.j=sqrt(t);
               q.k=mat[1][2]/(2*q.j);
       }
       return q;
#else
       /*
        * Transcribed from Ken Shoemake's new code -- not known to work
        */
       /* This algorithm avoids near-zero divides by looking for a large
        * component -- first r, then i, j, or k.  When the trace is greater than zero,
        * |r| is greater than 1/2, which is as small as a largest component can be.
        * Otherwise, the largest diagonal entry corresponds to the largest of |i|,
        * |j|, or |k|, one of which must be larger than |r|, and at least 1/2.
        */
       Quaternion qu;
       double tr, s;

       tr = mat[0][0] + mat[1][1] + mat[2][2];
       if (tr >= 0.0) {
               s = sqrt(tr + mat[3][3]);
               qu.r = s*0.5;
               s = 0.5 / s;
               qu.i = (mat[2][1] - mat[1][2]) * s;
               qu.j = (mat[0][2] - mat[2][0]) * s;
               qu.k = (mat[1][0] - mat[0][1]) * s;
       }
       else {
               int i = 0;
               if (mat[1][1] > mat[0][0]) i = 1;
               if (mat[2][2] > mat[i][i]) i = 2;
               switch(i){
               case 0:
                       s = sqrt( (mat[0][0] - (mat[1][1]+mat[2][2])) + mat[3][3] );
                       qu.i = s*0.5;
                       s = 0.5 / s;
                       qu.j = (mat[0][1] + mat[1][0]) * s;
                       qu.k = (mat[2][0] + mat[0][2]) * s;
                       qu.r = (mat[2][1] - mat[1][2]) * s;
                       break;
               case 1:
                       s = sqrt( (mat[1][1] - (mat[2][2]+mat[0][0])) + mat[3][3] );
                       qu.j = s*0.5;
                       s = 0.5 / s;
                       qu.k = (mat[1][2] + mat[2][1]) * s;
                       qu.i = (mat[0][1] + mat[1][0]) * s;
                       qu.r = (mat[0][2] - mat[2][0]) * s;
                       break;
               case 2:
                       s = sqrt( (mat[2][2] - (mat[0][0]+mat[1][1])) + mat[3][3] );
                       qu.k = s*0.5;
                       s = 0.5 / s;
                       qu.i = (mat[2][0] + mat[0][2]) * s;
                       qu.j = (mat[1][2] + mat[2][1]) * s;
                       qu.r = (mat[1][0] - mat[0][1]) * s;
                       break;
               }
       }
       if (mat[3][3] != 1.0){
               s=1/sqrt(mat[3][3]);
               qu.r*=s;
               qu.i*=s;
               qu.j*=s;
               qu.k*=s;
       }
       return (qu);
#endif
}
Quaternion qadd(Quaternion q, Quaternion r){
       q.r+=r.r;
       q.i+=r.i;
       q.j+=r.j;
       q.k+=r.k;
       return q;
}
Quaternion qsub(Quaternion q, Quaternion r){
       q.r-=r.r;
       q.i-=r.i;
       q.j-=r.j;
       q.k-=r.k;
       return q;
}
Quaternion qneg(Quaternion q){
       q.r=-q.r;
       q.i=-q.i;
       q.j=-q.j;
       q.k=-q.k;
       return q;
}
Quaternion qmul(Quaternion q, Quaternion r){
       Quaternion s;
       s.r=q.r*r.r-q.i*r.i-q.j*r.j-q.k*r.k;
       s.i=q.r*r.i+r.r*q.i+q.j*r.k-q.k*r.j;
       s.j=q.r*r.j+r.r*q.j+q.k*r.i-q.i*r.k;
       s.k=q.r*r.k+r.r*q.k+q.i*r.j-q.j*r.i;
       return s;
}
Quaternion qdiv(Quaternion q, Quaternion r){
       return qmul(q, qinv(r));
}
Quaternion qunit(Quaternion q){
       double l=qlen(q);
       q.r/=l;
       q.i/=l;
       q.j/=l;
       q.k/=l;
       return q;
}
/*
* Bug?: takes no action on divide check
*/
Quaternion qinv(Quaternion q){
       double l=q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k;
       q.r/=l;
       q.i=-q.i/l;
       q.j=-q.j/l;
       q.k=-q.k/l;
       return q;
}
double qlen(Quaternion p){
       return sqrt(p.r*p.r+p.i*p.i+p.j*p.j+p.k*p.k);
}
Quaternion slerp(Quaternion q, Quaternion r, double a){
       double u, v, ang, s;
       double dot=q.r*r.r+q.i*r.i+q.j*r.j+q.k*r.k;
       ang=dot<-1?PI:dot>1?0:acos(dot); /* acos gives NaN for dot slightly out of range */
       s=sin(ang);
       if(s==0) return ang<PI/2?q:r;
       u=sin((1-a)*ang)/s;
       v=sin(a*ang)/s;
       q.r=u*q.r+v*r.r;
       q.i=u*q.i+v*r.i;
       q.j=u*q.j+v*r.j;
       q.k=u*q.k+v*r.k;
       return q;
}
/*
* Only works if qlen(q)==qlen(r)==1
*/
Quaternion qmid(Quaternion q, Quaternion r){
       double l;
       q=qadd(q, r);
       l=qlen(q);
       if(l<1e-12){
               q.r=r.i;
               q.i=-r.r;
               q.j=r.k;
               q.k=-r.j;
       }
       else{
               q.r/=l;
               q.i/=l;
               q.j/=l;
               q.k/=l;
       }
       return q;
}
/*
* Only works if qlen(q)==1
*/
static Quaternion qident={1,0,0,0};
Quaternion qsqrt(Quaternion q){
       return qmid(q, qident);
}

You are viewing proxied material from sdf.org. The copyright of proxied material belongs to its original authors. Any comments or complaints in relation to proxied material should be directed to the original authors of the content concerned. Please see the disclaimer for more details.