/* Convert matrix to Euler angles (in radians). */
EulerAngles Eul_FromMatrix(LTMatrix& 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) {
float sy = (float)sqrt(M.El(i,j)*M.El(i,j) + M.El(i,k)*M.El(i,k));
if (sy > 16*FLT_EPSILON) {
ea.x = (float)atan2(M.El(i,j), M.El(i,k));
ea.y = (float)atan2(sy, M.El(i,i));
ea.z = (float)atan2(M.El(j,i), -M.El(k,i));
} else {
ea.x = (float)atan2(-M.El(j,k), M.El(j,j));
ea.y = (float)atan2(sy, M.El(i,i));
ea.z = 0;
}
} else {
float cy = (float)sqrt(M.El(i,i)*M.El(i,i) + M.El(j,i)*M.El(j,i));
if (cy > 16*FLT_EPSILON) {
ea.x = (float)atan2(M.El(k,j), M.El(k,k));
ea.y = (float)atan2(-M.El(k,i), cy);
ea.z = (float)atan2(M.El(j,i), M.El(i,i));
} else {
ea.x = (float)atan2(-M.El(j,k), M.El(j,j));
ea.y = (float)atan2(-M.El(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);
}
/* Construct quaternion from Euler angles (in radians). */
LTRotation Eul_ToQuat(EulerAngles ea)
{
LTRotation qu;
float a[3], ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
int i,j,k,h,n,s,f;
EulGetOrd((int)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.5f; tj = ea.y*0.5f; th = ea.z*0.5f;
ci = (float)cos(ti); cj = (float)cos(tj); ch = (float)cos(th);
si = (float)sin(ti); sj = (float)sin(tj); sh = (float)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[3] = cj*(cc - ss);
} else {
a[i] = cj*sc - sj*cs;
a[j] = cj*ss + sj*cc;
a[k] = cj*cs - sj*sc;
qu[3] = cj*cc + sj*ss;
}
if (n==EulParOdd) a[j] = -a[j];
qu[0] = a[EulX]; qu[1] = a[EulY]; qu[2] = a[EulZ];
return (qu);
}
/* 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(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);
}
/* 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 = order;
return (ea);
}
/* Construct matrix from Euler angles (in radians). */
void Eul_ToMatrix(EulerAngles ea, LTMatrix &M)
{
float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
int i,j,k,h,n,s,f;
EulGetOrd((int)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 = (float)cos(ti);
cj = (float)cos(tj);
ch = (float)cos(th);
si = (float)sin(ti);
sj = (float)sin(tj);
sh = (float)sin(th);
cc = ci*ch;
cs = ci*sh;
sc = si*ch;
ss = si*sh;
if (s==EulRepYes) {
M.El(i,i) = cj;
M.El(i,j) = sj*si;
M.El(i,k) = sj*ci;
M.El(j,i) = sj*sh;
M.El(j,j) = -cj*ss+cc;
M.El(j,k) = -cj*cs-sc;
M.El(k,i) = -sj*ch;
M.El(k,j) = cj*sc+cs;
M.El(k,k) = cj*cc-ss;
} else {
M.El(i,i) = cj*ch;
M.El(i,j) = sj*sc-cs;
M.El(i,k) = sj*cc+ss;
M.El(j,i) = cj*sh;
M.El(j,j) = sj*ss+cc;
M.El(j,k) = sj*cs-sc;
M.El(k,i) = -sj;
M.El(k,j) = cj*si;
M.El(k,k) = cj*ci;
}
M.El(EulW,EulX)=M.El(EulW,EulY)=M.El(EulW,EulZ)=M.El(EulX,EulW)=M.El(EulY,EulW)=M.El(EulZ,EulW)=0.0;
M.El(EulW,EulW)=1.0;
}
/* Convert matrix to Euler angles (in radians). */
GLvector glMatrixToEuler (GLmatrix mat, int order)
{
GLvector ea;
int i,j,k,h,n,s,f;
EulGetOrd (order,i,j,k,h,n,s,f);
if (s==EulRepYes) {
float sy = (float)sqrt(mat.elements[i][j]*mat.elements[i][j] + mat.elements[i][k]*mat.elements[i][k]);
if (sy > 16 * FLT_EPSILON) {
ea.x = (float)atan2(mat.elements[i][j], mat.elements[i][k]);
ea.y = (float)atan2(sy, mat.elements[i][i]);
ea.z = (float)atan2(mat.elements[j][i], -mat.elements[k][i]);
} else {
ea.x = (float)atan2(-mat.elements[j][k], mat.elements[j][j]);
ea.y = (float)atan2(sy, mat.elements[i][i]);
ea.z = 0;
}
} else {
float cy = (float)sqrt(mat.elements[i][i]*mat.elements[i][i] + mat.elements[j][i]*mat.elements[j][i]);
if (cy > 16*FLT_EPSILON) {
ea.x = (float)atan2(mat.elements[k][j], mat.elements[k][k]);
ea.y = (float)atan2(-mat.elements[k][i], cy);
ea.z = (float)atan2(mat.elements[j][i], mat.elements[i][i]);
} else {
ea.x = (float)atan2(-mat.elements[j][k], mat.elements[j][j]);
ea.y = (float)atan2(-mat.elements[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 = order;
return (ea);
}
/* 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(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;
}
