xsvector.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 XSVECTOR_H
#define XSVECTOR_H
 
#include "xsmath.h"
#include <stddef.h>
#include <string.h>     // memcpy
 
struct XsVector;
struct XsQuaternion;
 
#ifdef __cplusplus
#include <vector>
#include <algorithm>
#if __cplusplus >= 201103L && !defined(XSENS_HAVE_TYPE_TRAITS)
#include <type_traits>
#define XSENS_HAVE_TYPE_TRAITS
#endif
 
extern "C" {
#endif
#ifndef __cplusplus
#define XSVECTOR_INITIALIZER    { NULL, 0, 0 }
typedef struct XsVector XsVector;
#endif
 
XSTYPES_DLL_API void XsVector_ref(XsVector* thisPtr, XsSize sz, XsReal* buffer, XsDataFlags flags);
XSTYPES_DLL_API void XsVector_construct(XsVector* thisPtr, XsSize sz, const XsReal* src);
XSTYPES_DLL_API void XsVector_assign(XsVector* thisPtr, XsSize sz, const XsReal* src);
XSTYPES_DLL_API void XsVector_destruct(XsVector* thisPtr);
XSTYPES_DLL_API void XsVector_copy(XsVector* copy, XsVector const* src);
XSTYPES_DLL_API XsReal XsVector_dotProduct(const XsVector* a, const XsVector* b);
XSTYPES_DLL_API XsReal XsVector_cartesianLength(const XsVector* thisPtr);
XSTYPES_DLL_API void XsVector_normalize(XsVector* thisPtr);
XSTYPES_DLL_API void XsVector_setZero(XsVector* thisPtr);
XSTYPES_DLL_API int XsVector_empty(const XsVector* thisPtr);
XSTYPES_DLL_API void XsVector_multiplyScalar(const XsVector* thisPtr, XsReal scalar, XsVector* dest);
XSTYPES_DLL_API void XsVector_angularVelocityFromQuaternion(XsVector* thisPtr, XsReal deltaT, const struct XsQuaternion* quat);
XSTYPES_DLL_API void XsVector_swap(XsVector* a, XsVector* b);
XSTYPES_DLL_API void XsVector_fill(XsVector* thisPtr, XsReal value);
XSTYPES_DLL_API int XsVector_equal(const XsVector* thisPtr, const XsVector* thatPtr);
XSTYPES_DLL_API int XsVector_compare(const XsVector* thisPtr, const XsVector* thatPtr, XsReal epsilon);
 
#ifdef __cplusplus
} // extern "C"
#endif
#ifndef XSENS_NO_PACK
        #pragma pack(push,1)
#endif
struct XsVector
{
        XSCPPPROTECTED
        XsReal* const m_data;           
        const XsSize m_size;            
        const XsSize m_flags;                   
 
#ifdef __cplusplus
 
#ifdef __ICCARM__
#pragma diag_suppress=Pa039
#endif
 
        inline XsSize flags() const
        {
                return m_flags;
        }
public:
        inline explicit XsVector(XsSize sz = 0, const XsReal* src = 0)
                : m_data(0)
                , m_size(0)
                , m_flags(0)
        {
                if (sz)
                        XsVector_construct(this, sz, src);
        }
 
        inline XsVector(const XsVector& other)
                : m_data(0)
                , m_size(0)
                , m_flags(0)
        {
                XsVector_copy(this, &other);
        }
 
        inline explicit XsVector(XsReal* ref, XsSize sz, XsDataFlags flags_ = XSDF_None)
                : m_data(ref)
                , m_size(sz)
                , m_flags((XsSize) flags_)
        {
        }
 
        inline explicit XsVector(const XsVector& other, XsReal* ref, XsSize sz, XsDataFlags flags_ = XSDF_None)
                : m_data(ref)
                , m_size(sz)
                , m_flags((XsSize) flags_)
        {
                XsVector_copy(this, &other);
        }
 
        inline explicit XsVector(const XsQuaternion& quat, XsReal deltaT)
                : m_data(0)
                , m_size(0)
                , m_flags(0)
        {
                XsVector_angularVelocityFromQuaternion(this, deltaT, &quat);
        }
 
#if !defined(SWIG) && !defined(__ADSP21000__) && !defined(__AVR32__)
        inline XsVector(XsVector&& other)
                : m_data(0)
                , m_size(0)
                , m_flags(0)
        {
                if (!(other.m_flags & XSDF_Managed))
                        XsVector_copy(this, &other);
                else
                        XsVector_swap(this, &other);
        }
#endif
 
        inline XsVector& operator=(const XsVector& other)
        {
                XsVector_copy(this, &other);
                return *this;
        }
 
        inline ~XsVector()
        {
                XsVector_destruct(this);
        }
 
        inline void assign(XsSize sz, const XsReal* src)
        {
                XsVector_assign(this, sz, src);
        }
 
        inline void setSize(XsSize sz)
        {
                XsVector_assign(this, sz, 0);
        }
 
        inline XsSize size() const
        {
                return m_size;
        }
 
        inline const XsReal* data() const
        {
                return m_data;
        }
 
        inline XsVector operator * (XsReal scalar) const
        {
                XsVector v(m_size);
                for (XsSize i = 0; i < m_size; ++i)
                        v.m_data[i] = m_data[i] * scalar;
                return v;
        }
 
