public Vector3 ExtractRotation() { Matrix4x4 temp = new Matrix4x4(this); Vector3 scaleExtract = temp.ExtractScale(); Matrix4x4 temp1 = new Matrix4x4(temp.m00 / scaleExtract.x, temp.m01 / scaleExtract.y, temp.m02 / scaleExtract.z, temp.m03, temp.m10 / scaleExtract.x, temp.m11 / scaleExtract.y, temp.m12 / scaleExtract.z, temp.m13, temp.m20 / scaleExtract.x, temp.m21 / scaleExtract.y, temp.m22 / scaleExtract.z, temp.m23, temp.m30 / scaleExtract.x, temp.m31 / scaleExtract.y, temp.m32 / scaleExtract.z, temp.m33); Quaterion q = new Quaterion(); q.w = Mathf.Sqrt(1.0f + temp1.m00 + temp1.m11 + temp1.m22) / 2.0f; q.w *= 4.0f; q.x = (temp1.m21 - temp1.m12) / q.w; q.y = (temp1.m02 - temp1.m20) / q.w; q.z = (temp1.m10 - temp1.m01) / q.w; return(q.QuaterionToEuler()); }
public Vector3 QuaterionToEuler(Quaterion q) { //X rotation Vector3 eul = new Vector3(); float sinX = 2.0f * (q.w * q.x + q.y * q.z); float cosX = 1.0f - 2.0f * (q.x * q.x + q.y * q.y); eul.y = Mathf.Atan2(sinX, cosX); //Y rotation float sinY = 2.0f * (q.w * q.y - q.z * q.x); eul.y = Mathf.Asin(sinY); //Z rotation float sinZ = 2.0f * (q.w * q.z + q.x * q.y); float cosZ = 1.0f - 2.0f * (q.y * q.y + q.z * q.z); eul.z = Mathf.Atan2(sinZ, cosZ); return(eul); }
public Quaterion EulerToQuaterion(float yaw, float pitch, float roll) { yaw *= Mathf.Deg2Rad; pitch *= Mathf.Deg2Rad; roll *= Mathf.Deg2Rad; float rollOver2 = roll * 0.5f; float sinRollOver2 = (float)Mathf.Sin(rollOver2); float cosRollOver2 = (float)Mathf.Cos(rollOver2); float pitchOver2 = pitch * 0.5f; float sinPitchOver2 = (float)Mathf.Sin(pitchOver2); float cosPitchOver2 = (float)Mathf.Cos(pitchOver2); float yawOver2 = yaw * 0.5f; float sinYawOver2 = (float)Mathf.Sin(yawOver2); float cosYawOver2 = (float)Mathf.Cos(yawOver2); Quaterion result = new Quaterion(); result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2; result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; result.y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; return(result); }