Matrix.h

Go to the documentation of this file.

/*******************************************************************************
 * This file is part of the Zivid API
 *
 * Copyright 2015-2025 (C) Zivid AS
 * All rights reserved.
 *
 * Zivid Software License, v1.0
 *
 * 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.
 *
 * 3. Neither the name of Zivid AS nor the names of its contributors may be used
 * to endorse or promote products derived from this software without specific
 * prior written permission.
 *
 * 4. This software, with or without modification, must not be used with any
 * other 3D camera than from Zivid AS.
 *
 * 5. Any software provided in binary form under this license must not be
 * reverse engineered, decompiled, modified and/or disassembled.
 *
 * THIS SOFTWARE IS PROVIDED BY ZIVID AS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL ZIVID AS 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.
 *
 * Contact: Zivid Customer Success Team <customersuccess@zivid.com>
 * Info:    http://www.zivid.com
 ******************************************************************************/
 
#pragma once
 
#include <Zivid/Detail/UtilsExport.h>
 
#include <array>
#include <iterator>
#include <ostream>
#include <sstream>
#include <string>
 
namespace Zivid
{
    template<typename T, size_t rowCount, size_t colCount>
    class Matrix
    {
        static_assert(rowCount > 0 && colCount > 0, "rowCount and colCount must be > 0");
 
    private:
        using Storage = std::array<T, rowCount * colCount>;
 
        static constexpr bool invertSupported = ((rowCount == 4 && colCount == 4) || (rowCount == 3 && colCount == 3))
                                                && (std::is_same<T, float>::value || std::is_same<T, double>::value);
 
        static constexpr bool identitySupported = (rowCount == colCount) && (std::is_arithmetic<T>::value);
 
    public:
        using ValueType = T;
 
        using Iterator = typename Storage::iterator;
 
        using ConstIterator = typename Storage::const_iterator;
 
        static constexpr size_t rows{ rowCount };
 
        static constexpr size_t cols{ colCount };
 
        Matrix() = default;
 
        Matrix(const std::string &fileName)
        {
            load(fileName);
        }
 
        explicit Matrix(const std::array<T, colCount * rowCount> &arrArr)
            : m_mat{ arrArr }
        {}
 
        template<typename U>
        explicit Matrix(const Matrix<U, rowCount, colCount> &sourceMatrix)
            : Matrix(sourceMatrix.begin(), sourceMatrix.end())
        {}
 
        template<typename Iterator>
        Matrix(Iterator beginIt, Iterator endIt)
        {
            if(std::distance(beginIt, endIt) != rows * cols)
            {
                /* NOLINT */ throw std::out_of_range(
                    "Matrix constructor got input that does not match matrix size. Expected "
                    + std::to_string(rows * cols) + " items, got " + std::to_string(std::distance(beginIt, endIt))
                    + " items.");
            }
 
            std::copy(beginIt, endIt, m_mat.begin());
        }
 
        explicit Matrix(std::initializer_list<T> values)
            : Matrix{ values.begin(), values.end() }
        {}
 
        explicit Matrix(std::initializer_list<std::initializer_list<T>> values)
        {
            if(values.size() != rows)
            {
                throw std::out_of_range(
                    "Matrix constructor got input that does not match matrix size. Expected " + std::to_string(rows)
                    + " rows, got " + std::to_string(values.size()) + " rows");
            }
 
            auto iterator = begin();
            for(const auto &row : values)
            {
                if(row.size() != cols)
                {
                    throw std::out_of_range(
                        "Matrix constructor got input that does not match matrix size. Expected all rows to be of size "
                        + std::to_string(cols) + ", got row with " + std::to_string(row.size()) + " values");
                }
                std::copy(std::begin(row), std::end(row), iterator);
                std::advance(iterator, cols);
            }
        }
 
        template<typename Q = T, typename = typename std::enable_if_t<identitySupported, Q>>
        static Matrix identity()
        {
            auto matrix = Matrix();
            for(size_t i = 0; i < Matrix::rows; ++i)
            {
                matrix(i, i) = T{ 1 };
            }
            return matrix;
        }
 
        Iterator begin()
        {
            return m_mat.begin();
        }
 
        Iterator end()
        {
            return m_mat.end();
        }
 
        ConstIterator begin() const
        {
            return m_mat.begin();
        }
 
        ConstIterator end() const
        {
            return m_mat.end();
        }
 
        ConstIterator cbegin() const
        {
            return m_mat.cbegin();
        }
 
