Float Quaternion Propagate
From OpenLuna
qpropw(float *q,float *w,float *qnew)
/* propagate the rotation qutonian forward */ /* in time through the small angles in w */ /* qnew = q dot r, where w = rad displ */ /* r=[Ac,as*wx,as*wy,as*wz] ac=cos(w/2) */ /* as = sin(w/2)/w, w in radians */ /* */ /* need to eliminate sin and cos func */ /* ac= 1-mag/2^2 / 2! +mag/2^4/4! - ... */ /* 2*as= 1-mag/2^2 / 3! + mag/2 ^4/5!... */ /* to get rid of sin and sqrt */ { float r[4],ac,as,wmag,w2d2; w2d2=(w[0]*w[0]+w[1]*w[1]+w[2]*w[2]+w[3]*w[3])/4.0; wmag=sqrt(4.0*w2d2); ac=cos(wmag/2); as=sin(wmag/2)/wmag; r[0]=ac; r[1]=as*w[1]; r[2]=as*w[2]; r[3]=as*w[3]; qdotq(q,r,qnew); } /* end of qpropw */