/// <summary>
/// 可以用来计算ojbtoworld矩阵
/// </summary>
/// <param name="pos"></param>
/// <param name="q"></param>
/// <param name="s"></param>
/// <returns></returns>
public static Matrix4x4RightHand TRS(Vector3RightHand pos, QuaternionRightHand q, Vector3RightHand s)
{
Matrix4x4RightHand posMatrix = Matrix4x4RightHand.identity;
posMatrix.m03 = pos.x;
posMatrix.m13 = pos.y;
posMatrix.m23 = pos.z;
Matrix4x4RightHand rotateMatrix = Matrix4x4RightHand.identity;
rotateMatrix.m00 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
rotateMatrix.m10 = 2 * q.x * q.y + 2 * q.w * q.z;
rotateMatrix.m20 = 2 * q.x * q.z - 2 * q.w * q.y;
rotateMatrix.m01 = 2 * q.x * q.y - 2 * q.w * q.z;
rotateMatrix.m11 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
rotateMatrix.m21 = 2 * q.y * q.z + 2 * q.w * q.x;
rotateMatrix.m02 = 2 * q.x * q.z + 2 * q.w * q.y;
rotateMatrix.m12 = 2 * q.y * q.z - 2 * q.w * q.x;
rotateMatrix.m22 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
Matrix4x4RightHand scaleMatrix = Scale(s);
Matrix4x4RightHand ret = posMatrix * rotateMatrix * scaleMatrix;
return(ret);
}
public static Matrix4x4RightHand Perspective(float fov, float aspect, float zNear, float zFar)
{
// 为什么这里用的是opengl的透视矩阵？
Matrix4x4RightHand ret = Matrix4x4RightHand.zero;
//ret.m00 = 1 / (float)Math.Tan(fov * 0.5f) / aspect;
//ret.m11 = 1 / (float)Math.Tan(fov * 0.5f);
//ret.m22 = zFar / (zFar - zNear);
//ret.m23 = 1;
//ret.m32 = zFar * zNear / (zNear - zFar);
float fovRad = Mathf.Deg2Rad * fov;
//
// float width = (float)Math.Tan(fovRad / 2) * zNear * 2;
// float height = width / aspect;
// float fovYRad = (float)Math.Atan(height / 2 / zNear) * 2;
//
// float fovY = Math3d.Rad2Deg * fovYRad;
ret.m00 = 1 / (float)Mathf.Tan(fovRad * 0.5f) / aspect;
ret.m11 = 1 / (float)Mathf.Tan(fovRad * 0.5f);
ret.m22 = -(zFar + zNear) / (zFar - zNear);
ret.m23 = -2 * zNear * zFar / (zFar - zNear);
ret.m32 = -1;
return(ret);
}
public override bool Equals(object other)
{
if (!(other is Matrix4x4RightHand))
{
return(false);
}
Matrix4x4RightHand matrix4x = (Matrix4x4RightHand)other;
return(this.GetColumn(0).Equals(matrix4x.GetColumn(0)) && this.GetColumn(1).Equals(matrix4x.GetColumn(1)) && this.GetColumn(2).Equals(matrix4x.GetColumn(2)) && this.GetColumn(3).Equals(matrix4x.GetColumn(3)));
}
public static Matrix4x4RightHand Transpose(Matrix4x4RightHand m)
{
Matrix4x4RightHand ret = new Matrix4x4RightHand();
ret.m00 = m.m00; ret.m01 = m.m10; ret.m02 = m.m20; ret.m03 = m.m30;
ret.m10 = m.m01; ret.m11 = m.m11; ret.m12 = m.m21; ret.m13 = m.m31;
ret.m20 = m.m02; ret.m21 = m.m12; ret.m22 = m.m22; ret.m23 = m.m32;
ret.m30 = m.m03; ret.m31 = m.m13; ret.m32 = m.m23; ret.m33 = m.m33;
return(ret);
}
public static Matrix4x4RightHand Inverse(Matrix4x4RightHand m)
{
float[][] mat = new float[4][];
for (int i = 0; i < 4; ++i)
{
mat[i] = new float[4];
}
mat[0][0] = m.m00;
mat[0][1] = m.m01;
mat[0][2] = m.m02;
mat[0][3] = m.m03;
mat[1][0] = m.m10;
mat[1][1] = m.m11;
mat[1][2] = m.m12;
mat[1][3] = m.m13;
mat[2][0] = m.m20;
mat[2][1] = m.m21;
mat[2][2] = m.m22;
mat[2][3] = m.m23;
mat[3][0] = m.m30;
mat[3][1] = m.m31;
mat[3][2] = m.m32;
mat[3][3] = m.m33;
float[][] inverseMat = _MatrixInverse(mat);
Matrix4x4RightHand ret = new Matrix4x4RightHand();
ret.m00 = inverseMat[0][0];
ret.m01 = inverseMat[0][1];
ret.m02 = inverseMat[0][2];
ret.m03 = inverseMat[0][3];
ret.m10 = inverseMat[1][0];
ret.m11 = inverseMat[1][1];
ret.m12 = inverseMat[1][2];
ret.m13 = inverseMat[1][3];
ret.m20 = inverseMat[2][0];
ret.m21 = inverseMat[2][1];
ret.m22 = inverseMat[2][2];
ret.m23 = inverseMat[2][3];
ret.m30 = inverseMat[3][0];
ret.m31 = inverseMat[3][1];
ret.m32 = inverseMat[3][2];
ret.m33 = inverseMat[3][3];
return(ret);
}
/// <summary>
/// 将向量from向向量to旋转角度angle
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="angle"></param>
/// <returns></returns>
static Vector3RightHand RotateTo(Vector3RightHand from, Vector3RightHand to, float angle)
{
//如果两向量角度为0
if (Vector3RightHand.Angle(from, to) == 0)
{
return(from);
}
//旋转轴
Vector3RightHand n = Vector3RightHand.Cross(from, to);
//旋转轴规范化
n.Normalize();
//旋转矩阵
Matrix4x4RightHand rotateMatrix = new Matrix4x4RightHand();
//旋转的弧度
double radian = angle * Mathf.PI / 180;
float cosAngle = (float)Mathf.Cos((float)radian);
float sinAngle = (float)Mathf.Sin((float)radian);
//矩阵的数据
//这里看不懂的自行科普矩阵知识
rotateMatrix.SetRow(0, new Vector4(n.x * n.x * (1 - cosAngle) + cosAngle, n.x * n.y * (1 - cosAngle) + n.z * sinAngle, n.x * n.z * (1 - cosAngle) - n.y * sinAngle, 0));
rotateMatrix.SetRow(1, new Vector4(n.x * n.y * (1 - cosAngle) - n.z * sinAngle, n.y * n.y * (1 - cosAngle) + cosAngle, n.y * n.z * (1 - cosAngle) + n.x * sinAngle, 0));
rotateMatrix.SetRow(2, new Vector4(n.x * n.z * (1 - cosAngle) + n.y * sinAngle, n.y * n.z * (1 - cosAngle) - n.x * sinAngle, n.z * n.z * (1 - cosAngle) + cosAngle, 0));
rotateMatrix.SetRow(3, new Vector4(0, 0, 0, 1));
Vector4 v = Vector3RightHand.ToVector4(from);
Vector3RightHand vector = new Vector3RightHand();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; j++)
{
vector[i] += v[j] * rotateMatrix[j, i];
}
}
return(vector);
}
public static void Test()
{
Vector3RightHand pos = new Vector3RightHand(100, 200, 300);
QuaternionRightHand rotate = QuaternionRightHand.identity;
rotate.eulerAngles = new Vector3RightHand(40, 50, 60);
//Console.WriteLine("测试Quaternion " + rotate);
Vector3RightHand scale = new Vector3RightHand(7, 8, 9);
Matrix4x4RightHand trs = TRS(pos, rotate, scale);
Console.WriteLine("测试trs\n" + trs.ToString());
Matrix4x4RightHand transpose = Transpose(trs);
Console.WriteLine("测试转置矩阵\n" + transpose.ToString());
Matrix4x4RightHand inverse = Inverse(trs);
Console.WriteLine("测试逆矩阵\n" + inverse.ToString());
Matrix4x4RightHand posMatrix = Matrix4x4RightHand.identity;
posMatrix.m03 = pos.x;
posMatrix.m13 = pos.y;
posMatrix.m23 = pos.z;
Vector4 point = new Vector4(0, 0, 0, 1);
Vector4 newPoint = posMatrix * point;
float fov = 60;
float aspect = (float)16 / 9;
float zNear = 1;
float zFar = 1000;
Matrix4x4RightHand perspectiveMat = Matrix4x4RightHand.Perspective(fov, aspect, zNear, zFar);
Console.WriteLine("测试透视矩阵\n" + perspectiveMat.ToString());
//Console.ReadLine();
}
