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);
}