今天整理自己的框架代码,将Matrix2x2和Matrix3x3给扩展了一下,发现网上unity数学计算相关挺少的,所以记录一下。
首先扩展Matrix2x2:
using System.Collections;
using System.Collections.Generic;
using Unity.Mathematics;
using UnityEngine;
public class Matrix2x2
{
#region ///properties
public const int ROW = 2;
public const int COLUMN = 2;
public float M00
{
get { return dataArr[0, 0]; }
set { dataArr[0, 0] = value; }
}
public float M01
{
get { return dataArr[0, 1]; }
set { dataArr[0, 1] = value; }
}
public float M10
{
get { return dataArr[1, 0]; }
set { dataArr[1, 0] = value; }
}
public float M11
{
get { return dataArr[1, 1]; }
set { dataArr[1, 1] = value; }
}
private float[,] dataArr = new float[ROW, COLUMN];
public Vector2 Row0 { get { return new Vector2(M00, M01); } }
public Vector2 Row1 { get { return new Vector2(M10, M11); } }
public Vector2 Column0 { get { return new Vector2(M00, M10); } }
public Vector2 Column1 { get { return new Vector2(M01, M11); } }
#endregion
public float this[int row, int col]
{
get { return dataArr[row, col]; }
set { dataArr[row, col] = value; }
}
public Matrix2x2() { }
public Matrix2x2(float m00, float m01, float m10, float m11)
{
M00 = m00;
M01 = m01;
M10 = m10;
M11 = m11;
}
/// <summary>
/// xy基向量
/// 竖向排列
/// </summary>
/// <param name="ax"></param>
/// <param name="ay"></param>
public Matrix2x2(Vector2 ax, Vector2 ay)
{
M00 = ax.x;
M10 = ax.y;
M10 = ay.x;
M11 = ay.y;
}
/// <summary>
/// 2*2数组
/// </summary>
/// <param name="arr"></param>
public Matrix2x2(float[,] arr)
{
M00 = arr[0, 0];
M01 = arr[0, 1];
M10 = arr[1, 0];
M11 = arr[1, 1];
}
/// <summary>
/// 矩阵*vector2
/// </summary>
/// <param name="m2x2"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static Vector2 operator *(Matrix2x2 m2x2, Vector2 v2)
{
float x = Vector2.Dot(m2x2.Row0, v2);
float y = Vector2.Dot(m2x2.Row1, v2);
return new Vector2(x, y);
}
/// <summary>
/// 矩阵*矩阵
/// </summary>
/// <param name="m2x2a"></param>
/// <param name="m2x2b"></param>
/// <returns></returns>
public static Matrix2x2 operator *(Matrix2x2 m2x2a, Matrix2x2 m2x2b)
{
float c00 = Vector2.Dot(m2x2a.Row0, m2x2b.Column0);
float c01 = Vector2.Dot(m2x2a.Row0, m2x2b.Column1);
float c10 = Vector2.Dot(m2x2a.Row1, m2x2b.Column0);
float c11 = Vector2.Dot(m2x2a.Row1, m2x2b.Column1);
Matrix2x2 ret = new Matrix2x2(c00, c01, c10, c11);
return ret;
}
/// <summary>
/// 矩阵/标量
/// </summary>
/// <param name="m2x2"></param>
/// <param name="f"></param>
/// <returns></returns>
public static Matrix2x2 operator /(Matrix2x2 m2x2, float f)
{
for (int x = 0; x < ROW; x++)
{
for (int y = 0; y < COLUMN; y++)
{
m2x2[x, y] /= f;
}
}
return m2x2;
}
/// <summary>
/// 行列式
/// </summary>
/// <returns></returns>
public float GetDeterminant()
{
float det = M00 * M11 - M01 * M10;
return det;
}
/// <summary>
/// 求转置矩阵
/// </summary>
/// <returns></returns>
public Matrix2x2 GetTransposeMatrix()
{
Matrix2x2 m2x2T = new Matrix2x2();
for (int x = 0; x < ROW; x++)
{
for (int y = 0; y < COLUMN; y++)
{
m2x2T[x, y] = this[y, x];
}
}
return m2x2T;
}
/// <summary>
/// 求余子式标量
/// </summary>
/// <param name="r"></param>
/// <param name="c"></param>
/// <returns></returns>
public float GetCofactorScalar(int r, int c)
{
float cof = 0f;
for (int x = 0; x < ROW; x++)
{
for (int y = 0; y < COLUMN; y++)
{
if (x != r && y != c)
{
cof = dataArr[x, y];
break;
}
}
}
return cof;
}
/// <summary>
/// 求余子式标量矩阵
/// + -
/// - +
/// </summary>
/// <returns></returns>
public Matrix2x2 GetCofactorScalarMatrix()
{
Matrix2x2 m2x2 = new Matrix2x2();
for (int x = 0; x < ROW; x++)
{
for (int y = 0; y < COLUMN; y++)
{
float cof = GetCofactorScalar(x, y);
bool ispostive = (x + y) % 2 == 0;
m2x2[x, y] = ispostive ? cof : -cof;
}
}
return m2x2;
}
/// <summary>
/// 求伴随矩阵
/// 算法:余子式标量矩阵的转置
/// </summary>
/// <returns></returns>
public Matrix2x2 GetAdjointMatrix()
{
Matrix2x2 m2x2 = GetCofactorScalarMatrix();
Matrix2x2 m2x2T = m2x2.GetTransposeMatrix();
return m2x2T;
}
/// <summary>
/// 求逆矩阵
/// 算法:伴随矩阵/行列式值
/// </summary>
/// <returns></returns>
public Matrix2x2 GetInverseMatrix()
{
Matrix2x2 m2x2 = GetAdjointMatrix();
float det = GetDeterminant();
Matrix2x2 m2x2I = m2x2 / det;
return m2x2I;
}
public override string ToString()
{
string ret = $"换行\nM00:{M00} M01:{M01} \nM10:{M10} M11:{M11}";
return ret;
}
}
关于Matrix2x2,我设计了构造、转置、余子式(2x2矩阵的余子式为标量,或称1x1矩阵)、余子式标量矩阵、伴随矩阵和逆矩阵。
基本上数学运算开发够用了,每个函数的意义只在代码注释上简单说明。
这里只举一个例子:逆矩阵可以将矩阵变换后向量再变换回来,比如:
Matrix2x2 m2x2 = new Matrix2x2();
m2x2.M00 = 0.3f;
m2x2.M01 = 1.2f;
m2x2.M10 = 5.2f;
m2x2.M11 = -1f;
Debug.LogErrorFormat($"m2x2 = {m2x2}");
Vector2 vec0 = new Vector2(5.8f, 56.1f);
Vector2 vec1 = m2x2 * vec0;
Matrix2x2 m2x2I = m2x2.GetInverseMatrix();
Debug.LogErrorFormat($"m2x2I = {m2x2I}");
Vector2 vec2 = m2x2I * vec1;
Debug.LogErrorFormat($"vec0 = {vec0} vec1 = {vec1} vec2 = {vec2}");
结果:
接下来扩展Matrix3x3:
using NPOI.SS.Formula.Functions;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
[System.Serializable]
public class Matrix3x3
{
#region ///properties
public const int ROW = 3;
public const int COLUMN = 3;
public float M00
{
get { return dataArr[0, 0]; }
set { dataArr[0, 0] = value; }
}
public float M01
{
get { return dataArr[0, 1]; }
set { dataArr[0, 1] = value; }
}
public float M02
{
get { return dataArr[0, 2]; }
set { dataArr[0, 2] = value; }
}
public float M10
{
get { return dataArr[1, 0]; }
set { dataArr[1, 0] = value; }
}
public float M11
{
get { return dataArr[1, 1]; }
set { dataArr[1, 1] = value; }
}
public float M12
{
get { return dataArr[1, 2]; }
set { dataArr[1, 2] = value; }
}
public float M20
{
get { return dataArr[2, 0]; }
set { dataArr[2, 0] = value; }
}
public float M21
{
get { return dataArr[2, 1]; }
set { dataArr[2, 1] = value; }
}
public float M22
{
get { return dataArr[2, 2]; }
set { dataArr[2, 2] = value; }
}
public float[,] dataArr = new float[ROW, COLUMN];
public Vector3 Row0 { get { return new Vector3(M00, M01, M02); } }
public Vector3 Row1 { get { return new Vector3(M10, M11, M12); } }
public Vector3 Row2 { get { return new Vector3(M20, M21, M22); } }
public Vector3 Column0 { get { return new Vector3(M00, M10, M20); } }
public Vector3 Column1 { get { return new Vector3(M01, M11, M21); } }
public Vector3 Column2 { get { return new Vector3(M02, M12, M22); } }
#endregion
public float this[int row, int col]
{
get { return dataArr[row, col]; }
set { dataArr[row, col] = value; }
}
public Matrix3x3() { }
public Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
{
M00 = m00;
M01 = m01;
M02 = m02;
M10 = m10;
M11 = m11;
M12 = m12;
M20 = m20;
M21 = m21;
M22 = m22;
}
/// <summary>
/// xyz基向量排列
/// </summary>
/// <param name="ax">x基向量</param>
/// <param name="ay">y基向量</param>
/// <param name="az">z基向量</param>
public Matrix3x3(Vector3 ax, Vector3 ay, Vector3 az)
{
M00 = ax.x;
M10 = ax.y;
M20 = ax.z;
M01 = ay.x;
M11 = ay.y;
M21 = ay.z;
M02 = az.x;
M12 = az.y;
M22 = az.z;
}
/// <summary>
/// 数组排列
/// </summary>
/// <param name="arr"></param>
public Matrix3x3(float[,] arr)
{
M00 = arr[0, 0];
M01 = arr[0, 1];
M02 = arr[0, 2];
M10 = arr[1, 0];
M11 = arr[1, 1];
M12 = arr[1, 2];
M20 = arr[2, 0];
M21 = arr[2, 1];
M22 = arr[2, 2];
}
/// <summary>
/// 矩阵*vector2
/// </summary>
/// <param name="m3x3"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static Vector2 operator *(Matrix3x3 m3x3, Vector2 v2)
{
Vector3 v3 = new Vector3(v2.x, v2.y, 1);
v3 = m3x3 * v3;
v2 = new Vector2(v3.x, v3.y);
return v2;
}
/// <summary>
/// 矩阵*vector3
/// </summary>
/// <param name="m3x3"></param>
/// <param name="v3"></param>
/// <returns></returns>
public static Vector3 operator *(Matrix3x3 m3x3, Vector3 v3)
{
float x = Vector3.Dot(m3x3.Row0, v3);
float y = Vector3.Dot(m3x3.Row1, v3);
float z = Vector3.Dot(m3x3.Row2, v3);
return new Vector3(x, y, z);
}
/// <summary>
/// 矩阵*矩阵
/// </summary>
/// <param name="m3x3a"></param>
/// <param name="m3x3b"></param>
/// <returns></returns>
public static Matrix3x3 operator *(Matrix3x3 m3x3a, Matrix3x3 m3x3b)
{
float c00 = Vector2.Dot(m3x3a.Row0, m3x3b.Column0);
float c01 = Vector2.Dot(m3x3a.Row0, m3x3b.Column1);
float c02 = Vector2.Dot(m3x3a.Row0, m3x3b.Column2);
float c10 = Vector2.Dot(m3x3a.Row1, m3x3b.Column0);
float c11 = Vector2.Dot(m3x3a.Row1, m3x3b.Column1);
float c12 = Vector2.Dot(m3x3a.Row1, m3x3b.Column2);
float c20 = Vector2.Dot(m3x3a.Row2, m3x3b.Column0);
float c21 = Vector2.Dot(m3x3a.Row2, m3x3b.Column1);
float c22 = Vector2.Dot(m3x3a.Row2, m3x3b.Column2);
Matrix3x3 ret = new Matrix3x3(c00, c01, c02, c10, c11, c12, c20, c21, c22);
return ret;
}
/// <summary>
/// 矩阵/标量
/// </summary>
/// <param name="m3x3"></param>
/// <param name="f"></param>
/// <returns></returns>
public static Matrix3x3 operator /(Matrix3x3 m3x3, float f)
{
for (int x = 0; x < ROW; x++)
{
for (int y = 0; y < COLUMN; y++)
{
m3x3[x, y] /= f;
}
}
return m3x3;
}
/// <summary>
/// 求行列式
/// </summary>
/// <returns></returns>
public float GetDeterminant()
{
float det = M00 * M11 * M22 + M01 * M12 * M20 + M02 * M10 * M21 - M02 * M11 * M20 - M01 * M10 * M22 - M00 * M12 * M21;
return det;
}
/// <summary>
/// 求转置矩阵
/// </summary>
/// <returns></returns>
public Matrix3x3 GetTransposeMatrix()
{
Matrix3x3 m3x3T = new Matrix3x3();
for (int x = 0; x < ROW; x++)
{
for (int y = 0; y < COLUMN; y++)
{
m3x3T[x, y] = this[y, x];
}
}
return m3x3T;
}
/// <summary>
/// 求余子式矩阵
/// </summary>
/// <param name="r"></param>
/// <param name="c"></param>
/// <returns></returns>
public Matrix2x2 GetCofactorMatrix(int r, int c)
{
Matrix2x2 m2x2 = new Matrix2x2();
for (int x = 0; x < ROW; x++)
{
for (int y = 0; y < COLUMN; y++)
{
if (x != r && y != c)
{
int row = x > r ? x - 1 : x;
int col = y > c ? y - 1 : y;
m2x2[row, col] = this[x, y];
}
}
}
return m2x2;
}
/// <summary>
/// 求代数余子式(余子式矩阵的行列式)矩阵
/// 余子式矩阵行列式正负号
/// + - +
/// - + -
/// + - +
/// </summary>
/// <returns></returns>
public Matrix3x3 GetCofactorDeterminantMatrix()
{
Matrix3x3 m3x3 = new Matrix3x3();
for (int x = 0; x < ROW; x++)
{
for (int y = 0; y < COLUMN; y++)
{
Matrix2x2 m2x2 = GetCofactorMatrix(x, y);
float m2x2det = m2x2.GetDeterminant();
bool ispostive = (x + y) % 2 == 0;
m3x3[x, y] = ispostive ? m2x2det : -m2x2det;
}
}
return m3x3;
}
/// <summary>
/// 求伴随矩阵
/// 算法:代数余子式矩阵的转置
/// </summary>
/// <returns></returns>
public Matrix3x3 GetAdjointMatrix()
{
Matrix3x3 m3x3 = GetCofactorDeterminantMatrix();
Matrix3x3 m3x3T = m3x3.GetTransposeMatrix();
return m3x3T;
}
/// <summary>
/// 求逆矩阵
/// 算法:伴随矩阵/行列式值
/// </summary>
/// <returns></returns>
public Matrix3x3 GetInverseMatrix()
{
Matrix3x3 m3x3 = GetAdjointMatrix();
float det = GetDeterminant();
Matrix3x3 m3x3I = m3x3 / det;
return m3x3I;
}
public override string ToString()
{
string ret = $"换行\nM00:{M00} M01:{M01} M02:{M02} \nM10:{M10} M11:{M11} M12:{M12} \nM20:{M20} M21:{M21} M12:{M22}";
return ret;
}
}
还是用逆矩阵验证一下:
Matrix3x3 m3x3 = new Matrix3x3();
m3x3.M00 = -0.3f;
m3x3.M01 = 6.2f;
m3x3.M02 = 12.6f;
m3x3.M10 = 5.2f;
m3x3.M11 = -1.8f;
m3x3.M12 = 7.8f;
m3x3.M20 = -52.2f;
m3x3.M21 = 6.4f;
m3x3.M22 = -70.1f;
Debug.LogErrorFormat($"m3x3 = {m3x3}");
Vector3 vec0 = new Vector3(20.3f, -54f, 4.4f);
Vector3 vec1 = m3x3 * vec0;
Matrix3x3 m3x3I = m3x3.GetInverseMatrix();
Debug.LogErrorFormat($"m3x3I = {m3x3I}");
Vector3 vec2 = m3x3I * vec1;
Debug.LogErrorFormat($"vec0 = {vec0} vec1 = {vec1} vec2 = {vec2}");
结果:
OK,洗了睡,这里吐槽一下:这些矩阵计算unity应该直接提供,写起来眼睛都要看瞎了。