BilinearSampler with SSE.

BilinearSampler.hpp

#ifndef __simd_bilinear_sampler__
#define __simd_bilinear_sampler__

#include <immintrin.h>


class BilinearSampler {
protected:
    const uint8_t* _pixels;
    int _rowInBytes;
    __m128 _size;
    inline void color_add(__m128& rgb, float weight, int x, int y) const
    {
        const uint8_t* pix = _pixels + x*4 + y*_rowInBytes;
        __m128 col = _mm_setr_ps(pix[0], pix[1], pix[2], pix[3]);
        __m128 w = {weight, weight, weight, weight};
        col = _mm_mul_ps(col, w);
        rgb = _mm_add_ps(rgb, col);
    }

public:
    BilinearSampler(const uint8_t* source=NULL, int width=0, int height=0)
    {
        attach(source, width, height);
    }
    virtual ~BilinearSampler() {}

    void attach(const uint8_t* source, int width, int height)
    {
        _pixels = source;
        _size = _mm_setr_ps(width-1, height-1, width-1, height-1);
        _rowInBytes = width*4;
    }

    void sample(uint8_t* result, float x, float y) const
    {
        static const __m128 zero = {0,0,0,0};
        static const __m128 color_max = {255,255,255,255};

        int ix = x;
        int iy = y;
        __m128 xy = _mm_setr_ps(ix, iy, ix+1, iy+1);
        xy = _mm_max_ps(xy, zero);
        xy = _mm_min_ps(xy, _size);

        float ax = x-ix;
        float ay = y-iy;
        __m128 rgb = {0,0,0,0};
        color_add(rgb, (1-ax)*(1-ay), xy[0], xy[1]);
        color_add(rgb, (  ax)*(1-ay), xy[2], xy[1]);
        color_add(rgb, (1-ax)*(  ay), xy[0], xy[3]);
        color_add(rgb, (  ax)*(  ay), xy[2], xy[3]);

        rgb = _mm_max_ps(rgb, zero);
        rgb = _mm_min_ps(rgb, color_max);
        result[0] = rgb[0];
        result[1] = rgb[1];
        result[2] = rgb[2];
        result[3] = rgb[3];
    }
};

#endif /* defined(__simd_bilinear_sampler__) */