VectorNf_gnu.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#if FE_VDIM<3 || FE_VDIM>4
#error Full SIMD Vector specialization only implemented in 3 or 4 dimensions
#endif

#ifndef _MM_SHUFFLE
#define _MM_SHUFFLE(fp3,fp2,fp1,fp0)                                        \
        (((fp3) << 6) | ((fp2) << 4) | ((fp1) << 2) | (fp0))
#endif

namespace fe
{

//* NOTE FE_VDIM is a #define, not a real template parameter

/**************************************************************************//**
    @brief Fully specialized 3 or 4 component F32 vector using GNU SIMD

    @ingroup math
*//***************************************************************************/
template<>
class Vector<FE_VDIM,F32>
{
    public:

    class Raw
    {
        public:
                Raw(const Vector<FE_VDIM,F32>& vector)
                {
                    m_array[0]=vector[0];
                    m_array[1]=vector[1];
                    m_array[2]=vector[2];
#if FE_VDIM>3
                    m_array[3]=vector[3];
#else
                    m_array[3]=0.0f;
#endif
                }

                operator const F32*(void) const
                {   return m_array; }

        private:
            F32 m_array[4];
    };

                        Vector(void)
                        {
                            v4sf_assertAlignment(m_simd);

#if FE_VEC_CHECK_VALID
                            set4(0.0f,0.0f,0.0f,0.0f);
#endif
                        }
        template<int N,typename T>
                        Vector(const Vector<N,T>& other)
                        {   operator=(other); }
                        Vector(const Vector<FE_VDIM,F32>& other)
                        {   operator=(other); }
                        Vector(const F32* array)
                        {   operator=(array); }
        template<typename T, typename U>
                        Vector(T x,U y)
                        {   set4(x,y,0.0f,0.0f); }
        template<typename T, typename U, typename V>
                        Vector(T x,U y,V z)
                        {   set4(x,y,z,0.0f); }
        template<typename T, typename U, typename V, typename W>
                        Vector(T x,U y,V z,W w)
                        {   set4(x,y,z,w); }

        Vector<FE_VDIM,F32>&    operator=(const F32* array);
        template<typename T>
        Vector<FE_VDIM,F32>&    operator=(const Vector<3,T>& other);
        template<typename T>
        Vector<FE_VDIM,F32>&    operator=(const Vector<2,T>& other);
        template<typename T>
        Vector<FE_VDIM,F32>&    operator=(const Vector<1,T>& other);
        template<int N,typename T>
        Vector<FE_VDIM,F32>&    operator=(const Vector<N,T>& other);
        Vector<FE_VDIM,F32>&    operator=(const Vector<FE_VDIM,F32>& other);

        BWORD                   operator==(const Vector<FE_VDIM,F32>& other);
        BWORD                   operator!=(const Vector<FE_VDIM,F32>& other);

        F32             operator[](U32 index) const
                        {
                            FEASSERT(index<4);
                            checkValid();
                            return m_data[index];
                        }

                        /** @brief Access internal component

                            Warning, this provides internal access and should
                            be limited to immediate use only. */
        F32&            operator[](U32 index)
                        {
                            FEASSERT(index<4);
                            return m_data[index];
                        }

        template<typename T, typename U, typename V, typename W>
        Vector<FE_VDIM,F32>& set4(T x,U y,V z,W w);

        template<typename T>
        void            get4(T array[4]);

const   Raw             temp(void) const    { return *this; }

                        /// @internal
const   v4sf&           simd(void) const    { return m_simd; }

                        /// @internal
        v4sf&           simd(void)          { return m_simd; }

                        /// @internal
        void            checkValid(void) const
                        {
#if FE_VEC_CHECK_VALID
                            if(FE_INVALID_SCALAR(m_data[0]))
                                feX(e_corrupt,"Vector<N,F32>::checkValid",
                                        "element 0 invalid");
                            if(FE_INVALID_SCALAR(m_data[1]))
                                feX(e_corrupt,"Vector<N,F32>::checkValid",
                                        "element 1 invalid");
                            if(FE_INVALID_SCALAR(m_data[2]))
                                feX(e_corrupt,"Vector<N,F32>::checkValid",
                                        "element 2 invalid");
#if FE_VDIM>3
                            if(FE_INVALID_SCALAR(m_data[3]))
                                feX(e_corrupt,"Vector<4,F32>::checkValid",
                                        "element 3 invalid");
#else
                            if(m_data[3]!=0.0f)
                                feX(e_corrupt,"Vector<3,F32>::checkValid",
                                        "element 3 nonzero");
#endif
#endif
                        }

