core: add from_{x,y,z}_axis_angle()

crownengine · Dec 10, 2024 · fcf71f7 · fcf71f7
1 parent 045e81a
commit fcf71f7
Show file tree

Hide file tree

Showing 3 changed files with 101 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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()