311 lines
6.9 KiB
C++
311 lines
6.9 KiB
C++
#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<int, 3, 2> mat2({
|
|
1, 2,
|
|
3, 4,
|
|
5, 6
|
|
});
|
|
Matrix<int, 2, 4> 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;
|
|
}
|
|
|
|
// 投影矩阵依赖于图形库的NDC定义 观察空间 -> NDC空间
|
|
// Vulkan NDC Y向下 Z范围[0, 1]
|
|
dl::Matrix4 CreatePerspective(Float fovy, Float aspect, Float z_near, Float z_far)
|
|
{
|
|
assert(std::abs(aspect - std::numeric_limits<Float>::epsilon()) > 0);
|
|
|
|
Float x = std::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 / (bottom - top);
|
|
m[2][1] = 0;
|
|
m[3][1] = -(bottom + top) / (bottom - top);
|
|
|
|
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;
|
|
}
|
|
|
|
}
|
|
|