xsens_generic_matrix.h

Go to the documentation of this file.

 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  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 names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  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 HOLDERS 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.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  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 names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  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 HOLDERS 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.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
#ifndef XSENS_GENERIC_MATRIX_H
#define XSENS_GENERIC_MATRIX_H
 
#include "xsens_math_throw.h"
 
#ifndef XSENS_MATH_FIRMWARE
        #ifndef XSENS_EXCEPTION_H
                #include <xstypes/xsexception.h>
        #endif
#endif
 
#ifdef XSENS_GENERIC_MATRIX_RANGE_CHECKS
        #ifdef _MSC_VER
                #define XSENS_GENERIC_MATRIX_THROW throw(...)
        #else
                #define XSENS_GENERIC_MATRIX_THROW
        #endif
#else
        #define XSENS_GENERIC_MATRIX_THROW
#endif
 
#ifndef XSENS_THROW_BAD_ALLOC
        #ifdef XSENS_NO_EXCEPTIONS
                #include <assert.h>
                #define XSENS_THROW_BAD_ALLOC XM_THROW("Bad alloc")
        #else
                #define XSENS_THROW_BAD_ALLOC throw std::bad_alloc()
        #endif
#endif
 
#ifndef _PSTDINT_H_INCLUDED
        #include <xstypes/pstdint.h>
#endif
 
#include <stdlib.h>
#include <string.h>
 
#ifdef __cplusplus
namespace xsens
{
namespace
{
template <typename T>
inline void swapb(T& a, T& b)
{
        T temp = a;
        a = b;
        b = temp;
}
}
 
template <typename T>
class GenericMatrix
{
protected:
        T* m_data;                                      
        uint32_t m_rows;                        
        uint32_t m_cols;                        
        uint32_t m_allocSize;           
        int m_flags;                            
 
public:
        GenericMatrix();
        explicit GenericMatrix(const uint32_t newRows, const uint32_t newCols, bool zeroValues = true);
        GenericMatrix(const GenericMatrix<T>& src);
        explicit GenericMatrix(const uint32_t newRows, const uint32_t newCols, const T* src);
        explicit GenericMatrix(const uint32_t newRows, const uint32_t newCols, T* buffer, XsDataFlags flags);
        virtual ~GenericMatrix();
 
