static CHAR* RotOrdStr[7] = { "", "XYZ", "XZY", "YXZ", "YZX", "ZXY", "ZYX" }; // IPPDial.HPBToPoser - Convert HPB to Poser ordered-Euler angles //*---------------------------------------------------------------------------* void IPPDial::HPBToPoser(LONG rotorder, Matrix mat, Vector prot) //*---------------------------------------------------------------------------* { Vector v1; Vector v2; Real sx = 1.0f, sy = 1.0f, sz = 1.0f; Real x, y, z; Real ox = prot.x, oy = prot.y, oz = prot.z; // Take into account Rotation Order and extract Euler angles switch (rotorder) { case ROTORDER_XYZ: v1 = mat.v1; v2 = mat.v2; y = asin(v1.z); if (y < pi05) { if (y > -pi05) { x = Angle(-v2.z, mat.v3.z); z = Angle(-v1.y, v1.x); } else { x = -Angle(v2.x, v2.y); z = 0.0f; } } else { x = Angle(v2.x, v2.y); z = 0.0f; } sy = -sy; break; case ROTORDER_XZY: v1 = mat.v1; v2 = mat.v3; z = asin(-v1.y); if (z < pi05) { if (z > -pi05) { x = Angle(v2.y, mat.v2.y); y = Angle(v1.z, v1.x); } else { x = -Angle(-v2.x, v2.z); y = 0.0f; } } else { x = Angle(-v2.x, v2.z); y = 0.0f; } sz = -sz; break; case ROTORDER_YXZ: v1 = mat.v1; v2 = mat.v2; x = asin(-v2.z); if (x < pi05) { if (x > -pi05) { y = Angle(v1.z, mat.v3.z); z = Angle(v2.x, v2.y); } else { y = -Angle(-v1.y, v1.x); z = 0.0f; } } else { y = Angle(-v1.y, v1.x); z = 0.0f; } sx = -sx; break; case ROTORDER_YZX: v1 = mat.v3; v2 = mat.v2; z = asin(v2.x); if (z < pi05) { if (z > -pi05) { y = Angle(-v1.x, mat.v1.x); x = Angle(-v2.z, v2.y); } else { y = -Angle(v1.y, v1.z); x = 0.0f; } } else { y = Angle(v1.y, v1.z); x = 0.0f; } sz = -sz; break; case ROTORDER_ZXY: v1 = mat.v1; v2 = mat.v3; x = asin(v2.y); if (x < pi05) { if (x > -pi05) { z = Angle(-v1.y, mat.v2.y); y = Angle(-v2.x, v2.z); } else { z = -Angle(v1.z, v1.x); y = 0.0f; } } else { z = Angle(v1.z, v1.x); y = 0.0f; } sx = -sx; break; case ROTORDER_ZYX: v1 = mat.v1; v2 = mat.v3; y = asin(-v2.x); if (y < pi05) { if (y > -pi05) { z = Angle(mat.v2.x, v1.x); x = Angle(v2.y, v2.z); } else { z = -Angle(-v1.y, v1.z); x = 0.0f; } } else { z = Angle(-v1.y, v1.z); x = 0.0f; } sy = -sy; break; } Vector d1, d2; Real dc, dp, dn; Real adc, adp, adn; // Find minimum relation in full revolutions (factors of 2*pi or 360d) // r10 = r +/- (n*pi2) // r11 = r +/- (n*pi2) // r12 = r +/- (n*pi2) // - X dc = x; dp = x + pi2; dn = x - pi2; adc = Abs(x - ox); adp = Abs(dp - ox); adn = Abs(dn - ox); d1.x = (adc < adp) ? ((adc < adn) ? dc : dn) : ((adp < adn) ? dp : dn); // - Y dc = y; dp = y + pi2; dn = y - pi2; adc = Abs(y - oy); adp = Abs(dp - oy); adn = Abs(dn - oy); d1.y = (adc < adp) ? ((adc < adn) ? dc : dn) : ((adp < adn) ? dp : dn); // - Z dc = z; dp = z + pi2; dn = z - pi2; adc = Abs(z - oz); adp = Abs(dp - oz); adn = Abs(dn - oz); d1.z = (adc < adp) ? ((adc < adn) ? dc : dn) : ((adp < adn) ? dp : dn); // Make relative to hemi revolution (factor of pi or 180d) // r20 = pi + r10 +/- (n*pi2) // r21 = pi - r11 +/- (n*pi2) // r22 = pi + r12 +/- (n*pi2) // sx,sy,sz handle subtraction on second rotation (pi - r + (n*pi2)) // - X dc = pi + (sx*x); dp = pi + (sx*x) + pi2; dn = pi + (sx*x) - pi2; adc = Abs(dc - ox); adp = Abs(dp - ox); adn = Abs(dn - ox); d2.x = (adc < adp) ? ((adc < adn) ? dc : dn) : ((adp < adn) ? dp : dn); // - Y dc = pi + (sy*y); dp = pi + (sy*y) + pi2; dn = pi + (sy*y) - pi2; adc = Abs(dc - oy); adp = Abs(dp - oy); adn = Abs(dn - oy); d2.y = (adc < adp) ? ((adc < adn) ? dc : dn) : ((adp < adn) ? dp : dn); // - Z dc = pi + (sz*z); dp = pi + (sz*z) + pi2; dn = pi + (sz*z) - pi2; adc = Abs(dc - oz); adp = Abs(dp - oz); adn = Abs(dn - oz); d2.z = (adc < adp) ? ((adc < adn) ? dc : dn) : ((adp < adn) ? dp : dn); // Find closest to reference triplet (prot) Real d1a, d2a; d1a = Abs(d1.x - ox); d2a = Abs(d2.x - ox); x = (d1a < d2a) ? d1.x : d2.x; d1a = Abs(d1.y - oy); d2a = Abs(d2.y - oy); y = (d1a < d2a) ? d1.y : d2.y; d1a = Abs(d1.z - oz); d2a = Abs(d2.z - oz); z = -((d1a < d2a) ? d1.z : d2.z); GePrint("Rotation ("+String(RotOrdStr[rotorder])+") = "+VectorToString(Vector(ToDeg(x),ToDeg(y),ToDeg(z)))+" (xyz)"); }