Math.h

Go to the documentation of this file.

/*
 * Expression Template Matrix Library
 *
 * Copyright (C) 2004 - 2006 Ricky Lung <mtlung@users.sourceforge.net>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */


#ifdef _MSC_VER
#pragma once
#endif  // _MSC_VER

#ifndef _EXMAT_SIMD_SSE_MATH_H
#define _EXMAT_SIMD_SSE_MATH_H


namespace exmat { namespace SIMD {



    inline static void SSEVec4fDot_aps(float* d, const float* const l, const float* const r)
    {
        __m128 mv1, mv2, mbuf1, mbuf2;
        mv1 = _mm_load_ps(l);
        mv2 = _mm_load_ps(r);
        mbuf1 = _mm_mul_ps(mv1, mv2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(3, 2, 3, 2));
        mbuf1 = _mm_add_ps(mbuf1, mbuf2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(0, 1, 0, 1));
        mbuf1 = _mm_add_ss(mbuf1, mbuf2);
        _mm_store_ss(d, mbuf1);
    }

    inline static void SSEVec4fDot_ups(float* d, const float* const l, const float* const r)
    {
        __m128 mv1, mv2, mbuf1, mbuf2;
        mv1 = _mm_loadu_ps(l);
        mv2 = _mm_loadu_ps(r);
        mbuf1 = _mm_mul_ps(mv1, mv2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(3, 2, 3, 2));
        mbuf1 = _mm_add_ps(mbuf1, mbuf2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(0, 1, 0, 1));
        mbuf1 = _mm_add_ss(mbuf1, mbuf2);
        _mm_store_ss(d, mbuf1);
    }

    inline static void SSEVec3fDot_aps(float* d, const float* const l, const float* const r)
    {
        __m128 mv1, mv2, mbuf1, mbuf2;
        mv1 = _mm_load_ps(l);
        mv2 = _mm_load_ps(r);
        mbuf1 = _mm_mul_ps(mv1, mv2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(0, 0, 2, 1));//_MM_SHUFFLE(3, 2, 3, 2));
        mbuf1 = _mm_add_ps(mbuf1, mbuf2);
        mbuf2 = _mm_shuffle_ps(mbuf2, mbuf2, _MM_SHUFFLE(0, 0, 0, 1));
        mbuf1 = _mm_add_ss(mbuf1, mbuf2);
        //_mm_store_ps1(v3, mbuf1);
        *d = *((float*)&mbuf1);
    }

    inline static void SSEVec3fDot_ups(float* d, const float* const l, const float* const r)
    {
        __m128 mv1, mv2, mbuf1, mbuf2;
        mv1 = _mm_loadu_ps(l);
        mv2 = _mm_loadu_ps(r);
        mbuf1 = _mm_mul_ps(mv1, mv2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(0, 0, 2, 1));//_MM_SHUFFLE(3, 2, 3, 2));
        mbuf1 = _mm_add_ps(mbuf1, mbuf2);
        mbuf2 = _mm_shuffle_ps(mbuf2, mbuf2, _MM_SHUFFLE(0, 0, 0, 1));
        mbuf1 = _mm_add_ss(mbuf1, mbuf2);
        //_mm_store_ps1(v3, mbuf1);
        *d = *((float*)&mbuf1);
    }




    inline static void SSEVec4fNorm_aps(float* d, const float* const v)
    {
        __m128 mv1, mbuf1, mbuf2;
        mv1 = _mm_loadu_ps(v);
        mbuf1 = _mm_mul_ps(mv1, mv1);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(3, 2, 3, 2));
        mbuf1 = _mm_add_ps(mbuf1, mbuf2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(0, 1, 0, 1));
        mbuf1 = _mm_add_ss(mbuf1, mbuf2);
        _mm_store_ss(d, _mm_sqrt_ss(mbuf1));
    }

