Files
libcifpp/include/cif++/matrix.hpp
Maarten L. Hekkelman 6eb8857877 Fix matrix multiplication
2026-03-04 20:28:04 +01:00

751 lines
21 KiB
C++

/*-
* SPDX-License-Identifier: BSD-2-Clause
*
* Copyright (c) 2023 NKI/AVL, Netherlands Cancer Institute
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <array>
#include <cassert>
#include <cmath>
#include <ostream>
#include <type_traits>
#include <vector>
/**
* @file matrix.hpp
*
* Some basic matrix operations and classes to hold matrices.
*
* We're using expression templates for optimal performance.
*
*/
namespace cif
{
// --------------------------------------------------------------------
// We're using expression templates here
/**
* @brief Base for the matrix expression templates
* This all uses the Curiously recurring template pattern
*
* @tparam M The type of the derived class
*/
template <typename M>
class matrix_expression // NOLINT(bugprone-crtp-constructor-accessibility)
{
public:
[[nodiscard]] constexpr std::size_t dim_m() const { return static_cast<const M &>(*this).dim_m(); } ///< Return the size (dimension) in direction m
[[nodiscard]] constexpr std::size_t dim_n() const { return static_cast<const M &>(*this).dim_n(); } ///< Return the size (dimension) in direction n
[[nodiscard]] constexpr bool empty() const { return dim_m() == 0 or dim_n() == 0; } ///< Convenient way to test for empty matrices
/** Return a reference to element [ @a i, @a j ] */
[[nodiscard]] constexpr auto &operator()(std::size_t i, std::size_t j)
{
return static_cast<M &>(*this).operator()(i, j);
}
/** Return the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr auto operator()(std::size_t i, std::size_t j) const
{
return static_cast<const M &>(*this).operator()(i, j);
}
/** Swap the contents of rows @a r1 and @a r2 */
void swap_row(std::size_t r1, std::size_t r2)
{
for (std::size_t c = 0; c < dim_m(); ++c)
{
auto v = operator()(r1, c);
operator()(r1, c) = operator()(r2, c);
operator()(r2, c) = v;
}
}
/** Swap the contents of columns @a c1 and @a c2 */
void swap_col(std::size_t c1, std::size_t c2)
{
for (std::size_t r = 0; r < dim_n(); ++r)
{
auto &a = operator()(r, c1);
auto &b = operator()(r, c2);
std::swap(a, b);
}
}
/** write the matrix @a m to std::ostream @a os */
friend std::ostream &operator<<(std::ostream &os, const matrix_expression &m)
{
os << '[';
for (std::size_t i = 0; i < m.dim_m(); ++i)
{
os << '[';
for (std::size_t j = 0; j < m.dim_n(); ++j)
{
os << m(i, j);
if (j + 1 < m.dim_n())
os << ", ";
}
if (i + 1 < m.dim_m())
os << ", ";
os << ']';
}
os << ']';
return os;
}
/// compare two matrices
template <typename M2>
constexpr bool operator==(const matrix_expression<M2> &m) const
{
bool same = false;
if (dim_m() == m.dim_m() and dim_n() == m.dim_n())
{
same = true;
for (std::size_t i = 0; same and i < m.dim_m(); ++i)
{
for (std::size_t j = 0; same and j < m.dim_n(); ++j)
same = operator()(i, j) == m(i, j);
}
}
return same;
}
};
// --------------------------------------------------------------------
/**
* @brief Storage class implementation of matrix_expression.
*
* @tparam F The type of the stored values
*
* matrix is m x n, addressing i,j is 0 <= i < m and 0 <= j < n
* element m i,j is mapped to [i * n + j] and thus storage is row major
*/
template <typename F = float>
class matrix : public matrix_expression<matrix<F>>
{
public:
/** The value type */
using value_type = F;
/**
* @brief Copy construct a new matrix object using @a m
*
* @tparam M2 Type of @a m
* @param m The matrix expression to copy values from
*/
template <typename M2>
matrix(const matrix_expression<M2> &m)
: m_m(m.dim_m())
, m_n(m.dim_n())
, m_data(m_m * m_n)
{
for (std::size_t i = 0; i < m_m; ++i)
{
for (std::size_t j = 0; j < m_n; ++j)
operator()(i, j) = m(i, j);
}
}
/**
* @brief Construct a new matrix object with dimension @a m and @a n
* setting the values to @a v
*
* @param m Requested dimension M
* @param n Requested dimension N
* @param v Value to store in each element
*/
matrix(std::size_t m, std::size_t n, value_type v = 0)
: m_m(m)
, m_n(n)
, m_data(m_m * m_n)
{
std::fill(m_data.begin(), m_data.end(), v);
}
/** @cond */
matrix() = default;
matrix(matrix &&m) = default;
matrix(const matrix &m) = default;
matrix &operator=(matrix &&m) = default;
matrix &operator=(const matrix &m) = default;
/** @endcond */
[[nodiscard]] constexpr std::size_t dim_m() const { return m_m; } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return m_n; } ///< Return dimension n
/** Return the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type operator()(std::size_t i, std::size_t j) const
{
assert(i < m_m);
assert(j < m_n);
return m_data[i * m_n + j];
}
/** Return a reference to element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type &operator()(std::size_t i, std::size_t j)
{
assert(i < m_m);
assert(j < m_n);
return m_data[i * m_n + j];
}
private:
std::size_t m_m = 0, m_n = 0;
std::vector<value_type> m_data;
};
// --------------------------------------------------------------------
// special case, 3x3 matrix
/**
* @brief Storage class implementation of matrix_expression
* with compile time fixed size.
*
* @tparam F The type of the stored values
*
* matrix is m x n, addressing i,j is 0 <= i < m and 0 <= j < n
* element m i,j is mapped to [i * n + j] and thus storage is row major
*/
template <typename F, std::size_t M, std::size_t N>
class matrix_fixed : public matrix_expression<matrix_fixed<F, M, N>>
{
public:
/** The value type */
using value_type = F;
/** The storage size */
static constexpr std::size_t kSize = M * N;
/** Copy constructor */
template <typename M2>
matrix_fixed(const M2 &m)
{
assert(M == m.dim_m() and N == m.dim_n());
for (std::size_t i = 0; i < M; ++i)
{
for (std::size_t j = 0; j < N; ++j)
operator()(i, j) = m(i, j);
}
}
/** default constructor */
matrix_fixed(value_type v = 0)
{
m_data.fill(v);
}
/** Alternate constructor taking an array of values to store */
matrix_fixed(const F (&v)[kSize])
{
fill(v, std::make_index_sequence<kSize>{});
}
/** @cond */
matrix_fixed(matrix_fixed &&m) = default;
matrix_fixed(const matrix_fixed &m) = default;
matrix_fixed &operator=(matrix_fixed &&m) = default;
matrix_fixed &operator=(const matrix_fixed &m) = default;
/** @endcond */
/** Store the values in @a a in the matrix */
template <std::size_t... Ixs>
matrix_fixed &fill(const F (&a)[kSize], std::index_sequence<Ixs...>)
{
m_data = { a[Ixs]... };
return *this;
}
[[nodiscard]] constexpr std::size_t dim_m() const { return M; } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return N; } ///< Return dimension n
/** Return the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type operator()(std::size_t i, std::size_t j) const
{
assert(i < M);
assert(j < N);
return m_data[i * N + j];
}
/** Return a reference to element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type &operator()(std::size_t i, std::size_t j)
{
assert(i < M);
assert(j < N);
return m_data[i * N + j];
}
private:
std::array<value_type, M * N> m_data;
};
/** typedef of a fixed matrix of size 3x3 */
template <typename F>
using matrix3x3 = matrix_fixed<F, 3, 3>;
/** typedef of a fixed matrix of size 4x4 */
template <typename F>
using matrix4x4 = matrix_fixed<F, 4, 4>;
// --------------------------------------------------------------------
/**
* @brief Storage class implementation of symmetric matrix_expression
*
* @tparam F The type of the stored values
*
* matrix is m x n, addressing i,j is 0 <= i < m and 0 <= j < n
* element m i,j is mapped to [i * n + j] and thus storage is row major
*/
template <typename F = float>
class symmetric_matrix : public matrix_expression<symmetric_matrix<F>>
{
public:
/** The value type */
using value_type = F;
/** constructor for a matrix of size @a n x @a n elements with value @a v */
symmetric_matrix(std::size_t n, value_type v = 0)
: m_n(n)
, m_data((m_n * (m_n + 1)) / 2)
{
std::fill(m_data.begin(), m_data.end(), v);
}
/** @cond */
symmetric_matrix() = default;
symmetric_matrix(symmetric_matrix &&m) = default;
symmetric_matrix(const symmetric_matrix &m) = default;
symmetric_matrix &operator=(symmetric_matrix &&m) = default;
symmetric_matrix &operator=(const symmetric_matrix &m) = default;
/** @endcond */
[[nodiscard]] constexpr std::size_t dim_m() const { return m_n; } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return m_n; } ///< Return dimension n
/** Return the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type operator()(std::size_t i, std::size_t j) const
{
return i < j
? m_data[(j * (j + 1)) / 2 + i]
: m_data[(i * (i + 1)) / 2 + j];
}
/** Return a reference to element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type &operator()(std::size_t i, std::size_t j)
{
if (i > j)
std::swap(i, j);
assert(j < m_n);
return m_data[(j * (j + 1)) / 2 + i];
}
private:
std::size_t m_n;
std::vector<value_type> m_data;
};
// --------------------------------------------------------------------
/**
* @brief Storage class implementation of symmetric matrix_expression
* with compile time fixed size.
*
* @tparam F The type of the stored values
*
* matrix is m x n, addressing i,j is 0 <= i < m and 0 <= j < n
* element m i,j is mapped to [i * n + j] and thus storage is row major
*/
template <typename F, std::size_t M>
class symmetric_matrix_fixed : public matrix_expression<symmetric_matrix_fixed<F, M>>
{
public:
/** The value type */
using value_type = F;
/** constructor with all elements set to value @a v */
symmetric_matrix_fixed(value_type v = 0)
{
std::fill(m_data.begin(), m_data.end(), v);
}
/** @cond */
symmetric_matrix_fixed(symmetric_matrix_fixed &&m) = default;
symmetric_matrix_fixed(const symmetric_matrix_fixed &m) = default;
symmetric_matrix_fixed &operator=(symmetric_matrix_fixed &&m) = default;
symmetric_matrix_fixed &operator=(const symmetric_matrix_fixed &m) = default;
/** @endcond */
[[nodiscard]] constexpr std::size_t dim_m() const { return M; } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return M; } ///< Return dimension n
/** Return the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type operator()(std::size_t i, std::size_t j) const
{
return i < j
? m_data[(j * (j + 1)) / 2 + i]
: m_data[(i * (i + 1)) / 2 + j];
}
/** Return a reference to element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type &operator()(std::size_t i, std::size_t j)
{
if (i > j)
std::swap(i, j);
assert(j < M);
return m_data[(j * (j + 1)) / 2 + i];
}
private:
std::array<value_type, (M * (M + 1)) / 2> m_data;
};
/** typedef of a fixed symmetric matrix of size 3x3 */
template <typename F>
using symmetric_matrix3x3 = symmetric_matrix_fixed<F, 3>;
/** typedef of a fixed symmetric matrix of size 4x4 */
template <typename F>
using symmetric_matrix4x4 = symmetric_matrix_fixed<F, 4>;
// --------------------------------------------------------------------
/**
* @brief implementation of symmetric matrix_expression with a value
* of 1 for the diagonal values and 0 for all the others.
*
* @tparam F The type of the stored values
*
* matrix is m x n, addressing i,j is 0 <= i < m and 0 <= j < n
* element m i,j is mapped to [i * n + j] and thus storage is row major
*/
template <typename F = float>
class identity_matrix : public matrix_expression<identity_matrix<F>>
{
public:
/** the value type */
using value_type = F;
/** constructor taking a dimension @a n */
identity_matrix(std::size_t n)
: m_n(n)
{
}
[[nodiscard]] constexpr std::size_t dim_m() const { return m_n; } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return m_n; } ///< Return dimension n
/** Return the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr value_type operator()(std::size_t i, std::size_t j) const
{
return static_cast<value_type>(i == j ? 1 : 0);
}
private:
std::size_t m_n;
};
// --------------------------------------------------------------------
// matrix functions, implemented as expression templates
/**
* @brief Implementation of a substraction operation as a matrix expression
*
* @tparam M1 Type of matrix 1
* @tparam M2 Type of matrix 2
*/
template <typename M1, typename M2>
class matrix_subtraction : public matrix_expression<matrix_subtraction<M1, M2>>
{
public:
/** constructor */
matrix_subtraction(const M1 &m1, const M2 &m2)
: m_m1(m1)
, m_m2(m2)
{
assert(m_m1.dim_m() == m_m2.dim_m());
assert(m_m1.dim_n() == m_m2.dim_n());
}
[[nodiscard]] constexpr std::size_t dim_m() const { return m_m1.dim_m(); } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return m_m1.dim_n(); } ///< Return dimension n
/** Access to the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr auto operator()(std::size_t i, std::size_t j) const
{
return m_m1(i, j) - m_m2(i, j);
}
private:
const M1 &m_m1;
const M2 &m_m2;
};
/** operator to subtract two matrices and return a matrix expression */
template <typename M1, typename M2>
auto operator-(const matrix_expression<M1> &m1, const matrix_expression<M2> &m2)
{
return matrix_subtraction(m1, m2);
}
/**
* @brief Implementation of a multiplication operation as a matrix expression
*
* @tparam M1 Type of matrix 1
* @tparam M2 Type of matrix 2
*/
template <typename M1, typename M2>
class matrix_matrix_multiplication : public matrix_expression<matrix_matrix_multiplication<M1, M2>>
{
public:
/** constructor */
matrix_matrix_multiplication(const M1 &m1, const M2 &m2)
: m_m1(m1)
, m_m2(m2)
{
assert(m1.dim_n() == m2.dim_m());
}
[[nodiscard]] constexpr std::size_t dim_m() const { return m_m1.dim_m(); } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return m_m1.dim_n(); } ///< Return dimension n
/** Access to the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr auto operator()(std::size_t i, std::size_t j) const
{
using value_type = decltype(m_m1(0, 0));
value_type result = {};
for (std::size_t k = 0; k < m_m1.dim_n(); ++k)
result += m_m1(i, k) * m_m2(k, j);
return result;
}
private:
const M1 &m_m1;
const M2 &m_m2;
};
/**
* @brief Implementation of a multiplication operation of a matrix and a scalar value as a matrix expression
*
* @tparam M1 Type of matrix
* @tparam M2 Type of scalar value
*/
template <typename M, typename T>
class matrix_scalar_multiplication : public matrix_expression<matrix_scalar_multiplication<M, T>>
{
public:
/** value type */
using value_type = T;
/** constructor */
matrix_scalar_multiplication(const M &m, value_type v)
: m_m(m)
, m_v(v)
{
}
[[nodiscard]] constexpr std::size_t dim_m() const { return m_m.dim_m(); } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return m_m.dim_n(); } ///< Return dimension n
/** Access to the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr auto operator()(std::size_t i, std::size_t j) const
{
return m_m(i, j) * m_v;
}
private:
const M &m_m;
value_type m_v;
};
/** First implementation of operator*, enabled if the second parameter is a scalar */
template <typename M1, typename T>
auto operator*(const matrix_expression<M1> &m, T v)
requires(std::is_floating_point_v<T>)
{
return matrix_scalar_multiplication(m, v);
}
/** First implementation of operator*, enabled if the second parameter is not a scalar and thus must be a matrix, right? */
template <typename M1, typename M2>
auto operator*(const matrix_expression<M1> &m1, const matrix_expression<M2> &m2)
requires(not std::is_floating_point_v<M2>)
{
return matrix_matrix_multiplication(m1, m2);
}
// --------------------------------------------------------------------
/// A sub-view on a matrix
template <typename M2>
class sub_matrix : public matrix_expression<sub_matrix<M2>>
{
public:
/// Constructor
sub_matrix(const M2 &m, int i, int j)
: m_m(m)
, m_i(i)
, m_j(j)
{
}
[[nodiscard]] constexpr std::size_t dim_m() const { return m_m.dim_m() - 1; } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return m_m.dim_n() - 1; } ///< Return dimension n
/** Access to the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr auto operator()(std::size_t i, std::size_t j) const
{
return m_m(
i >= m_i ? i + 1 : i,
j >= m_j ? j + 1 : j);
}
private:
const M2 &m_m;
std::size_t m_i, m_j;
};
// --------------------------------------------------------------------
/** Generic routine to calculate the determinant of a matrix
*
* @note This is currently only implemented for fixed matrices of size 3x3
*/
template <typename M>
auto determinant(const M &m);
/** Implementation of the determinant function for fixed size matrices of size 3x3 */
template <typename F = float>
auto determinant(const matrix3x3<F> &m)
{
return (m(0, 0) * ((m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1))) +
m(0, 1) * ((m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2))) +
m(0, 2) * ((m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))));
}
/** Implementation of the determinant function for fixed size matrices of size 4x4 */
template <typename F = float>
F determinant(const matrix4x4<F> &m)
{
return m(0, 0) * determinant(matrix3x3<F>(sub_matrix<decltype(m)>(m, 0, 0))) -
m(0, 1) * determinant(matrix3x3<F>(sub_matrix<decltype(m)>(m, 0, 1))) +
m(0, 2) * determinant(matrix3x3<F>(sub_matrix<decltype(m)>(m, 0, 2))) -
m(0, 3) * determinant(matrix3x3<F>(sub_matrix<decltype(m)>(m, 0, 3)));
}
// --------------------------------------------------------------------
/** Generic routine to calculate the inverse of a matrix
*
* @note This is currently only implemented for fixed matrices of size 3x3
*/
template <typename M>
M inverse(const M &m);
/** Implementation of the inverse function for fixed size matrices of size 3x3 */
template <typename F = float>
matrix3x3<F> inverse(const matrix3x3<F> &m)
{
F det = determinant(m);
matrix3x3<F> result;
result(0, 0) = (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) / det;
result(1, 0) = (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) / det;
result(2, 0) = (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)) / det;
result(0, 1) = (m(2, 1) * m(0, 2) - m(2, 2) * m(0, 1)) / det;
result(1, 1) = (m(2, 2) * m(0, 0) - m(2, 0) * m(0, 2)) / det;
result(2, 1) = (m(2, 0) * m(0, 1) - m(2, 1) * m(0, 0)) / det;
result(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) / det;
result(1, 2) = (m(0, 2) * m(1, 0) - m(0, 0) * m(1, 2)) / det;
result(2, 2) = (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0)) / det;
return result;
}
// --------------------------------------------------------------------
/**
* @brief Implementation of a cofactor calculation as a matrix expression
*
* @tparam M Type of matrix
*/
template <typename M>
class matrix_cofactors : public matrix_expression<matrix_cofactors<M>>
{
public:
/** constructor */
matrix_cofactors(const M &m)
: m_m(m)
{
}
[[nodiscard]] constexpr std::size_t dim_m() const { return m_m.dim_m(); } ///< Return dimension m
[[nodiscard]] constexpr std::size_t dim_n() const { return m_m.dim_n(); } ///< Return dimension n
/** Access to the value of element [ @a i, @a j ] */
[[nodiscard]] constexpr auto operator()(std::size_t i, std::size_t j) const
{
const std::size_t ixs[4][3] = {
{ 1, 2, 3 },
{ 0, 2, 3 },
{ 0, 1, 3 },
{ 0, 1, 2 }
};
const std::size_t *ix = ixs[i];
const std::size_t *iy = ixs[j];
auto result =
m_m(ix[0], iy[0]) * m_m(ix[1], iy[1]) * m_m(ix[2], iy[2]) +
m_m(ix[0], iy[1]) * m_m(ix[1], iy[2]) * m_m(ix[2], iy[0]) +
m_m(ix[0], iy[2]) * m_m(ix[1], iy[0]) * m_m(ix[2], iy[1]) -
m_m(ix[0], iy[2]) * m_m(ix[1], iy[1]) * m_m(ix[2], iy[0]) -
m_m(ix[0], iy[1]) * m_m(ix[1], iy[0]) * m_m(ix[2], iy[2]) -
m_m(ix[0], iy[0]) * m_m(ix[1], iy[2]) * m_m(ix[2], iy[1]);
return (i + j) % 2 == 1 ? -result : result;
}
private:
const M &m_m;
};
} // namespace cif