        ConstIterator cend() const
        {
            return m_mat.cend();
        }
 
        T &at(size_t row, size_t col)
        {
            throwIfOutOfBounds(row, col);
            return m_mat[(row * cols) + col];
        }
 
        const T &at(size_t row, size_t col) const
        {
            throwIfOutOfBounds(row, col);
            return m_mat[(row * cols) + col];
        }
 
        T &operator()(size_t row, size_t col)
        {
            return m_mat[(row * cols) + col];
        }
 
        const T &operator()(size_t row, size_t col) const
        {
            return m_mat[(row * cols) + col];
        }
 
        T *data()
        {
            return m_mat.data();
        }
 
        const T *data() const
        {
            return m_mat.data();
        }
 
        void save(const std::string &fileName) const;
 
        void load(const std::string &fileName);
 
        template<typename Q = T, typename = typename std::enable_if_t<invertSupported, Q>>
        ZIVID_UTILS_EXPORT Matrix inverse() const;
 
        std::string toString() const
        {
            std::stringstream ss;
            ss << "[ ";
            for(size_t row = 0; row < rowCount; row++)
            {
                ss << (row == 0 ? "[" : "\n  [");
                for(size_t col = 0; col < colCount; col++)
                {
                    const auto value{ m_mat[(row * colCount) + col] };
                    ss << (value >= 0 ? " " : "") << std::to_string(value);
                    ss << (col == colCount - 1 ? "" : ", ");
                }
                ss << (row == rowCount - 1 ? "]" : "], ");
            }
            ss << " ]";
            return ss.str();
        }
 
    private:
        static void throwIfOutOfBounds(size_t row, size_t col)
        {
            if(row >= rows)
            {
                throw std::out_of_range{ "Trying to access row with index " + std::to_string(row)
                                         + ", but allowed range is [0, " + std::to_string(rows - 1) + "]" };
            }
            if(col >= cols)
            {
                throw std::out_of_range{ "Trying to access column with index " + std::to_string(col)
                                         + ", but allowed range is [0, " + std::to_string(cols - 1) + "]" };
            }
        }
 
        Storage m_mat{};
    };
 
    template<typename T, size_t rowCount, size_t colCount>
    std::ostream &operator<<(std::ostream &stream, const Matrix<T, rowCount, colCount> &matrix)
    {
        return stream << matrix.toString();
    }
 
    using Matrix3x3 = Matrix<float, 3, 3>;
 
    using Matrix3x3d = Matrix<double, 3, 3>;
 
    using Matrix4x4 = Matrix<float, 4, 4>;
 
    using Matrix4x4d = Matrix<double, 4, 4>;
 
#ifndef NO_DOC
    namespace Detail
    {
        ZIVID_UTILS_EXPORT void
        saveFloatMatrix(const float *data, size_t rowCount, size_t colCount, const std::string &fileName);
 
        ZIVID_UTILS_EXPORT void
        loadFloatMatrix(float *data, size_t rowCount, size_t colCount, const std::string &fileName);
    } // namespace Detail
#endif
 
#ifndef NO_DOC
    extern template ZIVID_UTILS_EXPORT Matrix<float, 4, 4> Matrix<float, 4, 4>::inverse<float, float>() const;
 
    extern template ZIVID_UTILS_EXPORT Matrix<double, 4, 4> Matrix<double, 4, 4>::inverse<double, double>() const;
 
    extern template ZIVID_UTILS_EXPORT Matrix<float, 3, 3> Matrix<float, 3, 3>::inverse<float, float>() const;
 
    extern template ZIVID_UTILS_EXPORT Matrix<double, 3, 3> Matrix<double, 3, 3>::inverse<double, double>() const;
 
    template<typename T, size_t rowCount, size_t colCount>
    void Matrix<T, rowCount, colCount>::save(const std::string &fileName) const
    {
        static_assert(std::is_same<T, float>::value, "Saving matrices of this type is not supported");
 
        Detail::saveFloatMatrix(data(), rowCount, colCount, fileName);
    }
 
    template<typename T, size_t rowCount, size_t colCount>
    void Matrix<T, rowCount, colCount>::load(const std::string &fileName)
    {
        static_assert(std::is_same<T, float>::value, "Loading matrices of this type is not supported");
 
        Detail::loadFloatMatrix(data(), rowCount, colCount, fileName);
    }
 
#endif
 
} // namespace Zivid