        void setSize(uint32_t newRows, uint32_t newCols, bool zeroValues = true);
        T& get(const uint32_t row, const uint32_t cols) const XSENS_GENERIC_MATRIX_THROW;
        T* operator [](const uint32_t row) const XSENS_GENERIC_MATRIX_THROW;
        uint32_t rows(void) const
        {
                return m_rows;
        }
        uint32_t cols(void) const
        {
                return m_cols;
        }
        uint32_t count(void) const
        {
                return m_rows * m_cols;
        }
        uint32_t size(void) const
        {
                return m_rows * m_cols;
        }
        GenericMatrix<T>& operator = (const GenericMatrix<T>& src);
        void swap(GenericMatrix<T>& other);
        friend void swap(GenericMatrix<T>& first, GenericMatrix<T>& second)
        {
                first.swap(second);
        }
        void zeroValues();
};
 
template <typename T> GenericMatrix<T>::GenericMatrix()
        : m_rows(1)
        , m_cols(1)
        , m_flags(XSDF_Managed)
{
        m_data = (T*)malloc(m_rows * m_cols * sizeof(T));
 
        if (!m_data)
                XSENS_THROW_BAD_ALLOC;
 
        m_allocSize = m_rows * m_cols;
}
 
template <typename T> GenericMatrix<T>::GenericMatrix(uint32_t newRows, uint32_t newCols, bool zeroVals)
        : m_rows(newRows)
        , m_cols(newCols)
        , m_flags(XSDF_Managed)
{
        m_data = (T*)malloc(sizeof(T) * (size_t)m_rows * m_cols);
 
        if (!m_data)
                XSENS_THROW_BAD_ALLOC;
 
        m_allocSize = m_rows * m_cols;
 
        if (zeroVals)
                zeroValues();
}
 
template <typename T> GenericMatrix<T>::GenericMatrix(const GenericMatrix<T>& src)
        : m_rows(src.m_rows)
        , m_cols(src.m_cols)
        , m_flags(XSDF_Managed)
{
        m_data = (T*)malloc(sizeof(T) * (size_t)m_rows * m_cols);
 
        if (!m_data)
                XSENS_THROW_BAD_ALLOC;
 
        m_allocSize = m_rows * m_cols;
 
        memcpy(m_data, src.m_data, sizeof(T) * (size_t)m_rows * m_cols);
}
 
template <typename T> GenericMatrix<T>::GenericMatrix(const uint32_t newRows, uint32_t newCols, const T* src)
        : m_rows(newRows)
        , m_cols(newCols)
        , m_flags(XSDF_Managed)
{
        m_data = (T*)malloc(sizeof(T) * (size_t)m_rows * m_cols);
 
        if (!m_data)
                XSENS_THROW_BAD_ALLOC;
 
        m_allocSize = m_rows * m_cols;
 
        memcpy(m_data, src, sizeof(T) * (size_t)m_rows * m_cols);
}
 
template <typename T> GenericMatrix<T>::GenericMatrix(const uint32_t newRows, const uint32_t newCols, T* const buffer, XsDataFlags flags)
        : m_rows(newRows)
        , m_cols(newCols)
        , m_flags(flags)
{
        if (m_flags & XSDF_FixedSize)
        {
                m_data = buffer;
 
                if (size() == 0)
                        m_flags |= XSDF_Empty;
        }
 
        if (m_flags & XSDF_Managed)
                m_data = (T*)malloc(sizeof(T) * (size_t)m_rows * m_cols);
 
        if (!m_data)
                XSENS_THROW_BAD_ALLOC;
 
        m_allocSize = m_rows * m_cols;
 
        if (m_flags & XSDF_Managed)
                memcpy(m_data, buffer, sizeof(T) * (size_t)m_rows * m_cols);
}
 
template <typename T> GenericMatrix<T>::~GenericMatrix()
{
        if (m_flags & XSDF_Managed)
                free(m_data);
 
        if (!(m_flags & XSDF_FixedSize))
                m_data = NULL;
        else
                m_flags |= XSDF_Empty;
}
 
template <typename T>
void GenericMatrix<T>::setSize(uint32_t newRows, uint32_t newCols, bool zeroVals)
{
        if (newRows == m_rows && newCols == m_cols)
        {
                if (zeroVals)
                        zeroValues();
 
                return;
        }
 
#ifdef XSENS_MATH_RESTRICT_RESIZE
        if (m_data && !(m_flags & XSDF_Managed))
                XM_THROW("resizing of this object is not allowed");
#endif
 
        const uint32_t newSize = newRows * newCols;
 
        if (m_flags & XSDF_FixedSize)
        {
                if (newSize == 0)
                {
                        m_flags |= XSDF_Empty;
                        m_rows = newRows;
                        m_cols = newCols;
                        return;
                }
 
                assert(newSize <= size());
                m_flags &= ~XSDF_Empty;
        }
 
        if (newSize > m_allocSize || newSize == 0)
        {
                if (m_flags & XSDF_Managed)
                {
                        free(m_data);
                        m_data = 0;
                        m_allocSize = 0;
                }
 
                if (newSize > 0)
                {
                        m_data = (T*)malloc(sizeof(T) * (size_t)newRows * newCols);
                        m_flags = XSDF_Managed;
                        m_allocSize = newRows * newCols;
                }
        }
 
        m_rows = newRows;
        m_cols = newCols;
 
        if (!m_data)
                XSENS_THROW_BAD_ALLOC;
 
        if (zeroVals)
                zeroValues();
}
 
template <typename T>
T& GenericMatrix<T>::get(uint32_t row, uint32_t col) const XSENS_GENERIC_MATRIX_THROW
{
#ifdef XSENS_GENERIC_MATRIX_RANGE_CHECKS
        if (row >= m_rows || col >= m_cols)
                XM_THROW("index out of bounds");
#endif
        return m_data[row * m_cols + col];
}
 
template <typename T>
T* GenericMatrix<T>::operator [](const uint32_t row) const XSENS_GENERIC_MATRIX_THROW
{
#ifdef XSENS_GENERIC_MATRIX_RANGE_CHECKS
        if (row >= m_rows)
                XM_THROW("index out of bounds");
#endif
        return &m_data[row * m_cols];
}
 
template <typename T>
void GenericMatrix<T>::zeroValues()
{
        memset(static_cast<void*>(m_data), 0, sizeof(T) * (size_t)m_rows * m_cols);
}
 
template <typename T>
void GenericMatrix<T>::swap(GenericMatrix<T>& other)
{
        swapb(m_rows, other.m_rows);
        swapb(m_cols, other.m_cols);
        swapb(m_allocSize, other.m_allocSize);
        swapb(m_flags, other.m_flags);
        swapb(m_data, other.m_data);
}
 
template <typename T>
GenericMatrix<T>& GenericMatrix<T>::operator = (const GenericMatrix<T>& src)
{
        if (this != &src)
        {
                GenericMatrix temp(src);
                swap(temp);
        }
        return *this;
}
 
}
 
#endif
#endif