#include "dl_matrix.h" #include "base/dl_string.h" namespace dl::Mat { void Test() { //平移矩阵 Matrix4 mat0 = Mat::CreateTranslation({ 7, 13, 17 }); Matrix4 mat1({ 1, 0, 0, 7, 0, 1, 0, 13, 0, 0, 1, 17, 0, 0, 0, 1 }); assert(mat0 == mat1); Position4 pos = { 10, 100, 1000, 1}; pos = mat0 * pos; log(to_string(pos)); assert((pos == Position4{17, 113, 1017, 1})); //矩阵乘法 Matrix mat2({ 1, 2, 3, 4, 5, 6 }); Matrix mat3({ 1, 2, 3, 4, 5, 6, 7, 8 }); Matrix mat4 = mat2 * mat3; decltype(mat4) mat5{ 11, 14, 17, 20, 23, 30, 37, 44, 35, 46, 57, 68 }; assert(mat4 == mat5); mat0.Dump(); mat2.Dump(); mat3.Dump(); mat4.Dump(); //方阵自乘 mat0 *= mat1; mat0.Dump(); Matrix4 mat6({ 1, 0, 0, 14, 0, 1, 0, 26, 0, 0, 1, 34, 0, 0, 0, 1 }); assert(mat4 == mat5); //逆矩阵 Matrix4 mat7 = Mat::GetInverse(mat6); mat7.Dump(); Matrix4 mat8({ 1, 0, 0, -14, 0, 1, 0, -26, 0, 0, 1, -34, 0, 0, 0, 1 }); assert(mat7 == mat8); } dl::Matrix4 CreateTranslation(const Position3& t) { Matrix4 m; m[0][0] = 1_f; m[1][0] = 0; m[2][0] = 0; m[3][0] = t[0]; m[0][1] = 0; m[1][1] = 1_f; m[2][1] = 0; m[3][1] = t[1]; m[0][2] = 0; m[1][2] = 0; m[2][2] = 1_f; m[3][2] = t[2]; m[0][3] = 0; m[1][3] = 0; m[2][3] = 0; m[3][3] = 1_f; return m; } void ApplyTranslation(Matrix4& m, const Position3& t) { m[3] = m[0] * t[0] + m[1] * t[1] + m[2] * t[2] + m[3]; } dl::Matrix4 CreateRotation(const Position3& axis, Float angle) { using namespace Math; Float a = angle; Float c = cos(a); Float s = sin(a); Position3 n = Normalize(axis); Position3 t = n * (1_f - c); Matrix4 m; m[0][0] = c + t[0] * n[0]; m[0][1] = t[0] * n[1] + s * n[2]; m[0][2] = t[0] * n[2] - s * n[1]; m[0][3] = 0; m[1][0] = t[1] * n[0] - s * n[2]; m[1][1] = c + t[1] * n[1]; m[1][2] = t[1] * n[2] + s * n[0]; m[1][3] = 0; m[2][0] = t[2] * n[0] + s * n[1]; m[2][1] = t[2] * n[1] - s * n[0]; m[2][2] = c + t[2] * n[2]; m[2][3] = 0; m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1_f; return m; } dl::Matrix4 CreateRotation(const Position4& q) { Matrix4 m; Float q00 = q[0] * q[0]; Float q11 = q[1] * q[1]; Float q12 = q[1] * q[2]; Float q03 = q[0] * q[3]; Float q13 = q[1] * q[3]; Float q02 = q[0] * q[2]; Float q22 = q[2] * q[2]; Float q23 = q[2] * q[3]; Float q01 = q[0] * q[1]; Float q33 = q[3] * q[3]; m[0][0] = 2_f * (q00 + q11) - 1_f; m[1][0] = 2_f * (q12 - q03); m[2][0] = 2_f * (q13 + q02); m[3][0] = 0; m[0][1] = 2_f * (q12 + q03); m[1][1] = 2_f * (q00 + q22) - 1_f; m[2][1] = 2_f * (q23 - q01); m[3][1] = 0; m[0][2] = 2_f * (q13 - q02); m[1][2] = 2_f * (q23 + q01); m[2][2] = 2_f * (q00 + q33) - 1; m[3][2] = 0; m[0][3] = 0; m[1][3] = 0; m[2][3] = 0; m[3][3] = 1_f; return m; } dl::Matrix4 CreateScaling(const Position3& s) { Matrix4 m; m[0][0] = s[0]; m[1][0] = 0; m[2][0] = 0; m[3][0] = 0; m[0][1] = 0; m[1][1] = s[1]; m[2][1] = 0; m[3][1] = 0; m[0][2] = 0; m[1][2] = 0; m[2][2] = s[2]; m[3][2] = 0; m[0][3] = 0; m[1][3] = 0; m[2][3] = 0; m[3][3] = 1_f; return m; } dl::Matrix4 CreateLookTo(const Position3& eye, const Position3& dir, const Position3& up) { using namespace Math; Position3 f = Normalize(dir); Position3 s = Normalize(CrossProduct(f, up)); Position3 u = CrossProduct(s, f); Matrix4 m; m[0][0] = s[0]; m[1][0] = s[1]; m[2][0] = s[2]; m[3][0] = -DotProduct(s, eye); m[0][1] = u[0]; m[1][1] = u[1]; m[2][1] = u[2]; m[3][1] = -DotProduct(u, eye); m[0][2] = -f[0]; m[1][2] = -f[1]; m[2][2] = -f[2]; m[3][2] = DotProduct(f, eye); m[0][3] = 0; m[1][3] = 0; m[2][3] = 0; m[3][3] = 1_f; return m; } dl::Matrix4 CreatePerspective(Float fovy, Float aspect, Float z_near, Float z_far) { assert(std::abs(aspect - std::numeric_limits::epsilon()) > 0); Float x = tan(fovy / 2_f); Matrix4 m; m[0][0] = 1_f / (aspect * x); m[1][0] = 0; m[2][0] = 0; m[3][0] = 0; m[0][1] = 0; m[1][1] = 1_f / x; m[2][1] = 0; m[3][1] = 0; m[0][2] = 0; m[1][2] = 0; m[2][2] = z_far / (z_near - z_far); m[3][2] = -(z_far * z_near) / (z_far - z_near); m[0][3] = 0; m[1][3] = 0; m[2][3] = -1_f; m[3][3] = 0; return m; } dl::Matrix4 CreateOrthogonal(Float left, Float top, Float right, Float bottom, Float z_near, Float z_far) { Matrix4 m; m[0][0] = 2_f / (right - left); m[1][0] = 0; m[2][0] = 0; m[3][0] = -(right + left) / (right - left); m[0][1] = 0; m[1][1] = 2_f / (top - bottom); m[2][1] = 0; m[3][1] = -(top + bottom) / (top - bottom); m[0][2] = 0; m[1][2] = 0; m[2][2] = -1_f / (z_far - z_near); m[3][2] = -z_near / (z_far - z_near); m[0][3] = 0; m[1][3] = 0; m[2][3] = 0; m[3][3] = 1_f; return m; } dl::Matrix4 GetInverse(const Matrix4& m) { Float Coef00 = m[2][2] * m[3][3] - m[3][2] * m[2][3]; Float Coef02 = m[1][2] * m[3][3] - m[3][2] * m[1][3]; Float Coef03 = m[1][2] * m[2][3] - m[2][2] * m[1][3]; Float Coef04 = m[2][1] * m[3][3] - m[3][1] * m[2][3]; Float Coef06 = m[1][1] * m[3][3] - m[3][1] * m[1][3]; Float Coef07 = m[1][1] * m[2][3] - m[2][1] * m[1][3]; Float Coef08 = m[2][1] * m[3][2] - m[3][1] * m[2][2]; Float Coef10 = m[1][1] * m[3][2] - m[3][1] * m[1][2]; Float Coef11 = m[1][1] * m[2][2] - m[2][1] * m[1][2]; Float Coef12 = m[2][0] * m[3][3] - m[3][0] * m[2][3]; Float Coef14 = m[1][0] * m[3][3] - m[3][0] * m[1][3]; Float Coef15 = m[1][0] * m[2][3] - m[2][0] * m[1][3]; Float Coef16 = m[2][0] * m[3][2] - m[3][0] * m[2][2]; Float Coef18 = m[1][0] * m[3][2] - m[3][0] * m[1][2]; Float Coef19 = m[1][0] * m[2][2] - m[2][0] * m[1][2]; Float Coef20 = m[2][0] * m[3][1] - m[3][0] * m[2][1]; Float Coef22 = m[1][0] * m[3][1] - m[3][0] * m[1][1]; Float Coef23 = m[1][0] * m[2][1] - m[2][0] * m[1][1]; Position4 Fac0{ Coef00, Coef00, Coef02, Coef03 }; Position4 Fac1{ Coef04, Coef04, Coef06, Coef07 }; Position4 Fac2{ Coef08, Coef08, Coef10, Coef11 }; Position4 Fac3{ Coef12, Coef12, Coef14, Coef15 }; Position4 Fac4{ Coef16, Coef16, Coef18, Coef19 }; Position4 Fac5{ Coef20, Coef20, Coef22, Coef23 }; Position4 Vec0{ m[1][0], m[0][0], m[0][0], m[0][0] }; Position4 Vec1{ m[1][1], m[0][1], m[0][1], m[0][1] }; Position4 Vec2{ m[1][2], m[0][2], m[0][2], m[0][2] }; Position4 Vec3{ m[1][3], m[0][3], m[0][3], m[0][3] }; Position4 Inv0{ Vec1 * Fac0 - Vec2 * Fac1 + Vec3 * Fac2 }; Position4 Inv1{ Vec0 * Fac0 - Vec2 * Fac3 + Vec3 * Fac4 }; Position4 Inv2{ Vec0 * Fac1 - Vec1 * Fac3 + Vec3 * Fac5 }; Position4 Inv3{ Vec0 * Fac2 - Vec1 * Fac4 + Vec2 * Fac5 }; Position4 SignA{ +1, -1, +1, -1 }; Position4 SignB{ -1, +1, -1, +1 }; Matrix4 ret; ret[0] = Inv0 * SignA; ret[1] = Inv1 * SignB; ret[2] = Inv2 * SignA; ret[3] = Inv3 * SignB; Position4 Row0{ ret[0][0], ret[1][0], ret[2][0], ret[3][0] }; Position4 Dot0{ m[0] * Row0 }; Float Dot1 = (Dot0[0] + Dot0[1]) + (Dot0[2] + Dot0[3]); Float OneOverDeterminant = (Float)1 / Dot1; ret *= OneOverDeterminant; return ret; } }