    protected:
/*
const   F32*            data(void) const
                        {   return reinterpret_cast<const F32*>(&m_simd); }
        F32*            data(void)
                        {   return reinterpret_cast<F32*>(&m_simd); }
*/

    union
    {
        v4sf            m_simd;
        float           m_data[4];
    };
};

/** Return text describing the Vector's state
    @relates Vector<4,F32>
    */
inline String print(const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    String s;
#if FE_VDIM>3
    s.sPrintf("[%g %g %g %g]",rhs[0],rhs[1],rhs[2],rhs[3]);
#else
    s.sPrintf("[%g %g %g]",rhs[0],rhs[1],rhs[2]);
#endif
    return s;
}

template<typename T>
inline Vector<FE_VDIM,F32>& Vector<FE_VDIM,F32>::operator=(
        const Vector<3,T>& other)
{
    v4sf_assertAlignment(m_simd);
    return set4(other[0],other[1],other[2],0.0f);
}

template<typename T>
inline Vector<FE_VDIM,F32>& Vector<FE_VDIM,F32>::operator=(
        const Vector<2,T>& other)
{
    v4sf_assertAlignment(m_simd);
    return set4(other[0],other[1],0.0f,0.0f);
}

template<typename T>
inline Vector<FE_VDIM,F32>& Vector<FE_VDIM,F32>::operator=(
        const Vector<1,T>& other)
{
    v4sf_assertAlignment(m_simd);
    return set4(other[0],0.0f,0.0f,0.0f);
}

template<int N,typename T>
inline Vector<FE_VDIM,F32>& Vector<FE_VDIM,F32>::operator=(
        const Vector<N,T>& other)
{
    v4sf_assertAlignment(m_simd);
    return set4(other[0],other[1],other[2],other[3]);
}

inline Vector<FE_VDIM,F32>& Vector<FE_VDIM,F32>::operator=(
        const Vector<FE_VDIM,F32>& other)
{
    v4sf_assertAlignment(m_simd);
    v4sf_assertAlignment(other.m_simd);
    other.checkValid();

    m_simd=other.m_simd;
    checkValid();
    return *this;
}

inline Vector<FE_VDIM,F32>& Vector<FE_VDIM,F32>::operator=(const F32* array)
{
    v4sf_assertAlignment(m_simd);

//  memcpy(&m_simd,array,sizeof(m_simd));
    m_simd= __builtin_ia32_loadups(array);

#if FE_VDIM<4
    m_data[3]=0.0f;
#endif
    checkValid();
    return *this;
}

inline BWORD Vector<FE_VDIM,F32>::operator==(const Vector<FE_VDIM,F32>& other)
{
    if(this!=&other)
    {
        for(U32 i=0;i<FE_VDIM;i++)
        {
            if(m_data[i]!=other[i])
            {
                return false;
            }
        }
    }
    return true;
}

inline BWORD Vector<FE_VDIM,F32>::operator!=(const Vector<FE_VDIM,F32>& other)
{
    return !operator==(other);
}

template<typename T, typename U, typename V, typename W>
inline Vector<FE_VDIM,F32>& Vector<FE_VDIM,F32>::set4(T x,U y,V z,W w)
{
    v4sf_assertAlignment(m_simd);

    m_data[0]=x;
    m_data[1]=y;
    m_data[2]=z;
#if FE_VDIM>3
    m_data[3]=w;
#else
    m_data[3]=0.0f;
#endif
    checkValid();
    return *this;
}

template<typename T>
inline void Vector<FE_VDIM,F32>::get4(T array[4])
{
    v4sf_assertAlignment(m_simd);
    checkValid();

    array[0]=m_data[0];
    array[1]=m_data[1];
    array[2]=m_data[2];
    array[3]=m_data[3];
}

/** Set components
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> &set(Vector<FE_VDIM,F32> &r)
{
    v4sf_assertAlignment(r.simd());

    r.set4(0.0f,0.0f,0.0f,0.0f);
    r.checkValid();
    return r;
}

/** Set components
    @relates Vector<4,F32>
    */
template<typename T>
inline Vector<FE_VDIM,F32> &set(Vector<FE_VDIM,F32> &r,T x)
{
    v4sf_assertAlignment(r.simd());

    r.set4(x,0.0f,0.0f,0.0f);
    r.checkValid();
    return r;
}

/** Set components
    @relates Vector<4,F32>
    */
template<typename T, typename U>
inline Vector<FE_VDIM,F32> &set(Vector<FE_VDIM,F32> &r,T x,U y)
{
    v4sf_assertAlignment(r.simd());

    r.set4(x,y,0.0f,0.0f);
    r.checkValid();
    return r;
}

/** Set components
    @relates Vector<4,F32>
    */
template<typename T, typename U, typename V>
inline Vector<FE_VDIM,F32> &set(Vector<FE_VDIM,F32> &r,T x,U y,V z)
{
    v4sf_assertAlignment(r.simd());

    r.set4(x,y,z,0.0f);
    r.checkValid();
    return r;
}

/** Set components
    @relates Vector<4,F32>
    */
template<typename T, typename U, typename V, typename W>
inline Vector<FE_VDIM,F32> &set(Vector<FE_VDIM,F32> &r,T x,U y,V z,W w)
{
    v4sf_assertAlignment(r.simd());

    r.set4(x,y,z,w);
    r.checkValid();
    return r;
}

/** Set all components to the same value
    @relates Vector<4,F32>
    */
template<typename T>
inline Vector<FE_VDIM,F32> &setAll(Vector<FE_VDIM,F32> &lhs,T value)
{
    v4sf_assertAlignment(lhs.simd());

    lhs.set4(value,value,value,value);
    lhs.checkValid();
    return lhs;
}

/** Set indexed component
    @relates Vector<4,F32>
    */
template<typename T>
inline Vector<FE_VDIM,F32> &setAt(Vector<FE_VDIM,F32> &lhs,U32 index,T value)
{
    v4sf_assertAlignment(lhs.simd());
    lhs[index]=value;
    lhs.checkValid();
    return lhs;
}

/** Return the number of elements
    @relates Vector<4,F32>
    */
template<typename T>
inline U32 size(const Vector<FE_VDIM,F32> &lhs)
{
    return FE_VDIM;
}

/** Add to Vector in place
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> &operator+=(Vector<FE_VDIM,F32> &lhs,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    lhs.simd()=__builtin_ia32_addps(lhs.simd(),rhs.simd());
    lhs.checkValid();
    return lhs;
}

/** Subtract from Vector in place
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> &operator-=(Vector<FE_VDIM,F32> &lhs,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    lhs.simd()=__builtin_ia32_subps(lhs.simd(),rhs.simd());
    lhs.checkValid();
    return lhs;
}

/** Negate the Vector
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> operator-(const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    Vector<FE_VDIM,F32> v;
#if FALSE
    v4sf zero=__builtin_ia32_setzerops();
#else
    v4sf zero;
    v4sf_setSame(zero,0.0f);
#endif
    v.simd()=__builtin_ia32_subps(zero,rhs.simd());
    v.checkValid();
    return v;
}

/** Independently scale components in place
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> &operator*=(Vector<FE_VDIM,F32> &lhs,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    lhs.simd()=__builtin_ia32_mulps(lhs.simd(),rhs.simd());
    lhs.checkValid();
    return lhs;
}

/** Uniformly scale components in place
    @relates Vector<4,F32>
    */
template<typename T>
inline Vector<FE_VDIM,F32> &operator*=(Vector<FE_VDIM,F32> &lhs,T rhs)
{
    v4sf_assertAlignment(lhs.simd());
    lhs.checkValid();

    v4sf scale;
    v4sf_setSame(scale,rhs);
    lhs.simd()=__builtin_ia32_mulps(scale,lhs.simd());
    lhs.checkValid();
    return lhs;
}

/** Return dot product
    @relates Vector<4,F32>
    */
inline F32 dot(const Vector<FE_VDIM,F32> &lhs,const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

//  v4sf_log("lhs",lhs.simd());
//  v4sf_log("rhs",rhs.simd());
    v4sf combine=__builtin_ia32_mulps(lhs.simd(),rhs.simd());
#if FE_SSE<3
//  v4sf_log("combine",combine);
//  v4sf shift=__builtin_ia32_shufps(combine,combine,0x4E);
    v4sf shift=__builtin_ia32_shufps(combine,combine,_MM_SHUFFLE(1,0,3,2));
//  v4sf_log("shift",shift);
    combine=__builtin_ia32_addps(combine,shift);
//  v4sf_log("combine",combine);
//  shift=__builtin_ia32_shufps(combine,combine,0x39);
    shift=__builtin_ia32_shufps(combine,combine,_MM_SHUFFLE(0,3,2,1));
//  v4sf_log("shift",shift);
    combine=__builtin_ia32_addps(combine,shift);
//  v4sf_log("combine",combine);
#else
    combine=__builtin_ia32_haddps(combine,combine);
    combine=__builtin_ia32_haddps(combine,combine);
#endif
    return reinterpret_cast<F32*>(&combine)[0];
}

/** Return square of the Vector length
    @relates Vector<4,F32>
    */
inline F32 magnitudeSquared(const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    return dot(rhs,rhs);
}

/** Return the Vector length
    @relates Vector<4,F32>
    */
inline F32 magnitude(const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    return sqrtf(magnitudeSquared(rhs));
}

/** Return sum of Vectors
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> operator+(const Vector<FE_VDIM,F32> &lhs,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    Vector<FE_VDIM,F32> v;
    v.simd()=__builtin_ia32_addps(lhs.simd(),rhs.simd());
    v.checkValid();
    return v;
}

/** Return difference of Vectors
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> operator-(const Vector<FE_VDIM,F32> &lhs,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    Vector<FE_VDIM,F32> v;
    v.simd()=__builtin_ia32_subps(lhs.simd(),rhs.simd());
    v.checkValid();
    return v;
}

/** Return a Vector of products of each component
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> operator*(const Vector<FE_VDIM,F32> &lhs,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    Vector<FE_VDIM,F32> v;
    v.simd()=__builtin_ia32_mulps(lhs.simd(),rhs.simd());
    v.checkValid();
    return v;
}

/** Return a uniformly scaled Vector (pre)
    @relates Vector<4,F32>
    */
template <typename T>
inline
typename boost::enable_if<boost::is_arithmetic<T>,Vector<FE_VDIM,F32> >::type
operator*(const T lhs,const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    Vector<FE_VDIM,F32> v;
    v4sf scale;
    v4sf_setSame(scale,lhs);
    v.simd()=__builtin_ia32_mulps(scale,rhs.simd());
    v.checkValid();
    return v;
}

/** Return a uniformly scaled Vector (post)
    @relates Vector<4,F32>
    */
template<typename T>
inline
typename boost::enable_if<boost::is_arithmetic<T>,Vector<FE_VDIM,F32> >::type
operator*(const Vector<FE_VDIM,F32> &lhs,const T rhs)
{
    v4sf_assertAlignment(lhs.simd());
    lhs.checkValid();

    Vector<FE_VDIM,F32> v;
    v4sf scale;
    v4sf_setSame(scale,F32(rhs));
    v.simd()=__builtin_ia32_mulps(scale,lhs.simd());
    v.checkValid();
    return v;
}

/** Return the Vector direction scaled to unit length
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> unit(const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    F32 mag=magnitude(rhs);
    if(mag==0.0f)
    {
        feX(e_unsolvable,"unit(Vector<N,F32>)",
                "attempt to normalize zero magnitude vector");
    }
    return rhs*F32(1.0f/mag);
}

/** Return the Vector direction scaled to unit length with zero check
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> unitSafe(const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    F32 mag=magnitude(rhs);
    if(mag>0.0f)
    {
        return rhs*F32(1.0f/mag);
    }
    return rhs;
}

/** Scale Vector to unit length
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> &normalize(Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    F32 mag=magnitude(rhs);
    if(mag==0.0f)
    {
        feX(e_unsolvable,"normalize",
                "attempt to normalize zero magnitude vector");
    }
    return rhs*=F32(1.0f/mag);
}

/** Scale Vector to unit length
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> &normalizeSafe(Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(rhs.simd());
    rhs.checkValid();

    F32 mag=magnitude(rhs);
    if(mag>0.0f)
    {
        rhs*=F32(1.0f/mag);
    }
    return rhs;
}

/** Return a cross product of Vectors
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> &cross3(Vector<FE_VDIM,F32> &r,
        const Vector<FE_VDIM,F32> &lhs,const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(r.simd());
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

#if FALSE
    set(r,  lhs[1] * rhs[2] - lhs[2] * rhs[1],
            lhs[2] * rhs[0] - lhs[0] * rhs[2],
            lhs[0] * rhs[1] - lhs[1] * rhs[0]);
#else
    v4sf op1=__builtin_ia32_shufps(lhs.simd(),lhs.simd(),_MM_SHUFFLE(3,0,2,1));
    v4sf op2=__builtin_ia32_shufps(rhs.simd(),rhs.simd(),_MM_SHUFFLE(3,1,0,2));
    v4sf pr1=__builtin_ia32_mulps(op1,op2);

    op1=__builtin_ia32_shufps(lhs.simd(),lhs.simd(),_MM_SHUFFLE(3,1,0,2));
    op2=__builtin_ia32_shufps(rhs.simd(),rhs.simd(),_MM_SHUFFLE(3,0,2,1));
    v4sf pr2=__builtin_ia32_mulps(op1,op2);

    r.simd()=__builtin_ia32_subps(pr1,pr2);
    r[3]=0.0f;
#endif
    r.checkValid();
    return r;
}

/** Set the Vector as a cross product of Vectors
    @relates Vector<4,F32>
    */
inline Vector<FE_VDIM,F32> cross3(const Vector<FE_VDIM,F32> &lhs,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    Vector<FE_VDIM,F32> v;
    return cross3(v, lhs, rhs);
}

#if FE_VDIM==3
/** Return a cross product of Vectors
    @relates Vector<3,F32>
    */
inline Vector<FE_VDIM,F32> &cross(Vector<FE_VDIM,F32> &r,
        const Vector<FE_VDIM,F32> &lhs,const Vector<FE_VDIM,F32> &rhs)
{
    return cross3(r,lhs,rhs);
}

/** Set the Vector as a cross product of Vectors
    @relates Vector<3,F32>
    */
inline Vector<FE_VDIM,F32> cross(const Vector<FE_VDIM,F32> &lhs,
        const Vector<FE_VDIM,F32> &rhs)
{
    return cross3(lhs,rhs);
}
#endif

/** Add with scaling

    lhs = lhs + rhs * scalar

    @relates DenseVector
    */
template<typename U>
inline Vector<FE_VDIM,F32> &addScaled(Vector<FE_VDIM,F32> &lhs,U scalar,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    v4sf scale;
    v4sf_setSame(scale,scalar);
    v4sf scaled=__builtin_ia32_mulps(scale,rhs.simd());
    lhs.simd()=__builtin_ia32_addps(lhs.simd(),scaled);
    lhs.checkValid();
    return lhs;
}

/** Scale then add

    lhs = lhs * scalar + rhs

    @relates DenseVector
    */
template<typename U>
inline Vector<FE_VDIM,F32> &scaleAndAdd(Vector<FE_VDIM,F32> &lhs,U scalar,
        const Vector<FE_VDIM,F32> &rhs)
{
    v4sf_assertAlignment(lhs.simd());
    v4sf_assertAlignment(rhs.simd());
    lhs.checkValid();
    rhs.checkValid();

    v4sf scale;
    v4sf_setSame(scale,scalar);
    v4sf scaled=__builtin_ia32_mulps(scale,lhs.simd());
    lhs.simd()=__builtin_ia32_addps(rhs.simd(),scaled);
    lhs.checkValid();
    return lhs;
}

} /* namespace */