public static LMatrix33 QuaternionToMatrix(LQuaternion quat) { LMatrix33 m = new LMatrix33(); LFloat x = quat.x * 2; LFloat y = quat.y * 2; LFloat z = quat.z * 2; LFloat xx = quat.x * x; LFloat yy = quat.y * y; LFloat zz = quat.z * z; LFloat xy = quat.x * y; LFloat xz = quat.x * z; LFloat yz = quat.y * z; LFloat wx = quat.w * x; LFloat wy = quat.w * y; LFloat wz = quat.w * z; m[0] = 1 - (yy + zz); m[1] = xy + wz; m[2] = xz - wy; m[3] = xy - wz; m[4] = 1 - (xx + zz); m[5] = yz + wx; m[6] = xz + wy; m[7] = yz - wx; m[8] = 1 - (xx + yy); return(m); }
private static LQuaternion MatrixToQuaternion(LMatrix33 m) { LQuaternion quat = new LQuaternion(); LFloat fTrace = m[0, 0] + m[1, 1] + m[2, 2]; LFloat root; if (fTrace > 0) { root = LMath.Sqrt(fTrace + 1); quat.w = LFloat.half * root; root = LFloat.half / root; quat.x = (m[2, 1] - m[1, 2]) * root; quat.y = (m[0, 2] - m[2, 0]) * root; quat.z = (m[1, 0] - m[0, 1]) * root; } else { int[] s_iNext = new int[] { 1, 2, 0 }; int i = 0; if (m[1, 1] > m[0, 0]) { i = 1; } if (m[2, 2] > m[i, i]) { i = 2; } int j = s_iNext[i]; int k = s_iNext[j]; root = LMath.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1); if (root < 0) { throw new IndexOutOfRangeException("error!"); } quat[i] = LFloat.half * root; root = LFloat.half / root; quat.w = (m[k, j] - m[j, k]) * root; quat[j] = (m[j, i] + m[i, j]) * root; quat[k] = (m[k, i] + m[i, k]) * root; } LFloat nor = LMath.Sqrt(Dot(quat, quat)); quat = new LQuaternion(quat.x / nor, quat.y / nor, quat.z / nor, quat.w / nor); return(quat); }
private LVector3 MatrixToEuler(LMatrix33 m) { LVector3 v = new LVector3(); if (m[1, 2] < 1) { if (m[1, 2] > -1) { v.x = LMath.Asin(-m[1, 2]); v.y = LMath.Atan2(m[0, 2], m[2, 2]); v.z = LMath.Atan2(m[1, 0], m[1, 1]); } else { v.x = LMath.PI * LFloat.half; v.y = LMath.Atan2(m[0, 1], m[0, 0]); v.z = (LFloat)0; } } else { v.x = -LMath.PI * LFloat.half; v.y = LMath.Atan2(-m[0, 1], m[0, 0]); v.z = (LFloat)0; } for (int i = 0; i < 3; i++) { if (v[i] < 0) { v[i] += LMath.PI2; } else if (v[i] > LMath.PI2) { v[i] -= LMath.PI2; } } return(v); }
private static LMatrix33 LookRotationToMatrix(LVector3 viewVec, LVector3 upVec) { LVector3 z = viewVec; LMatrix33 m = new LMatrix33(); LFloat mag = z.magnitude; if (mag <= 0) { m = LMatrix33.identity; } z /= mag; LVector3 x = Cross(upVec, z); mag = x.magnitude; if (mag <= 0) { m = LMatrix33.identity; } x /= mag; LVector3 y = Cross(z, x); m[0, 0] = x.x; m[0, 1] = y.x; m[0, 2] = z.x; m[1, 0] = x.y; m[1, 1] = y.y; m[1, 2] = z.y; m[2, 0] = x.z; m[2, 1] = y.z; m[2, 2] = z.z; return(m); }
/// <summary> /// 注视旋转 /// </summary> /// <param name="forward"></param> /// <param name="upwards"></param> /// <returns></returns> public static LQuaternion LookRotation(LVector3 forward, LVector3 upwards) { LMatrix33 m = LookRotationToMatrix(forward, upwards); return(MatrixToQuaternion(m)); }