matrix eigen value work

include/cif++/matrix.hpp

@@ -57,6 +57,16 @@ class matrix_expression
 		return static_cast<const M &>(*this).operator()(i, j);
 	}
 
+	void swap_row(uint32_t r1, uint32_t r2)
+	{
+		for (uint32_t c = 0; c < dim_m(); ++c)
+		{
+			auto v = operator()(r1, c);
+			operator()(r1, c) = operator()(r2, c);
+			operator()(r2, c) = v;
+		}
+	}
+
 	friend std::ostream &operator<<(std::ostream &os, const matrix_expression &m)
 	{
 		os << '[';
@@ -454,7 +464,7 @@ matrix3x3<F> inverse(const matrix3x3<F> &m)
 }
 
 template <typename M>
-auto eigen(const matrix_expression<M> &mat)
+auto eigen(const matrix_expression<M> &mat, bool sort)
 {
 	using value_type = decltype(mat(0, 0));
 
@@ -565,6 +575,20 @@ auto eigen(const matrix_expression<M> &mat)
 		}
 	}
 
+	if (sort)
+	{
+		for (size_t p = 0; p < N; ++p)
+		{
+			size_t j = p; // set j to index of largest remaining eval
+			for (size_t q = p + 1; q < N; ++q)
+				if (ev[q] < ev[j])
+					j = q;
+
+			std::swap(ev[j], ev[p]);
+			em.swap_row(j, p);
+		}
+	}
+
 	return std::make_tuple(ev, em);
 }
 

test/unit-3d-test.cpp

@@ -381,7 +381,7 @@ BOOST_AUTO_TEST_CASE(eigen_1, *utf::tolerance(0.1f))
 	cif::matrix4x4<float> m2;
 	m2 = m;
 
-	const auto &[ev, em] = cif::eigen(m2);
+	const auto &[ev, em] = cif::eigen(m2, true);
 
 	BOOST_TEST(ev[0] == 0.1666428611718905f);
 	BOOST_TEST(ev[1] == 1.4780548447781369f);