diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp index 7efac12f78..9581a5a4ce 100644 --- a/Source/Core/Common/Matrix.cpp +++ b/Source/Core/Common/Matrix.cpp @@ -84,6 +84,23 @@ Matrix33 Matrix33::RotateZ(float rad) return mtx; } +Matrix33 Matrix33::Rotate(float rad, const Vec3& axis) +{ + const float s = std::sin(rad); + const float c = std::cos(rad); + Matrix33 mtx; + mtx.data[0] = axis.x * axis.x * (1 - c) + c; + mtx.data[1] = axis.x * axis.y * (1 - c) - axis.z * s; + mtx.data[2] = axis.x * axis.z * (1 - c) + axis.y * s; + mtx.data[3] = axis.y * axis.x * (1 - c) + axis.z * s; + mtx.data[4] = axis.y * axis.y * (1 - c) + c; + mtx.data[5] = axis.y * axis.z * (1 - c) - axis.x * s; + mtx.data[6] = axis.z * axis.x * (1 - c) - axis.y * s; + mtx.data[7] = axis.z * axis.y * (1 - c) + axis.x * s; + mtx.data[8] = axis.z * axis.z * (1 - c) + c; + return mtx; +} + Matrix33 Matrix33::Scale(const Vec3& vec) { Matrix33 mtx = {}; diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h index bf23963b3d..ed58abdccb 100644 --- a/Source/Core/Common/Matrix.h +++ b/Source/Core/Common/Matrix.h @@ -20,6 +20,10 @@ union TVec3 TVec3() = default; TVec3(T _x, T _y, T _z) : data{_x, _y, _z} {} + TVec3 Cross(const TVec3& rhs) const + { + return {(y * rhs.z) - (rhs.y * z), (z * rhs.x) - (rhs.z * x), (x * rhs.y) - (rhs.x * y)}; + } T Dot(const TVec3& other) const { return x * other.x + y * other.y + z * other.z; } T LengthSquared() const { return Dot(*this); } T Length() const { return std::sqrt(LengthSquared()); } @@ -275,6 +279,8 @@ public: static Matrix33 RotateY(float rad); static Matrix33 RotateZ(float rad); + static Matrix33 Rotate(float rad, const Vec3& axis); + static Matrix33 Scale(const Vec3& vec); // set result = a x b