vec2d.cpp

一个外国人写的人脸检测程序

void vec2D::add(float scalar)           //this = this.+scalar   sse optimized
{
        vec2D& c = *this;
        __m128 mscalar = _mm_load_ps1(&scalar);

        for (unsigned int y = 0; y < c.height(); y++) {
                for (unsigned int x = 0; x < c.width() / 4; x++)
                        _mm_store_ps(&c(y, 4*x), _mm_add_ps(_mm_load_ps(&c(y, 4*x)), mscalar));

                if ((c.width() % 4) != 0) {
                        for (unsigned int x = c.width() - c.width() % 4; x < c.width(); x++)
                                c(y, x) += scalar;
                }
        }
}
void vec2D::sub(float scalar)           //this = this.-scalar   sse optimized
{
        vec2D& c = *this;
        __m128 mscalar = _mm_load_ps1(&scalar);

        for (unsigned int y = 0; y < c.height(); y++) {
                for (unsigned int x = 0; x < c.width() / 4; x++)
                        _mm_store_ps(&c(y, 4*x), _mm_sub_ps(_mm_load_ps(&c(y, 4*x)), mscalar));

                if ((c.width() % 4) != 0) {
                        for (unsigned int x = c.width() - c.width() % 4; x < c.width(); x++)
                                c(y, x) -= scalar;
                }
        }
}
void vec2D::mul(float scalar)           //this = this.*scalar   sse optimized
{
        vec2D& c = *this;
        __m128 mscalar = _mm_load_ps1(&scalar);

        for (unsigned int y = 0; y < c.height(); y++) {
                for (unsigned int x = 0; x < c.width() / 4; x++)
                        _mm_store_ps(&c(y, 4*x), _mm_mul_ps(_mm_load_ps(&c(y, 4*x)), mscalar));

                if ((c.width() % 4) != 0) {
                        for (unsigned int x = c.width() - c.width() % 4; x < c.width(); x++)
                                c(y, x) *= scalar;
                }
        }
}
void vec2D::div(float scalar)           //this = this./scalar   sse optimized
{
        vec2D& c = *this;
        __m128 mscalar = _mm_load_ps1(&scalar);

        for (unsigned int y = 0; y < c.height(); y++) {
                for (unsigned int x = 0; x < c.width() / 4; x++)
                        _mm_store_ps(&c(y, 4*x), _mm_div_ps(_mm_load_ps(&c(y, 4*x)), mscalar));

                if ((c.width() % 4) != 0) {
                        for (unsigned int x = c.width() - c.width() % 4; x < c.width(); x++)
                                c(y, x) /= scalar;
                }
        }
}
float vec2D::prod() const
{
        const vec2D& c = *this;

        float fres = 1.0f;
        __declspec(align(16)) float tmp[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
        __m128 mres;

        for (unsigned int y = 0; y < height(); y++) {
                for (unsigned int x = 0; x < width() / 4; x++) {
                        if (y == 0)
                                mres = _mm_load_ps(c.data(0, 0));
                        else
                                mres = _mm_mul_ps(mres, _mm_load_ps(c.data(y, 4 * x)));
                }

                if ((width() % 4) != 0) {
                        for (unsigned int x = width() - width() % 4; x < width(); x++)
                                fres *= c(y, x);
                }
        }

        //mres: a,b,c,d
        __m128 m = _mm_movehl_ps(mres, mres);    //m:  c,d,c,d
        mres = _mm_mul_ps(m, mres);              //mres: a*c,b*d,*,*
        _mm_store_ps(tmp, mres);

        return tmp[0] * tmp[1] * fres;
}

void vec2D::inter2(const vec2D& src, vec2D& dst_grdx, vec2D& dst_grdy)                //2d interpolation
{
        vec2D& trg = *this;

        float xrto = float(width() - 1) / float(src.width() - 1);       //trg/src
        float yrto = float(height() - 1) / float(src.height() - 1);

        //arrange before srcx,srcy,frcx,frcy to speed calcs
        for (unsigned int y = 0; y < height(); y++) {
                float srcy = (float)y / yrto;
                float frcy = srcy - float((int)srcy);
                dst_grdy(0, y) = srcy;
                dst_grdy(1, y) = frcy;
        }

        for (unsigned int x = 0; x < width(); x++) {
                float srcx = (float)x / xrto;               //position to take from src vec2D
                float frcx = srcx - float((int)srcx);       //srcx=1.34  frcx=.34
                dst_grdx(0, x) = srcx;
                dst_grdx(1, x) = frcx;
        }
        //arrange before srcx,srcy,frcx,frcy to speed calcs

        for (unsigned int y = 0; y < height(); y++) {
                for (unsigned int x = 0; x < width(); x++) {

                        unsigned int sx = (unsigned int)dst_grdx(0, x);         //x index to source
                        unsigned int sy = (unsigned int)dst_grdy(0, y);         //y index to source
                        trg(y, x) = src(sy, sx) * (1.0f - dst_grdy(1, y)) * (1.0f - dst_grdx(1, x));        //1-frcy 1-frcx

                        if (dst_grdx(1, x) > 0.0f && sx + 1 < src.width())
                                trg(y, x) += src(sy, sx + 1) * (1.0f - dst_grdy(1, y)) * dst_grdx(1, x);        //1-frcy frcx
                        if (dst_grdy(1, y) > 0.0f && sy + 1 < src.height())
                                trg(y, x) += src(sy + 1, sx) * dst_grdy(1, y) * (1.0f - dst_grdx(1, x));        //frcy 1-frcx
                        if ((dst_grdx(1, x) > 0.0f && dst_grdy(1, y) > 0.0f) && (sx + 1 < src.width() && sy + 1 < src.height()))
                                trg(y, x) += src(sy + 1, sx + 1) * dst_grdy(1, y) * dst_grdx(1, x);            //frcy frcx
                }
        }
}
///////zero offset operations////////////////////////////////////////////////////





///////////////////mat operations///////////////////////////////////////////////
void vec2D::conv2D(const vec2D& a, const vec2D& filter)        //this = conv2D(A, filter)  (this = A in sizes)
{
        vec2D& c = *this;
        for (int y = c.yfirst(); y <= c.ylast(); y++) {
                for (int x = c.xfirst(); x <= c.xlast(); x++) {

                        c(y, x) = 0.0f;
                        for (int n = filter.yfirst(); n <= filter.ylast(); n++)
                                for (int m = filter.xfirst(); m <= filter.xlast(); m++)
                                        c(y, x) += filter(n, m) * a.get(y + n, x + m);
                }
        }
}
void vec2D::conv2D(const vec2D& a, const vec2D& re, const vec2D& im)        //this = conv2D(A, Im,Re)  (this = A in sizes)
{
        vec2D& c = *this;
        for (int y = c.yfirst(); y <= c.ylast(); y++) {
                for (int x = c.xfirst(); x <= c.xlast(); x++) {

                        float imag = 0.0f;
                        float real = 0.0f, tmp;
                        for (int n = re.yfirst(); n <= re.ylast(); n++) {
                                for (int m = re.xfirst(); m <= re.xlast(); m++) {
                                        tmp = a.get(y + n, x + m);
                                        real += re(n, m) * tmp;
                                        imag += im(n, m) * tmp;
                                }
                        }
                        c(y, x) = sqrt(real * real + imag * imag);  //abs value
                }
        }
}
void vec2D::minmax(float& min, float& max) const
{
        const vec2D& c = *this;

        min = c(yfirst(), xfirst());
        max = min;

        for (int y = c.yfirst(); y <= c.ylast(); y++) {
                for (int x = c.xfirst(); x <= c.xlast(); x++) {
                        if (c(y, x) < min) min = c(y, x);
                        if (c(y, x) > max) max = c(y, x);
                }
        }
}
float vec2D::minval() const
{
        const vec2D& c = *this;

        float min = c(yfirst(), xfirst());

        for (int y = c.yfirst(); y <= c.ylast(); y++) {
                for (int x = c.xfirst(); x <= c.xlast(); x++) {
                        if (c(y, x) < min) min = c(y, x);
                }
        }

        return min;
}
float vec2D::maxval() const
{
        const vec2D& c = *this;

        float max = c(yfirst(), xfirst());

        for (int y = c.yfirst(); y <= c.ylast(); y++) {
                for (int x = c.xfirst(); x <= c.xlast(); x++) {
                        if (c(y, x) > max) max = c(y, x);
                }
        }

        return max;
}

//get max value with [x,y] coord from offset dx,dy in rect [sizex,sizey]
void vec2D::maxval(float& max, int& x, int& y, int sizex, int sizey, int dx, int dy) const
{
        const vec2D& c = *this;

        max = -FLT_MAX;

        if (dx == 0 || dx < (sizex - 1) / 2)
                dx = (sizex - 1) / 2;
        if (dy == 0 || dy < (sizey - 1) / 2)
                dy = (sizey - 1) / 2;

        x = dx;
        y = dy;

        float tmp = 0.0f;
        for (unsigned int n = dy; n < height() - dy; n++) {
                for (unsigned int m = dx; m < width() - dx; m++) {

                        tmp = 0.0f;
                        for (int i = -(sizey - 1) / 2; i <= (sizey - 1) / 2; i++)
                                for (int j = -(sizex - 1) / 2; j <= (sizex - 1) / 2; j++)
                                        tmp += c(n + i, m + j);

                        if (tmp > max) {
                                max = tmp;
                                y = n;
                                x = m;
                        }
                }
        }
}

void vec2D::normalize(float a, float b)
{
        vec2D& c = *this;

        float min, max;
        minmax(min, max);

        c.sub(min);
        c.mul((b - a) / (max - min));
        c.add(a);
}
void vec2D::histeq(vec2D &hist)    //hist 1x256 array
{
        vec2D& c = *this;

        hist.set(0.0f);

        for (int y = c.yfirst(); y <= c.ylast(); y++)
                for (int x = c.xfirst(); x <= c.xlast(); x++)
                        hist(0, int(c(y, x)))++;

        for (unsigned int x = 1; x < hist.width(); x++)
                hist((unsigned)0, x) += hist((unsigned)0, x - 1);

        hist.div(float(width() * height()));

        for (int y = c.yfirst(); y <= c.ylast(); y++)
                for (int x = c.xfirst(); x <= c.xlast(); x++)
                        c(y, x) = (float) hist(0, int(c(y, x)));
}
void vec2D::fliplr()
{
        vec2D& c = *this;
        float tmp;

        for (unsigned int i = 0; i < width() / 2; i++) {
                for (int y = yfirst(); y <= ylast(); y++) {
                        tmp = c(y, xfirst() + i);
                        c(y, xfirst() + i) = c(y, xlast() - i);
                        c(y, xlast() - i) = tmp;
                }
        }
}