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)); }