public OBB3d GetOBB()
{
Matrix4x4L obj2World = GetObj2WorldMatrix();
OBB3d obb = new OBB3d(obj2World * m_aabb.m_pos, m_rotation, m_aabb.m_size.x, m_aabb.m_size.y, m_aabb.m_size.z);
return(obb);
}
public static Matrix4x4L TRS(Vector3L pos, QuaternionL q, Vector3L s)
{
Matrix4x4L posMatrix = Matrix4x4L.identity;
posMatrix.m03 = pos.x;
posMatrix.m13 = pos.y;
posMatrix.m23 = pos.z;
Matrix4x4L rotateMatrix = Matrix4x4L.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;
Matrix4x4L scaleMatrix = Scale(s);
Matrix4x4L ret = posMatrix * rotateMatrix * scaleMatrix;
return(ret);
}
public override bool Equals(object other)
{
if (!(other is Matrix4x4L))
{
return(false);
}
Matrix4x4L matrix4x = (Matrix4x4L)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 Matrix4x4L Transpose(Matrix4x4L m)
{
Matrix4x4L ret = new Matrix4x4L();
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 Matrix4x4L Inverse(Matrix4x4L m)
{
FloatL[][] mat = new FloatL[4][];
for (int i = 0; i < 4; ++i)
{
mat[i] = new FloatL[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;
FloatL[][] inverseMat = _MatrixInverse(mat);
Matrix4x4L ret = new Matrix4x4L();
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);
}
public List <Vector2L> GetRotateList()
{
List <Vector2L> ret = new List <Vector2L>();
Matrix4x4L trs = Matrix4x4L.TRS(Vector3L.zero, m_rotation, Vector3L.one);
foreach (var iter in m_pointList)
{
Vector3L objPoint3d = new Vector3L(iter.x, 0, iter.y);
Vector3L worldPoint3d = trs * objPoint3d;
ret.Add(new Vector2L(worldPoint3d.x, worldPoint3d.z));
}
return(ret);
}
public static Matrix4x4L Perspective(FloatL fov, FloatL aspect /*纵横比*/, FloatL zNear, FloatL zFar)
{
// 为什么这里用的是opengl的透视矩阵？
Matrix4x4L ret = Matrix4x4L.zero;
FloatL fovRad = FixPointMath.Deg2Rad * fov;
ret.m00 = 1 / FixPointMath.Tan(fovRad * 0.5f) / aspect;
ret.m11 = 1 / FixPointMath.Tan(fovRad * 0.5f);
ret.m22 = -(zFar + zNear) / (zFar - zNear);
ret.m23 = -2 * zNear * zFar / (zFar - zNear);
ret.m32 = -1;
return(ret);
}
public FloatL[] GetOrientationMatrixAsArray()
{
Matrix4x4L trs = Matrix4x4L.TRS(Vector3L.zero, m_rotation, Vector3L.one);
FloatL[] matrixArray = new FloatL[9];
matrixArray[0] = trs.m00;
matrixArray[1] = trs.m01;
matrixArray[2] = trs.m02;
matrixArray[3] = trs.m10;
matrixArray[4] = trs.m11;
matrixArray[5] = trs.m12;
matrixArray[6] = trs.m20;
matrixArray[7] = trs.m21;
matrixArray[8] = trs.m22;
return(matrixArray);
}
static Vector3L RotateTo(Vector3L from, Vector3L to, FloatL angle)
{
//如果两向量角度为0
if (Vector3L.Angle(from, to) == 0)
{
return(from);
}
//旋转轴
Vector3L n = Vector3L.Cross(from, to);
//旋转轴规范化
n.Normalize();
//旋转矩阵
Matrix4x4L rotateMatrix = new Matrix4x4L();
//旋转的弧度
double radian = (angle * FixPointMath.PI / 180).ToDouble();
FloatL cosAngle = FixPointMath.Cos(radian);
FloatL sinAngle = FixPointMath.Sin(radian);
//矩阵的数据
//这里看不懂的自行科普矩阵知识
rotateMatrix.SetRow(0, new Vector4L(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 Vector4L(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 Vector4L(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 Vector4L(0, 0, 0, 1));
Vector4L v = Vector3L.ToVector4(from);
Vector3L vector = new Vector3L();
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 Vector3L ClosestPointOfPoint3dWithOBB3d(Vector3L point, OBB3d obb)
{
//Vector3L objMin = obb.GetAABBMin();
//Vector3L objMax = obb.GetAABBMax();
Matrix4x4L obj2World = obb.GetObjToWorld();
Matrix4x4L worldToObj = obj2World.inverse;
Vector3L objMin = (Vector3L)(worldToObj * obb.m_pos) - obb.GetHalfSize();
Vector3L objMax = (Vector3L)(worldToObj * obb.m_pos) + obb.GetHalfSize();
Vector3L objPoint = worldToObj * point;
Vector3L objResult = objPoint;
objResult.x = (objResult.x < objMin.x) ? objMin.x : objResult.x;
objResult.y = (objResult.y < objMin.x) ? objMin.y : objResult.y;
objResult.z = (objResult.z < objMin.x) ? objMin.z : objResult.z;
objResult.x = (objResult.x > objMax.x) ? objMax.x : objResult.x;
objResult.y = (objResult.y > objMax.x) ? objMax.y : objResult.y;
objResult.z = (objResult.z > objMax.x) ? objMax.z : objResult.z;
Vector3L worldResult = obj2World * objResult;
return(worldResult);
}
/// <summary>
/// 从矩阵中提取Quaternion
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public static QuaternionL GetRotationFromMatrix(Matrix4x4L m)
{
return(QuaternionL.LookRotation(m.GetColumn(2), m.GetColumn(1)));
}
public Matrix4x4L GetObj2WorldMatrix()
{
return(Matrix4x4L.TRS(m_pos, m_rotation, Vector3L.one));
}
