USE rbmat
TYPE QUATERNION=STRUCTURED ARRAY(REAL,IMAG,JMAG,KMAG) OF REAL
INLINE QUATERNION FUNCTION CONJG(QUATERNION x)
RESULT.REAL=x.REAL
RESULT.IMAG=-x.IMAG
RESULT.JMAG=-x.JMAG
RESULT.KMAG=-x.KMAG
END CONJG
QUATERNION FUNCTION qmult(QUATERNION a,b)
RESULT.REAL=a.REAL∗b.REAL-a.IMAG∗b.IMAG-a.JMAG∗b.JMAG-a.KMAG∗b.KMAG
RESULT.IMAG=a.IMAG∗b.REAL+a.REAL∗b.IMAG+a.JMAG∗b.KMAG-a.KMAG∗b.JMAG
RESULT.JMAG=a.JMAG∗b.REAL+a.REAL∗b.JMAG+a.KMAG∗b.IMAG-a.IMAG∗b.KMAG
RESULT.KMAG=a.KMAG∗b.REAL+a.REAL∗b.KMAG+a.IMAG∗b.JMAG-a.JMAG∗b.IMAG
END qmult
INLINE QUATERNION FUNCTION INV(QUATERNION x)=CONJG(x)/NORM(x)
OPERATOR QUATERNION x ∗ QUATERNION y = qmult(x,y)
OPERATOR QUATERNION x / QUATERNION y = qmult(x,INV(y))
OPERATOR REAL x / QUATERNION y = x∗INV(y)
QUATERNION FUNCTION EXP(QUATERNION x)
REAL m=EXP(x.REAL)
s=SQRT(x.IMAG^2+x.JMAG^2+x.KMAG^2)
RESULT.REAL=m∗COS(s)
IF s>1E-8 THEN m=m∗SIN(s)/s
RESULT.IMAG=m∗x.IMAG
RESULT.JMAG=m∗x.JMAG
RESULT.KMAG=m∗x.KMAG
END EXP
QUATERNION FUNCTION LOG(QUATERNION x)
REAL m=x.IMAG^2+x.JMAG^2+x.KMAG^2
RESULT.REAL=0.5∗LOG(m+x.REAL^2)
IF m>1E-16 THEN m=atan2(SQRT(m),x.REAL)/SQRT(m) ELSE m=1/x.REAL
RESULT.IMAG=m∗x.IMAG
RESULT.JMAG=m∗x.JMAG
RESULT.KMAG=m∗x.KMAG
END LOG
INLINE QUATERNION FUNCTION POWER(QUATERNION x; REAL y)=EXP[LOG(x)∗y]
ARRAY(1..3,1..3) OF REAL FUNCTION rotation(QUATERNION x)
u=x/ABS(x)
RESULT(1,1)=1-2∗(u.JMAG^2+u.KMAG^2)
RESULT(1,2)=2∗(u.IMAG∗u.JMAG-u.REAL∗u.KMAG)
RESULT(1,3)=2∗(u.IMAG∗u.KMAG+u.REAL∗u.JMAG)
RESULT(2,1)=2∗(u.IMAG∗u.JMAG+u.REAL∗u.KMAG)
RESULT(2,2)=1-2∗(u.IMAG^2+u.KMAG^2)
RESULT(2,3)=2∗(u.JMAG∗u.KMAG-u.REAL∗u.IMAG)
RESULT(3,1)=2∗(u.IMAG∗u.KMAG-u.REAL∗u.JMAG)
RESULT(3,2)=2∗(u.JMAG∗u.KMAG+u.REAL∗u.IMAG)
RESULT(3,3)=1-2∗(u.IMAG^2+u.JMAG^2)
END rotation