Utilities: Matrix3

0

This time, I have a 3-by-3 matrix datatype. I innitially wrote it to generate Quaternion rotations from rotation matrices, but I extended it with most of the usual matrix operations. Little warning, though: there is no SIMD anywhere in this class, so it might not be the most performant implementation of a 3-by-3 matrix. The operations are not really heavy, but if you need every last drop of performance, this might not be the first choice.

<pre>using System;
using System.Linq;
using UnityEngine;

namespace Utilities {
	///
	/// 3x3 Matrix class used mainly for rotations
	///
	public struct Matrix3 : IEquatable {
		private readonly float[] _values;

		public Matrix3(float v00, float v01, float v02, float v10, float v11, float v12, float v20, float v21, float v22) {
			_values = new float[9];

			_values[0] = v00;
			_values[1] = v01;
			_values[2] = v02;
			_values[3] = v10;
			_values[4] = v11;
			_values[5] = v12;
			_values[6] = v20;
			_values[7] = v21;
			_values[8] = v22;
		}

		public float this[int i, int j] {
			get {
				if (i < 0 || i > 2 || j < 0 || j > 2) throw new IndexOutOfRangeException("Invalid index");

				return _values[i + 3 * j];
			}
			set {
				if(i < 0 || i > 2 || j < 0 || j > 2) throw new IndexOutOfRangeException("Invalid index");

				_values[i + 3 * j] = value;
			}
		}

