mirror of
https://github.com/PDB-REDO/libcifpp.git
synced 2026-06-04 13:54:25 +08:00
751 lines
21 KiB
C++
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
|