public static void CreateFromYawPitchRoll( float yaw, float pitch, float roll, out Quaternion result )
{
float rsin = ( float )Math.Sin( roll / 2.0f );
float rcos = ( float )Math.Cos( roll / 2.0f );
float psin = ( float )Math.Sin( pitch / 2.0f );
float pcos = ( float )Math.Cos( pitch / 2.0f );
float ysin = ( float )Math.Sin( yaw / 2.0f );
float ycos = ( float )Math.Cos( yaw / 2.0f );
result.X = ( ( ycos * psin ) * rcos ) + ( ( ysin * pcos ) * rsin );
result.Y = ( ( ysin * pcos ) * rcos ) - ( ( ycos * psin ) * rsin );
result.Z = ( ( ycos * pcos ) * rsin ) - ( ( ysin * psin ) * rcos );
result.W = ( ( ycos * pcos ) * rcos ) + ( ( ysin * psin ) * rsin );
}
// TODO: cleanup
public static void Transform( ref Vector3 value, ref Quaternion rotation, out Vector3 result )
{
float xxx = rotation.X * ( rotation.X + rotation.X );
float wxx = rotation.W * ( rotation.X + rotation.X );
float xyy = rotation.X * ( rotation.Y + rotation.Y );
float yyy = rotation.Y * ( rotation.Y + rotation.Y );
float wyy = rotation.W * ( rotation.Y + rotation.Y );
float xzz = rotation.X * ( rotation.Z + rotation.Z );
float yzz = rotation.Y * ( rotation.Z + rotation.Z );
float zzz = rotation.Z * ( rotation.Z + rotation.Z );
float wzz = rotation.W * ( rotation.Z + rotation.Z );
result = new Vector3();
result.X = ( ( value.X * ( ( 1.0f - yyy ) - zzz ) ) + ( value.Y * ( xyy - wzz ) ) ) + ( value.Z * ( xzz + wyy ) );
result.Y = ( ( value.X * ( xyy + wzz ) ) + ( value.Y * ( ( 1.0f - xxx ) - zzz ) ) ) + ( value.Z * ( yzz - wxx ) );
result.Z = ( ( value.X * ( xzz - wyy ) ) + ( value.Y * ( yzz + wxx ) ) ) + ( value.Z * ( ( 1.0f - xxx ) - yyy ) );
}
public void glRotate( ref Quaternion q )
{
Matrix m;
Matrix.CreateFromQuaternion( ref q, out m );
glMultMatrixf( ref m );
}
// TODO: cleanup
public static void CreateFromQuaternion( ref Quaternion quaternion, out Matrix result )
{
float num9 = quaternion.X * quaternion.X;
float num8 = quaternion.Y * quaternion.Y;
float num7 = quaternion.Z * quaternion.Z;
float num6 = quaternion.X * quaternion.Y;
float num5 = quaternion.Z * quaternion.W;
float num4 = quaternion.Z * quaternion.X;
float num3 = quaternion.Y * quaternion.W;
float num2 = quaternion.Y * quaternion.Z;
float num = quaternion.X * quaternion.W;
result.M11 = 1f - ( 2f * ( num8 + num7 ) );
result.M12 = 2f * ( num6 + num5 );
result.M13 = 2f * ( num4 - num3 );
result.M14 = 0f;
result.M21 = 2f * ( num6 - num5 );
result.M22 = 1f - ( 2f * ( num7 + num9 ) );
result.M23 = 2f * ( num2 + num );
result.M24 = 0f;
result.M31 = 2f * ( num4 + num3 );
result.M32 = 2f * ( num2 - num );
result.M33 = 1f - ( 2f * ( num8 + num9 ) );
result.M34 = 0f;
result.M41 = 0f;
result.M42 = 0f;
result.M43 = 0f;
result.M44 = 1f;
}
