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