diff --git a/src/core/math/matrix3x3.cpp b/src/core/math/matrix3x3.cpp index c7d766306..7f3fca9fc 100644 --- a/src/core/math/matrix3x3.cpp +++ b/src/core/math/matrix3x3.cpp @@ -33,6 +33,57 @@ Matrix3x3 from_axes(const Vector3 &x, const Vector3 &y, const Vector3 &z) return m; } +Matrix3x3 from_x_axis_angle(f32 angle) +{ + Matrix3x3 m; + m.x.x = 1.0f; + m.x.y = 0.0f; + m.x.z = 0.0f; + + m.y.x = 0.0f; + m.y.y = fcos(angle); + m.y.z = fsin(angle); + + m.z.x = 0.0f; + m.z.y = -fsin(angle); + m.z.z = fcos(angle); + return m; +} + +Matrix3x3 from_y_axis_angle(f32 angle) +{ + Matrix3x3 m; + m.x.x = fcos(angle); + m.x.y = 0.0f; + m.x.z = -fsin(angle); + + m.y.x = 0.0f; + m.y.y = 1.0f; + m.y.z = 0.0f; + + m.z.x = fsin(angle); + m.z.y = 0.0f; + m.z.z = fcos(angle); + return m; +} + +Matrix3x3 from_z_axis_angle(f32 angle) +{ + Matrix3x3 m; + m.x.x = fcos(angle); + m.x.y = fsin(angle); + m.x.z = 0.0f; + + m.y.x = -fsin(angle); + m.y.y = fcos(angle); + m.y.z = 0.0f; + + m.z.x = 0.0f; + m.z.y = 0.0f; + m.z.z = 1.0f; + return m; +} + Matrix3x3 from_quaternion(const Quaternion &r) { const float xx = r.x * r.x; diff --git a/src/core/math/matrix3x3.inl b/src/core/math/matrix3x3.inl index 5d5cc7dba..e4db68148 100644 --- a/src/core/math/matrix3x3.inl +++ b/src/core/math/matrix3x3.inl @@ -19,6 +19,15 @@ Matrix3x3 from_elements(f32 xx, f32 xy, f32 xz, f32 yx, f32 yy, f32 yz, f32 zx, /// Returns a new matrix from axes @a x, @a y and @a z. Matrix3x3 from_axes(const Vector3 &x, const Vector3 &y, const Vector3 &z); +/// Returns a new matrix that rotates around +X axis by @a angle radians. +Matrix3x3 from_x_axis_angle(f32 angle); + +/// Returns a new matrix that rotates around +Y axis by @a angle radians. +Matrix3x3 from_y_axis_angle(f32 angle); + +/// Returns a new matrix that rotates around +Z axis by @a angle radians. +Matrix3x3 from_z_axis_angle(f32 angle); + /// Returns a new matrix from rotation @a r. Matrix3x3 from_quaternion(const Quaternion &r); diff --git a/src/core/unit_tests.cpp b/src/core/unit_tests.cpp index 80c202d2e..c2fa61338 100644 --- a/src/core/unit_tests.cpp +++ b/src/core/unit_tests.cpp @@ -630,6 +630,47 @@ static void test_matrix3x3() ENSURE(fequal(b.z.y, 1.1f, 0.00001f)); ENSURE(fequal(b.z.z, -3.8f, 0.00001f)); } + { + const f32 angle = frad(10.0f); + Matrix3x3 a; + Matrix3x3 b; + + a = from_quaternion(from_axis_angle(vector3(1.0f, 0.0f, 0.0f), angle)); + b = from_x_axis_angle(angle); + ENSURE(fequal(a.x.x, b.x.x, 0.00001f)); + ENSURE(fequal(a.x.y, b.x.y, 0.00001f)); + ENSURE(fequal(a.x.z, b.x.z, 0.00001f)); + ENSURE(fequal(a.y.x, b.y.x, 0.00001f)); + ENSURE(fequal(a.y.y, b.y.y, 0.00001f)); + ENSURE(fequal(a.y.z, b.y.z, 0.00001f)); + ENSURE(fequal(a.z.x, b.z.x, 0.00001f)); + ENSURE(fequal(a.z.y, b.z.y, 0.00001f)); + ENSURE(fequal(a.z.z, b.z.z, 0.00001f)); + + a = from_quaternion(from_axis_angle(vector3(0.0f, 1.0f, 0.0f), angle)); + b = from_y_axis_angle(angle); + ENSURE(fequal(a.x.x, b.x.x, 0.00001f)); + ENSURE(fequal(a.x.y, b.x.y, 0.00001f)); + ENSURE(fequal(a.x.z, b.x.z, 0.00001f)); + ENSURE(fequal(a.y.x, b.y.x, 0.00001f)); + ENSURE(fequal(a.y.y, b.y.y, 0.00001f)); + ENSURE(fequal(a.y.z, b.y.z, 0.00001f)); + ENSURE(fequal(a.z.x, b.z.x, 0.00001f)); + ENSURE(fequal(a.z.y, b.z.y, 0.00001f)); + ENSURE(fequal(a.z.z, b.z.z, 0.00001f)); + + a = from_quaternion(from_axis_angle(vector3(0.0f, 0.0f, 1.0f), angle)); + b = from_z_axis_angle(angle); + ENSURE(fequal(a.x.x, b.x.x, 0.00001f)); + ENSURE(fequal(a.x.y, b.x.y, 0.00001f)); + ENSURE(fequal(a.x.z, b.x.z, 0.00001f)); + ENSURE(fequal(a.y.x, b.y.x, 0.00001f)); + ENSURE(fequal(a.y.y, b.y.y, 0.00001f)); + ENSURE(fequal(a.y.z, b.y.z, 0.00001f)); + ENSURE(fequal(a.z.x, b.z.x, 0.00001f)); + ENSURE(fequal(a.z.y, b.z.y, 0.00001f)); + ENSURE(fequal(a.z.z, b.z.z, 0.00001f)); + } } static void test_matrix4x4()