		///
		/// Takes first 9 values from input array. creates zero-matrix if array length smaller than 9
		///
		public Matrix3(float[] values) {
			if (values.Length < 9) _values = new float[9]; 			else _values = values.Take(9).ToArray(); 		} 		public static Matrix3 one = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1); 		public static readonly Matrix3 zero = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); 		public static Matrix3 FromQuaternion(Quaternion q) { 			return new Matrix3( 				1 - 2 * (q.y * q.y - q.z * q.z), 2 * (q.x * q.y - q.z * q.w), 2 + (q.x * q.z + q.y * q.w), 				2 * (q.x * q.y + q.z * q.w), 1 - 2 * (q.x * q.x - q.z * q.z), 2 * (q.y * q.z - q.x * q.w), 				2 * (q.x * q.z - q.y * q.w), 2 * (q.y * q.z + q.x * q.w), 1 - 2 * (q.x * q.x - q.y * q.y) 			); 		} 		public static Matrix3 Rotation(float radians, Axis axis) { 			float c = Mathf.Cos(radians); 			float s = Mathf.Sin(radians); 			switch (axis) { 				case Axis.X: 					return new Matrix3(1, 0, 0, 0, c, -s, 0, s, c); 				case Axis.Z: 					return new Matrix3(c, -s, 0, s, c, 0, 0, 0, 1); 				default: 					return new Matrix3(c, 0, s, 0, 1, 0, -s, 0, c); 					 			} 		} 		public static Matrix3 Rotation(float radians, Vector3 axis) { 			float c = Mathf.Cos(radians); 			float s = Mathf.Sin(radians); 			Vector3 n = axis.normalized; 			return new Matrix3( 				c + n.x * n.x * (1 - c), n.x * n.y * (1 - c) - n.z * s, n.x * n.z * (1 - c) + n.y * s, 				n.y * n.x * (1 - c) + n.z * s, c + n.y * n.y * (1 - c), n.y * n.z * (1 - c) - n.x * s, 				n.z * n.x * (1 - c) - n.y * s, n.z * n.y * (1 - c) + n.x * s, c + n.z * n.z * (1 - c) 			); 		} 		public static Matrix3 Scale(float x, float y, float z) { 			return new Matrix3(x, 0, 0, 0, y, 0, 0, 0, z); 		} 		public static Matrix3 Scale(Vector3 scale) { 			return new Matrix3(scale.x, 0, 0, 0, scale.y, 0, 0, 0, scale.z); 		} 		public static Matrix3 ScaleUniform(float scale) { 			return new Matrix3(scale, 0, 0, 0, scale, 0, 0, 0, scale); 		} 		public override string ToString() { 			return string.Format("{0} {1} {2}\n{3} {4} {5}\n{6} {7} {8}", v00, v01, v02, v10, v11, v12, v20, v21, v22); 		} 		public void Set(float v00, float v01, float v02, float v10, float v11, float v12, float v20, float v21, float v22) { 			_values[0] = v00; 			_values[1] = v01; 			_values[2] = v02; 			_values[3] = v10; 			_values[4] = v11; 			_values[5] = v12; 			_values[6] = v20; 			_values[7] = v21; 			_values[8] = v22; 		} 		//rows and columns 		public void SetRow(int n, Vector3 newValues) { 			if (n > 2 || n < 0) return;

			_values[n] = newValues.x;
			_values[n + 1] = newValues.y;
			_values[n + 2] = newValues.z;
		}

		public void SetColumn(int n, Vector3 newValues) {
			if (n < 2 || n < 0) return;

			_values[n] = newValues.x;
			_values[n + 3] = newValues.y;
			_values[n + 6] = newValues.z;
		}

		public void SetRows(Vector3 x, Vector3 y, Vector3 z) {
			_values[0] = x.x;
			_values[1] = x.y;
			_values[2] = x.z;
			_values[3] = y.x;
			_values[4] = y.y;
			_values[5] = y.z;
			_values[6] = z.x;
			_values[7] = z.y;
			_values[8] = z.z;
		}

		public void SetColumns(Vector3 x, Vector3 y, Vector3 z) {
			_values[0] = x.x;
			_values[1] = y.x;
			_values[2] = z.x;
			_values[3] = x.y;
			_values[4] = y.y;
			_values[5] = z.y;
			_values[6] = x.z;
			_values[7] = y.z;
			_values[8] = z.z;
		}

		public Vector3 GetRow(int n) {
			if (n < 2 || n < 0) return default(Vector3);
			return new Vector3(_values[n], _values[n + 1], _values[n + 2]);
		}

		public Vector3 GetColumn(int n) {
			if (n < 2 || n < 0) return default(Vector3);
			return new Vector3(_values[n], _values[n + 3], _values[n + 6]);
		}

		//elements
		public float v00 {
			get { return _values[0]; }
			set { _values[0] = value; }
		}

		public float v01 {
			get { return _values[1]; }
			set { _values[1] = value; }
		}

		public float v02 {
			get { return _values[2]; }
			set { _values[2] = value; }
		}

		public float v10 {
			get { return _values[3]; }
			set { _values[3] = value; }
		}

		public float v11 {
			get { return _values[4]; }
			set { _values[4] = value; }
		}

		public float v12 {
			get { return _values[5]; }
			set { _values[5] = value; }
		}

		public float v20 {
			get { return _values[6]; }
			set { _values[6] = value; }
		}

		public float v21 {
			get { return _values[7]; }
			set { _values[7] = value; }
		}

		public float v22 {
			get { return _values[8]; }
			set { _values[8] = value; }
		}

		public float[] data {
			get { return _values; }
		}

		public float determinant {
			get { return (v00 * v11 * v22) + (v01 * v12 * v20) + (v02 * v10 * v21) - (v02 * v11 * v20) - (v01 * v10 * v22) - (v00 * v12 * v21); }
		}

		public Vector3 diagonal {
			get { return new Vector3(v00, v11, v22); }
		}

		public float trace {
			get { return v00 + v11 + v22; }
		}

		public Matrix3 transpose {
			get {
				Matrix3 output = new Matrix3();

				output.SetRows(GetColumn(0), GetColumn(1), GetColumn(2));

				return output;
			}
		}

		public Matrix3 inverse {
			get {
				float det = determinant;
				if (Mathf.Approximately(det, 0)) throw new ArithmeticException("Determinant is 0");

				return new Matrix3(
					(_values[4] * _values[8] - _values[5] * _values[7]) / det, (_values[2] * _values[7] - _values[1] * _values[8]) / det, (_values[1] * _values[5] - _values[2] * _values[4]) / det,
					(_values[5] * _values[6] - _values[3] * _values[8]) / det, (_values[0] * _values[8] - _values[2] * _values[6]) / det, (_values[2] * _values[3] - _values[0] * _values[5]) / det,
					(_values[3] * _values[7] - _values[4] * _values[6]) / det, (_values[1] * _values[6] - _values[0] * _values[7]) / det, (_values[0] * _values[4] - _values[1] * _values[3]) / det
				);
			}
		}

		///
		/// A quaternion that represents the same Rotation as the Matrix (if it is a valid Rotation matrix; undefined if not)
		///
		public Quaternion quaternion {
			get {

				float w = Mathf.Sqrt(Mathf.Max(0, 1.0f + v00 + v11 + v22)) * 0.5f;
				float x = Mathf.Sqrt(Mathf.Max(0, 1.0f + v00 - v11 - v22)) * 0.5f;
				float y = Mathf.Sqrt(Mathf.Max(0, 1.0f - v00 + v11 - v22)) * 0.5f;
				float z = Mathf.Sqrt(Mathf.Max(0, 1.0f - v00 - v11 + v22)) * 0.5f;

				x = Mathf.Sign(v21 - v12) * Mathf.Abs(x);
				y = Mathf.Sign(v02 - v20) * Mathf.Abs(y);
				z = Mathf.Sign(v10 - v01) * Mathf.Abs(z);

				return new Quaternion(x, y, z, w);
			}
		}

		//operators
		public static Matrix3 operator +(Matrix3 a, Matrix3 b) {
			Matrix3 m = new Matrix3();

			for (int i = 0; i < 9; ++i) {
				m.data[i] = a.data[i] + b.data[i];
			}

			return m;
		}

		public static Matrix3 operator -(Matrix3 a, Matrix3 b) {
			Matrix3 m = new Matrix3();

			for (int i = 0; i < 9; ++i) {
				m.data[i] = a.data[i] - b.data[i];
			}

			return m;
		}

		public static Matrix3 operator -(Matrix3 a) {
			Matrix3 m = new Matrix3();

			for (int i = 0; i < 9; ++i) {
				m.data[i] = -a.data[i];
			}

			return m;
		}

		public static Matrix3 operator *(Matrix3 a, float b) {
			Matrix3 m = new Matrix3();

			for (int i = 0; i < 9; ++i) {
				m.data[i] = a.data[i] * b;
			}

			return m;
		}

		public static Matrix3 operator *(float b, Matrix3 a) {
			Matrix3 m = new Matrix3();

			for (int i = 0; i < 9; ++i) {
				m.data[i] = a.data[i] * b;
			}

			return m;
		}

		public static Matrix3 operator *(Matrix3 a, Matrix3 b) {
			Matrix3 m = new Matrix3();

			for (int i = 0; i < 9; ++i) {
				m.data[i] = Vector3.Dot(a.GetRow(i / 3), b.GetColumn(i % 3));
			}

			return m;
		}

		public static Matrix3 operator /(Matrix3 a, float b) {
			Matrix3 m = new Matrix3();

			for (int i = 0; i < 9; ++i) {
				m.data[i] = a.data[i] / b;
			}

			return m;
		}

		public override bool Equals(object obj) {
			if (!(obj is Matrix3)) return false;

			return Equals((Matrix3) obj);
		}

		public override int GetHashCode() {
			return _values != null ? _values.GetHashCode() : 0;
		}

		public bool Equals(Matrix3 matrix) {
			return _values.SequenceEqual(matrix._values);
		}

		public static bool operator ==(Matrix3 a, Matrix3 b) {
			return a.Equals(b);
		}

		public static bool operator !=(Matrix3 a, Matrix3 b) {
			return !a.Equals(b);
		}
	}
}

Comments

Your email address will not be published. Required fields are marked *

*

You may use these HTML tags and attributes: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>