/* Convert quaternion to Euler angles (in radians). */ EulerAngles Eul_FromQuat(LTRotation const& q, int order) { LTMatrix M; float Nq = q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]; float s = (Nq > 0.0f) ? (2.0f / Nq) : 0.0f; float xs = q[0]*s, ys = q[1]*s, zs = q[2]*s; float wx = q[3]*xs, wy = q[3]*ys, wz = q[3]*zs; float xx = q[0]*xs, xy = q[0]*ys, xz = q[0]*zs; float yy = q[1]*ys, yz = q[1]*zs, zz = q[2]*zs; M.El(EulX,EulX) = 1.0f - (yy + zz); M.El(EulX,EulY) = xy - wz; M.El(EulX,EulZ) = xz + wy; M.El(EulY,EulX) = xy + wz; M.El(EulY,EulY) = 1.0f - (xx + zz); M.El(EulY,EulZ) = yz - wx; M.El(EulZ,EulX) = xz - wy; M.El(EulZ,EulY) = yz + wx; M.El(EulZ,EulZ) = 1.0f - (xx + yy); 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.0f; return (Eul_FromMatrix(M, order)); }
/* 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). */ 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); }