        inline void operator *=(XsReal scalar)
        {
                for (XsSize i = 0; i < m_size; ++i)
                        m_data[i] *= scalar;
        }
 
        inline XsReal& at(XsSize index)
        {
                assert(index < m_size);
                return m_data[index];
        }
 
        inline const XsReal& at(XsSize index) const
        {
                assert(index < m_size);
                return m_data[index];
        }
 
        inline XsReal value(XsSize index) const
        {
                assert(index < m_size);
                return m_data[index];
        }
 
        inline void setValue(XsSize index, XsReal val)
        {
                assert(index < m_size);
                m_data[index] = val;
        }
 
#ifndef SWIG
        template<typename J>
        inline XsReal operator[](J index) const
        {
#ifdef XSENS_HAVE_TYPE_TRAITS
                static_assert(std::is_integral<J>::value || std::is_enum<J>::value, "Integral index required.");
#endif
                assert(static_cast<XsSize>(index) < m_size);
                return m_data[index];
        }
 
        template<typename J>
        inline XsReal& operator[](J index)
        {
#ifdef XSENS_HAVE_TYPE_TRAITS
                static_assert(std::is_integral<J>::value || std::is_enum<J>::value, "Integral index required.");
#endif
                assert(static_cast<XsSize>(index) < m_size);
                return m_data[index];
        }
#else
        inline XsReal operator[](XsSize index) const
        {
                assert(index < m_size);
                return m_data[index];
        }
 
        inline XsReal& operator[](XsSize index)
        {
                assert(index < m_size);
                return m_data[index];
        }
#endif
 
        inline XsReal dotProduct(const XsVector& v) const
        {
                return XsVector_dotProduct(this, &v);
        }
 
        inline XsReal cartesianLength() const
        {
                return XsVector_cartesianLength(this);
        }
 
        inline void normalize()
        {
                XsVector_normalize(this);
        }
 
        inline void setZero()
        {
                return XsVector_setZero(this);
        }
 
        inline bool empty() const
        {
                return 0 != XsVector_empty(this);
        }
 
        inline XsVector& angularVelocityFromQuaternion(const XsQuaternion& quat, XsReal deltaT)
        {
                XsVector_angularVelocityFromQuaternion(this, deltaT, &quat);
                return *this;
        }
 
        XsVector operator-(const XsVector& sub) const
        {
                assert(m_size == sub.m_size);
                XsVector tmp(m_size);
                for (XsSize i = 0; i < m_size; ++i)
                        tmp[i] = m_data[i] - sub.m_data[i];
                return tmp;
        }
 
        XsVector operator+(const XsVector& sub) const
        {
                assert(m_size == sub.m_size);
                XsVector tmp(m_size);
                for (XsSize i = 0; i < m_size; ++i)
                        tmp[i] = m_data[i] + sub.m_data[i];
                return tmp;
        }
 
        XsVector operator-=(const XsVector& sub)
        {
                assert(m_size == sub.m_size);
                for (XsSize i = 0; i < m_size; ++i)
                        m_data[i] -= sub.m_data[i];
                return *this;
        }
 
        XsVector operator+=(const XsVector& sub)
        {
                assert(m_size == sub.m_size);
                for (XsSize i = 0; i < m_size; ++i)
                        m_data[i] += sub.m_data[i];
                return *this;
        }
 
        bool operator==(const XsVector& other) const
        {
                return 0 != XsVector_equal(this, &other);
        }
 
        bool isEqual(const XsVector& other, XsReal epsilon) const
        {
                return 0 != XsVector_compare(this, &other, epsilon);
        }
 
#ifndef XSENS_NO_STL
        inline std::vector<XsReal> toVector() const
        {
                std::vector<XsReal> tmp(m_size);
                if (m_size)
                        memcpy(&tmp[0], m_data, m_size * sizeof(XsReal));
                return tmp;
        }
#endif
 
        inline void zero()
        {
                for (XsSize i = 0; i < m_size; ++i)
                        m_data[i] = XsMath_zero;
        }
 
        inline void fill(XsReal val)
        {
                for (XsSize i = 0; i < m_size; ++i)
                        m_data[i] = val;
        }
 
        inline void swap(XsVector& b)
        {
                XsVector_swap(this, &b);
        }
 
        friend void swap(XsVector& first, XsVector& second)
        {
                first.swap(second);
        }
 
        inline void append(XsVector const& other)
        {
                XsVector tmp(size() + other.size());
                for (XsSize i = 0; i < size(); ++i)
                        tmp[i] = (*this)[i];
                for (XsSize i = 0; i < other.size(); ++i)
                        tmp[i + size()] = other[i];
                swap(tmp);
        }
 
        inline void clear()
        {
                setSize(0);
        }
 
#ifndef XSENS_NO_STL
 
        inline void reverse()
        {
                using std::swap;
 
                XsSize sz = size();
                XsSize half = sz >> 1;
                --sz;
                for (XsSize i = 0; i < half; ++i)
                        swap(operator[](i), operator[](sz - i));
        }
#endif
 
#ifdef __ICCARM__
#pragma diag_default=Pa039
#endif
 
#endif
};
#ifndef XSENS_NO_PACK
        #pragma pack(pop)
#endif
 
#ifdef __cplusplus
inline XsVector operator *(XsReal scalar, const XsVector& v)
{
        return v * scalar;
}
#endif
 
#endif