diff options
author | rsc <devnull@localhost> | 2005-01-04 21:23:01 +0000 |
---|---|---|
committer | rsc <devnull@localhost> | 2005-01-04 21:23:01 +0000 |
commit | d1e9002f81f14fbfef1ebc4261edccd9eb97b72c (patch) | |
tree | 50d409a15e719b7860472b49e0f91ac24fcaf127 /src/libgeometry/quaternion.c | |
parent | 46f79934b79ef526ed42bbe5a565e6b5d884d24a (diff) | |
download | plan9port-d1e9002f81f14fbfef1ebc4261edccd9eb97b72c.tar.gz plan9port-d1e9002f81f14fbfef1ebc4261edccd9eb97b72c.tar.bz2 plan9port-d1e9002f81f14fbfef1ebc4261edccd9eb97b72c.zip |
3D geometry
Diffstat (limited to 'src/libgeometry/quaternion.c')
-rw-r--r-- | src/libgeometry/quaternion.c | 242 |
1 files changed, 242 insertions, 0 deletions
diff --git a/src/libgeometry/quaternion.c b/src/libgeometry/quaternion.c new file mode 100644 index 00000000..1f920f5a --- /dev/null +++ b/src/libgeometry/quaternion.c @@ -0,0 +1,242 @@ +/* + * 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); +} |