/*   from http://www1.acm.org/pubs/tog/GraphicsGems/index.html   This is the official on-line repository for the code from the    Graphics Gems series of books (from Academic Press). This series   focusses on short to medium length pieces of code which perform a    wide variety of computer graphics related tasks. All code   here can be used without restrictions. The code distributions here    contain all known bug fixes and enhancements. We also   provide errata listings for the text of each book. Please report any    new errata or bugs to Eric Haines (erich@acm.org).    http://www1.acm.org/pubs/tog/GraphicsGems/gemsiv/euler_angle/*//**** EulerAngles.c - Convert Euler angles to/from matrix or quat ****//* Ken Shoemake, 1993 */#include <math.h>#include <float.h>#include "stdafx.h"#include "EulerAngles.h"EulerAngles Eul_(float ai, float aj, float ah, int order){    EulerAngles ea;    ea.x = ai; ea.y = aj; ea.z = ah;    ea.w = (float) order;    return (ea);}/* Construct quaternion from Euler angles (in radians). */Quat Eul_ToQuat(EulerAngles ea){    Quat qu;    double a[3], ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;    int i,j,k,h,n,s,f;    EulGetOrd((unsigned) ea.w,i,j,k,h,n,s,f);    if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}    if (n==EulParOdd) ea.y = -ea.y;    ti = ea.x*0.5; tj = ea.y*0.5; th = ea.z*0.5;    ci = cos(ti);  cj = cos(tj);  ch = cos(th);    si = sin(ti);  sj = sin(tj);  sh = sin(th);    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;    if (s==EulRepYes) {	a[i] = cj*(cs + sc);	/* Could speed up with */	a[j] = sj*(cc + ss);	/* trig identities. */	a[k] = sj*(cs - sc);	qu.w = cj*(cc - ss);    } else {	a[i] = cj*sc - sj*cs;	a[j] = cj*ss + sj*cc;	a[k] = cj*cs - sj*sc;	qu.w = cj*cc + sj*ss;    }    if (n==EulParOdd) a[j] = -a[j];    qu.x = a[X]; qu.y = a[Y]; qu.z = a[Z];    return (qu);}/* Construct matrix from Euler angles (in radians). */void Eul_ToHMatrix(EulerAngles ea, HMatrix M){    double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;    int i,j,k,h,n,s,f;    EulGetOrd((unsigned) ea.w,i,j,k,h,n,s,f);    if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}    if (n==EulParOdd) {ea.x = -ea.x; ea.y = -ea.y; ea.z = -ea.z;}    ti = ea.x;	  tj = ea.y;	th = ea.z;    ci = cos(ti); cj = cos(tj); ch = cos(th);    si = sin(ti); sj = sin(tj); sh = sin(th);    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;    if (s==EulRepYes) {	M[i][i] = cj;	  M[i][j] =  sj*si;    M[i][k] =  sj*ci;	M[j][i] = sj*sh;  M[j][j] = -cj*ss+cc; M[j][k] = -cj*cs-sc;	M[k][i] = -sj*ch; M[k][j] =  cj*sc+cs; M[k][k] =  cj*cc-ss;    } else {	M[i][i] = cj*ch; M[i][j] = sj*sc-cs; M[i][k] = sj*cc+ss;	M[j][i] = cj*sh; M[j][j] = sj*ss+cc; M[j][k] = sj*cs-sc;	M[k][i] = -sj;	 M[k][j] = cj*si;    M[k][k] = cj*ci;    }    M[W][X]=M[W][Y]=M[W][Z]=M[X][W]=M[Y][W]=M[Z][W]=0.0; M[W][W]=1.0;}/* Convert matrix to Euler angles (in radians). */EulerAngles Eul_FromHMatrix(HMatrix M, int order){    EulerAngles ea;    int i,j,k,h,n,s,f;    EulGetOrd(order,i,j,k,h,n,s,f);    if (s==EulRepYes) {	double sy = sqrt(M[i][j]*M[i][j] + M[i][k]*M[i][k]);	if (sy > 16*FLT_EPSILON) {	    ea.x = atan2(M[i][j], M[i][k]);	    ea.y = atan2(sy, M[i][i]);	    ea.z = atan2(M[j][i], -M[k][i]);	} else {	    ea.x = atan2(-M[j][k], M[j][j]);	    ea.y = atan2(sy, M[i][i]);	    ea.z = 0;	}    } else {	double cy = sqrt(M[i][i]*M[i][i] + M[j][i]*M[j][i]);	if (cy > 16*FLT_EPSILON) {	    ea.x = atan2(M[k][j], M[k][k]);	    ea.y = atan2(-M[k][i], cy);	    ea.z = atan2(M[j][i], M[i][i]);	} else {	    ea.x = atan2(-M[j][k], M[j][j]);	    ea.y = atan2(-M[k][i], cy);	    ea.z = 0;	}    }    if (n==EulParOdd) {ea.x = -ea.x; ea.y = - ea.y; ea.z = -ea.z;}    if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}    ea.w = (float) order;    return (ea);}/* Convert quaternion to Euler angles (in radians). */EulerAngles Eul_FromQuat(Quat q, int order){    HMatrix M;    double Nq = q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w;    double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;    double xs = q.x*s,	  ys = q.y*s,	 zs = q.z*s;    double wx = q.w*xs,	  wy = q.w*ys,	 wz = q.w*zs;    double xx = q.x*xs,	  xy = q.x*ys,	 xz = q.x*zs;    double yy = q.y*ys,	  yz = q.y*zs,	 zz = q.z*zs;    M[X][X] = 1.0 - (yy + zz); M[X][Y] = xy - wz; M[X][Z] = xz + wy;    M[Y][X] = xy + wz; M[Y][Y] = 1.0 - (xx + zz); M[Y][Z] = yz - wx;    M[Z][X] = xz - wy; M[Z][Y] = yz + wx; M[Z][Z] = 1.0 - (xx + yy);    M[W][X]=M[W][Y]=M[W][Z]=M[X][W]=M[Y][W]=M[Z][W]=0.0; M[W][W]=1.0;    return (Eul_FromHMatrix(M, order));}