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