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 */