Add inverse function for matrix3x2 (#527)

boostorg · Jan 20, 2021 · 68cdbdd · 68cdbdd
1 parent 0c0fe1a
commit 68cdbdd
Show file tree

Hide file tree

Showing 2 changed files with 52 additions and 4 deletions.
diff --git a/include/boost/gil/extension/numeric/affine.hpp b/include/boost/gil/extension/numeric/affine.hpp
@@ -89,6 +89,25 @@ point<F> transform(matrix3x2<F> const& mat, point<F2> const& src)
     return src * mat;
 }
 
+/// Returns the inverse of the given affine transformation matrix
+///
+/// \warning Floating point arithmetic, use Boost.Rational if precision maters
+template <typename T>
+boost::gil::matrix3x2<T> inverse(boost::gil::matrix3x2<T> m)
+{
+    T const determinant = m.a * m.d - m.b * m.c;
+
+    boost::gil::matrix3x2<T> res;
+    res.a = m.d / determinant;
+    res.b = -m.b / determinant;
+    res.c = -m.c / determinant;
+    res.d = m.a / determinant;
+    res.e = (m.c * m.f - m.d * m.e) / determinant;
+    res.f = (m.b * m.e - m.a * m.f) / determinant;
+
+    return res;
+}
+
 }} // namespace boost::gil
 
 #endif
diff --git a/test/extension/numeric/matrix3x2.cpp b/test/extension/numeric/matrix3x2.cpp
@@ -16,8 +16,19 @@
 
 namespace gil = boost::gil;
 
-// FIXME: Remove when https://github.com/boostorg/core/issues/38 happens
-#define BOOST_GIL_TEST_IS_CLOSE(a, b, epsilon) BOOST_TEST_LT(std::fabs((a) - (b)), (epsilon))
+// Tolerance predicate for floating point comparison to use with BOOST_TEST_WITH
+template <typename T>
+struct with_tolerance
+{
+    with_tolerance(T tolerance) : tolerance(tolerance) {}
+    bool operator()(T lhs, T rhs)
+    {
+        return (std::abs(lhs - rhs) <= tolerance);
+    }
+
+private:
+    T tolerance;
+};
 
 namespace {
 constexpr double HALF_PI = 1.57079632679489661923;
@@ -123,10 +134,10 @@ void test_matrix3x2_vector_multiplication()
 void test_matrix3x2_get_rotate()
 {
     auto m1 = gil::matrix3x2<double>::get_rotate(HALF_PI);
-    BOOST_GIL_TEST_IS_CLOSE(m1.a, std::cos(HALF_PI), 0.03);
+    BOOST_TEST_WITH(m1.a, std::cos(HALF_PI), with_tolerance<double>(0.03));
     BOOST_TEST_EQ(m1.b, 1);
     BOOST_TEST_EQ(m1.c, -1);
-    BOOST_GIL_TEST_IS_CLOSE(m1.d, std::cos(HALF_PI), 0.03);
+    BOOST_TEST_WITH(m1.d, std::cos(HALF_PI), with_tolerance<double>(0.03));
     BOOST_TEST_EQ(m1.e, 0);
     BOOST_TEST_EQ(m1.f, 0);
 }
@@ -173,6 +184,23 @@ void test_matrix3x2_transform()
     BOOST_TEST_EQ(v2.y, 4);
 }
 
+void test_matrix3x2_inverse()
+{
+    using matrix_t = gil::matrix3x2<double>;
+    using point_t = gil::point<double>;
+
+    matrix_t mo = matrix_t::get_translate(0, 16);
+    matrix_t mb = matrix_t::get_rotate(HALF_PI);
+    auto m = mo * mb;
+
+    point_t p(10, 10);
+    point_t q = gil::transform(inverse(m), p);
+    point_t p2 = gil::transform(m, q);
+
+    BOOST_TEST_WITH(p.x, p2.x, with_tolerance<double>(1e-9));
+    BOOST_TEST_WITH(p.y, p2.y, with_tolerance<double>(1e-9));
+}
+
 int main()
 {
     test_matrix3x2_default_constructor();
@@ -186,6 +214,7 @@ int main()
     test_matrix3x2_get_scale();
     test_matrix3x2_get_translate();
     test_matrix3x2_transform();
+    test_matrix3x2_inverse();
 
     return ::boost::report_errors();
 }