/// <summary>
/// Gets a 3x3 rotation matrix from this Quaternion.
/// </summary>
/// <returns></returns>
public Matrix33 ToRotationMatrix()
{
Matrix33 rotation = new Matrix33();
float tx = 2.0f * this.x;
float ty = 2.0f * this.y;
float tz = 2.0f * this.z;
float twx = tx * this.w;
float twy = ty * this.w;
float twz = tz * this.w;
float txx = tx * this.x;
float txy = ty * this.x;
float txz = tz * this.x;
float tyy = ty * this.y;
float tyz = tz * this.y;
float tzz = tz * this.z;
rotation.m00 = 1.0f - (tyy + tzz);
rotation.m01 = txy - twz;
rotation.m02 = txz + twy;
rotation.m10 = txy + twz;
rotation.m11 = 1.0f - (txx + tzz);
rotation.m12 = tyz - twx;
rotation.m20 = txz - twy;
rotation.m21 = tyz + twx;
rotation.m22 = 1.0f - (txx + tyy);
return(rotation);
}
/// <summary>
///
/// </summary>
/// <param name="matrix"></param>
public void FromRotationMatrix(Matrix33 matrix)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
float trace = matrix.m00 + matrix.m11 + matrix.m22;
float root = 0.0f;
if (trace > 0.0f)
{
// |this.w| > 1/2, may as well choose this.w > 1/2
root = MathUtil.Sqrt(trace + 1.0f); // 2w
this.w = 0.5f * root;
root = 0.5f / root; // 1/(4w)
this.x = (matrix.m21 - matrix.m12) * root;
this.y = (matrix.m02 - matrix.m20) * root;
this.z = (matrix.m10 - matrix.m01) * root;
}
else
{
// |this.w| <= 1/2
int i = 0;
if (matrix.m11 > matrix.m00)
{
i = 1;
}
if (matrix.m22 > matrix[i, i])
{
i = 2;
}
int j = next[i];
int k = next[j];
root = MathUtil.Sqrt(matrix[i, i] - matrix[j, j] - matrix[k, k] + 1.0f);
unsafe
{
fixed(float *apkQuat = &this.x)
{
apkQuat[i] = 0.5f * root;
root = 0.5f / root;
this.w = (matrix[k, j] - matrix[j, k]) * root;
apkQuat[j] = (matrix[j, i] + matrix[i, j]) * root;
apkQuat[k] = (matrix[k, i] + matrix[i, k]) * root;
}
}
}
}
/// <summary>
/// Used to subtract two matrices.
/// </summary>
/// <param name="left"></param>
/// <param name="right"></param>
/// <returns></returns>
public static Matrix33 operator -(Matrix33 left, Matrix33 right)
{
Matrix33 result = new Matrix33();
for (int row = 0; row < 3; row++)
{
for (int col = 0; col < 3; col++)
{
result[row, col] = left[row, col] - right[row, col];
}
}
return(result);
}
/// <summary>
/// Multiplies all the items in the Matrix33 by a scalar value.
/// </summary>
/// <param name="matrix"></param>
/// <param name="scalar"></param>
/// <returns></returns>
public static Matrix33 operator *(float scalar, Matrix33 matrix)
{
Matrix33 result = new Matrix33();
result.m00 = matrix.m00 * scalar;
result.m01 = matrix.m01 * scalar;
result.m02 = matrix.m02 * scalar;
result.m10 = matrix.m10 * scalar;
result.m11 = matrix.m11 * scalar;
result.m12 = matrix.m12 * scalar;
result.m20 = matrix.m20 * scalar;
result.m21 = matrix.m21 * scalar;
result.m22 = matrix.m22 * scalar;
return(result);
}
/// <summary>
/// Constructs this Matrix from 3 euler angles, in degrees.
/// </summary>
/// <param name="yaw"></param>
/// <param name="pitch"></param>
/// <param name="roll"></param>
public void FromEulerAnglesXYZ(float yaw, float pitch, float roll)
{
float cos = MathUtil.Cos(yaw);
float sin = MathUtil.Sin(yaw);
Matrix33 xMat = new Matrix33(1, 0, 0, 0, cos, -sin, 0, sin, cos);
cos = MathUtil.Cos(pitch);
sin = MathUtil.Sin(pitch);
Matrix33 yMat = new Matrix33(cos, 0, sin, 0, 1, 0, -sin, 0, cos);
cos = MathUtil.Cos(roll);
sin = MathUtil.Sin(roll);
Matrix33 zMat = new Matrix33(cos, -sin, 0, sin, cos, 0, 0, 0, 1);
this = xMat * (yMat * zMat);
}
/// <summary>
/// Negates all the items in the Matrix.
/// </summary>
/// <param name="matrix"></param>
/// <returns></returns>
public static Matrix33 operator -(Matrix33 matrix)
{
Matrix33 result = new Matrix33();
result.m00 = -matrix.m00;
result.m01 = -matrix.m01;
result.m02 = -matrix.m02;
result.m10 = -matrix.m10;
result.m11 = -matrix.m11;
result.m12 = -matrix.m12;
result.m20 = -matrix.m20;
result.m21 = -matrix.m21;
result.m22 = -matrix.m22;
return(result);
}
/// <summary>
/// Multiply (concatenate) two Matrix33 instances together.
/// </summary>
/// <param name="left"></param>
/// <param name="right"></param>
/// <returns></returns>
public static Matrix33 operator *(Matrix33 left, Matrix33 right)
{
Matrix33 result = new Matrix33();
result.m00 = left.m00 * right.m00 + left.m01 * right.m10 + left.m02 * right.m20;
result.m01 = left.m00 * right.m01 + left.m01 * right.m11 + left.m02 * right.m21;
result.m02 = left.m00 * right.m02 + left.m01 * right.m12 + left.m02 * right.m22;
result.m10 = left.m10 * right.m00 + left.m11 * right.m10 + left.m12 * right.m20;
result.m11 = left.m10 * right.m01 + left.m11 * right.m11 + left.m12 * right.m21;
result.m12 = left.m10 * right.m02 + left.m11 * right.m12 + left.m12 * right.m22;
result.m20 = left.m20 * right.m00 + left.m21 * right.m10 + left.m22 * right.m20;
result.m21 = left.m20 * right.m01 + left.m21 * right.m11 + left.m22 * right.m21;
result.m22 = left.m20 * right.m02 + left.m21 * right.m12 + left.m22 * right.m22;
return(result);
}
/// <summary>
///
/// </summary>
/// <param name="xAxis"></param>
/// <param name="yAxis"></param>
/// <param name="zAxis"></param>
public void FromAxes(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
{
Matrix33 rotation = new Matrix33();
rotation.m00 = xAxis.x;
rotation.m10 = xAxis.y;
rotation.m20 = xAxis.z;
rotation.m01 = yAxis.x;
rotation.m11 = yAxis.y;
rotation.m21 = yAxis.z;
rotation.m02 = zAxis.x;
rotation.m12 = zAxis.y;
rotation.m22 = zAxis.z;
// set this quaternions values from the rotation matrix built
FromRotationMatrix(rotation);
}
/// <summary>
///
/// </summary>
/// <param name="xAxis"></param>
/// <param name="yAxis"></param>
/// <param name="zAxis"></param>
public void ToAxes(out Vector3 xAxis, out Vector3 yAxis, out Vector3 zAxis)
{
xAxis = new Vector3();
yAxis = new Vector3();
zAxis = new Vector3();
Matrix33 rotation = this.ToRotationMatrix();
xAxis.x = rotation.m00;
xAxis.y = rotation.m10;
xAxis.z = rotation.m20;
yAxis.x = rotation.m01;
yAxis.y = rotation.m11;
yAxis.z = rotation.m21;
zAxis.x = rotation.m02;
zAxis.y = rotation.m12;
zAxis.z = rotation.m22;
}
/// <summary>
/// Negates all the items in the Matrix.
/// </summary>
/// <param name="matrix"></param>
/// <returns></returns>
public static Matrix33 Negate(Matrix33 matrix)
{
return(-matrix);
}
/// <summary>
/// Used to subtract two matrices.
/// </summary>
/// <param name="left"></param>
/// <param name="right"></param>
/// <returns></returns>
public static Matrix33 Subtract(Matrix33 left, Matrix33 right)
{
return(left - right);
}
/// <summary>
/// Used to add two matrices together.
/// </summary>
/// <param name="left"></param>
/// <param name="right"></param>
/// <returns></returns>
public static Matrix33 Add(Matrix33 left, Matrix33 right)
{
return(left + right);
}
/// <summary>
/// Multiplies all the items in the Matrix33 by a scalar value.
/// </summary>
/// <param name="matrix"></param>
/// <param name="scalar"></param>
/// <returns></returns>
public static Matrix33 Multiply(float scalar, Matrix33 matrix)
{
return(scalar * matrix);
}
/// <summary>
/// matrix * vector [3x3 * 3x1 = 3x1]
/// </summary>
/// <param name="vector"></param>
/// <param name="matrix"></param>
/// <returns></returns>
public static Vector3 Multiply(Matrix33 matrix, Vector3 vector)
{
return(matrix * vector);
}
/// <summary>
/// vector * matrix [1x3 * 3x3 = 1x3]
/// </summary>
/// <param name="vector"></param>
/// <param name="matrix"></param>
/// <returns></returns>
public static Vector3 Multiply(Vector3 vector, Matrix33 matrix)
{
return(vector * matrix);
}
/// <summary>
/// Multiply (concatenate) two Matrix33 instances together.
/// </summary>
/// <param name="left"></param>
/// <param name="right"></param>
/// <returns></returns>
public static Matrix33 Multiply(Matrix33 left, Matrix33 right)
{
return(left * right);
}
/// <summary>
/// Used to allow assignment from a Matrix33 to a Matrix44 object.
/// </summary>
/// <param name="right"></param>
/// <returns></returns>
public static Matrix44 FromMatrix3(Matrix33 right)
{
return(right);
}
/// <summary>
/// Multiplies all the items in the Matrix33 by a scalar value.
/// </summary>
/// <param name="matrix"></param>
/// <param name="scalar"></param>
/// <returns></returns>
public static Matrix33 Multiply(Matrix33 matrix, float scalar)
{
return(matrix * scalar);
}