    inline static void SSEVec4fNorm_ups(float* d, const float* const v)
    {
        __m128 mv1, mbuf1, mbuf2;
        mv1 = _mm_loadu_ps(v);
        mbuf1 = _mm_mul_ps(mv1, mv1);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(3, 2, 3, 2));
        mbuf1 = _mm_add_ps(mbuf1, mbuf2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(0, 1, 0, 1));
        mbuf1 = _mm_add_ss(mbuf1, mbuf2);
        _mm_store_ss(d, _mm_sqrt_ss(mbuf1));
    }

    inline static void SSEVec3fNorm_aps(float* d, const float* const v)
    {
        __m128 mv1, mbuf1, mbuf2;
        mv1 = _mm_load_ps(v);
        mbuf1 = _mm_mul_ps(mv1, mv1);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(3, 2, 3, 2));
        mbuf1 = _mm_add_ps(mbuf1, mbuf2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(0, 1, 0, 1));
        mbuf1 = _mm_add_ss(mbuf1, mbuf2);
        mbuf1 = _mm_sqrt_ss(mbuf1);
        //_mm_store_ps1(v3, _mm_sqrt_ss(mbuf1));
        *d = *((float*)&mbuf1);
    }

    inline static void SSEVec3fNorm_ups(float* d, const float* const v)
    {
        __m128 mv1, mbuf1, mbuf2;
        mv1 = _mm_load_ps(v);
        mbuf1 = _mm_mul_ps(mv1, mv1);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(3, 2, 3, 2));
        mbuf1 = _mm_add_ps(mbuf1, mbuf2);
        mbuf2 = _mm_shuffle_ps(mbuf1, mbuf1, _MM_SHUFFLE(0, 1, 0, 1));
        mbuf1 = _mm_add_ss(mbuf1, mbuf2);
        mbuf1 = _mm_sqrt_ss(mbuf1);
        //_mm_store_ps1(v3, _mm_sqrt_ss(mbuf1));
        *d = *((float*)&mbuf1);
    }




    inline static void SSEVec3fCross_aps(float* d, const float* const v1, const float* const v2)
    {
        __m128 mv1, mv2, mbuf1;
        mv1 = _mm_load_ps(v1);
        mv1 = _mm_shuffle_ps(mv1, mv1, 201);
        mv2 = _mm_load_ps(v2);
        mbuf1 = _mm_shuffle_ps(mv2, mv2, 210);
        mv2 = _mm_mul_ps(mv2, mv1);
        mbuf1 = _mm_mul_ps(mbuf1, mv1);
        mv2 = _mm_shuffle_ps(mv2, mv2, 201);
        mbuf1 = _mm_sub_ps(mbuf1, mv2);
        //_mm_store_ps(v3, _mm_sub_ps(mbuf1, mv2));
        memcpy(d, &mbuf1, sizeof(float) * 3);
    }

    inline static void SSEVec3fCross_ups(float* d, const float* const v1, const float* const v2)
    {
        __m128 mv1, mv2, mbuf1;
        mv1 = _mm_loadu_ps(v1);
        mv1 = _mm_shuffle_ps(mv1, mv1, 201);
        mv2 = _mm_loadu_ps(v2);
        mbuf1 = _mm_shuffle_ps(mv2, mv2, 210);
        mv2 = _mm_mul_ps(mv2, mv1);
        mbuf1 = _mm_mul_ps(mbuf1, mv1);
        mv2 = _mm_shuffle_ps(mv2, mv2, 201);
        mbuf1 = _mm_sub_ps(mbuf1, mv2);
        //_mm_store_ps(v3, _mm_sub_ps(mbuf1, mv2));
        memcpy(d, &mbuf1, sizeof(float) * 3);
    }



    inline static void SSEVec4fQuatMul_aps(float* d, const float* const l, const float* const r)
    {
        /*An optimization can also be made by rearranging to
        ms1     ms2     ms3     ms4
        w = w1*w2 - x1*x2 - z1*z2 - y1*y2;
        x = x1*w2 + w1*x2 + y1*z2 - z1*y2;
        y = y1*w2 + w1*y2 + z1*x2 - x1*z2;
        z = z1*w2 + w1*z2 + x1*y2 - y1*x2;*/

        __m128 mq1 = _mm_load_ps(l), mq2 = _mm_load_ps(r), mans;
        __m128 ms1 = _mm_mul_ps(_mm_shuffle_ps(mq1, mq1, _MM_SHUFFLE(2, 1, 0, 3)), _mm_shuffle_ps(mq2, mq2, _MM_SHUFFLE(3, 3, 3, 3)));
        __m128 ms2 = _mm_mul_ps(_mm_shuffle_ps(mq1, mq1, _MM_SHUFFLE(3, 3, 3, 0)), _mm_shuffle_ps(mq2, mq2, _MM_SHUFFLE(2, 1, 0, 0)));
        mans = _mm_add_ps(ms1, ms2);
        __m128 ms3 = _mm_mul_ps(_mm_shuffle_ps(mq1, mq1, _MM_SHUFFLE(0, 2, 1, 2)), _mm_shuffle_ps(mq2, mq2, _MM_SHUFFLE(1, 0, 2, 2)));
        mans = _mm_add_ps(mans, ms3);
        __m128 mlastterm = _mm_add_ss(ms2, ms3);
        mans =_mm_sub_ss(mans, mlastterm);
        __m128 ms4 = _mm_mul_ps(_mm_shuffle_ps(mq1, mq1, _MM_SHUFFLE(1, 0, 2, 1)), _mm_shuffle_ps(mq2, mq2, _MM_SHUFFLE(0, 2, 1, 1)));
        mans = _mm_sub_ps(mans, ms4);
        mans =_mm_sub_ss(mans, mlastterm);
        mans = _mm_shuffle_ps(mans, mans, _MM_SHUFFLE(0, 3, 2, 1));
        _mm_store_ps(d, mans);
    }

    inline static void SSEVec4fQuatMul_ups(float* d, const float* const l, const float* const r)
    {
        /*An optimization can also be made by rearranging to
        ms1     ms2     ms3     ms4
        w = w1*w2 - x1*x2 - z1*z2 - y1*y2;
        x = x1*w2 + w1*x2 + y1*z2 - z1*y2;
        y = y1*w2 + w1*y2 + z1*x2 - x1*z2;
        z = z1*w2 + w1*z2 + x1*y2 - y1*x2;*/

        __m128 mq1 = _mm_loadu_ps(l), mq2 = _mm_loadu_ps(r), mans;
        __m128 ms1 = _mm_mul_ps(_mm_shuffle_ps(mq1, mq1, _MM_SHUFFLE(2, 1, 0, 3)), _mm_shuffle_ps(mq2, mq2, _MM_SHUFFLE(3, 3, 3, 3)));
        __m128 ms2 = _mm_mul_ps(_mm_shuffle_ps(mq1, mq1, _MM_SHUFFLE(3, 3, 3, 0)), _mm_shuffle_ps(mq2, mq2, _MM_SHUFFLE(2, 1, 0, 0)));
        mans = _mm_add_ps(ms1, ms2);
        __m128 ms3 = _mm_mul_ps(_mm_shuffle_ps(mq1, mq1, _MM_SHUFFLE(0, 2, 1, 2)), _mm_shuffle_ps(mq2, mq2, _MM_SHUFFLE(1, 0, 2, 2)));
        mans = _mm_add_ps(mans, ms3);
        __m128 mlastterm = _mm_add_ss(ms2, ms3);
        mans =_mm_sub_ss(mans, mlastterm);
        __m128 ms4 = _mm_mul_ps(_mm_shuffle_ps(mq1, mq1, _MM_SHUFFLE(1, 0, 2, 1)), _mm_shuffle_ps(mq2, mq2, _MM_SHUFFLE(0, 2, 1, 1)));
        mans = _mm_sub_ps(mans, ms4);
        mans =_mm_sub_ss(mans, mlastterm);
        mans = _mm_shuffle_ps(mans, mans, _MM_SHUFFLE(0, 3, 2, 1));
        _mm_storeu_ps(d, mans);
    }


};  // namespace SIMD
}   // namespace exmat


#endif  // _EXMAT_SIMD_SSE_MATH_H

---