vec2d.cpp

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

#include "stdafx.h"
#include "vec2d.h"





/////////////////////////constructors/destructors////////////////////////////////////////////////////
vec2D::vec2D(unsigned int ysize, unsigned int xsize,
             int yoffset, int xoffset, const float* data) : m_width(xsize), m_height(ysize),
                m_xoffset(xoffset), m_yoffset(yoffset), m_data(0)
{
        init(ysize, xsize, yoffset, xoffset);

        const float* pdata = data;
        for (int y = yfirst(); y <= ylast(); y++) {
                for (int x = xfirst(); x <= xlast() ; x++) {
                        if (data != 0)
                                m_data[y][x] = *pdata++;
                        else
                                m_data[y][x] = 0.0f;
                }
        }
}
vec2D::vec2D(const vec2D& v)
{
        init(v.height(), v.width(), v.yfirst(), v.xfirst());
        init(v);
}
vec2D::vec2D(const wchar_t* file) : m_width(0), m_height(0), m_xlast(0), m_ylast(0),
                m_xoffset(0), m_yoffset(0), m_data(0)
{
        float val;

        FILE* fp = _wfopen(file, L"rt");
        if (fp) {
                if (fwscanf(fp, L"%d %d", &m_height, &m_width) != 2) {
                        fclose(fp);
                        init(1, 1);
                        return;
                }

                init(m_height, m_width);

                for (int y = yfirst(); y <= ylast(); y++) {
                        for (int x = xfirst(); x <= xlast() ; x++) {
                                if (fwscanf(fp, L"%f", &val) != 1) {
                                        m_data[y][x] = 0.0f;
                                } else
                                        m_data[y][x] = val;
                        }
                }

                fclose(fp);
        } else
                init(1, 1);
}
vec2D::~vec2D()
{
        if (m_data != 0)
                close();
}
////////////////////////////////////////////////////////////////////////////////





//////////////////////init,free memory//////////////////////////////////////////
void vec2D::init(unsigned int ysize, unsigned int xsize, int yoffset, int xoffset)
{
        m_width = xsize;
        m_height = ysize;
        m_xoffset = xoffset;
        m_yoffset = yoffset;

        m_xlast = (m_width + m_xoffset) - 1;
        m_ylast = (m_height + m_yoffset) - 1;

        m_data = (float**) malloc(m_height * sizeof(float*));         //setup rows
        for (unsigned int j = 0; j < m_height; j++) {
                m_data[j] = (float*) _aligned_malloc(m_width * sizeof(float), 16);       //setup cols
                m_data[j] -= m_xoffset;
        }
        m_data -= m_yoffset;
}
void vec2D::init(const vec2D& v)
{
        for (int y = v.yfirst(); y <= v.ylast(); y++)
                for (int x = v.xfirst(); x <= v.xlast(); x++)
                        (*this)(y, x) = v(y, x);
}
void vec2D::close(void)
{
        m_data += m_yoffset;
        for (unsigned int j = 0; j < m_height; j++)
                _aligned_free(m_data[j] + m_xoffset);      //delete colums
        free(m_data);       //delete rows

        m_data = 0;
        m_width = 0;
        m_height = 0;
}
void vec2D::print(const wchar_t* file) const
{
        const vec2D &v = *this;
        if (file) {
                FILE *fp = _wfopen(file, L"wt");
                if (fp) {
                        fwprintf(fp, L"\n vec: %p\n", this);
                        for (int y = v.yfirst(); y <= v.ylast(); y++) {
                                for (int x = v.xfirst(); x <= v.xlast(); x++)
                                        fwprintf(fp, L" %g", v(y, x));
                                fwprintf(fp, L"\n");
                        }
                        fclose(fp);
                }
        } else {
                wprintf(L"\n vec: %p\n", this);
                for (int y = v.yfirst(); y <= v.ylast(); y++) {
                        for (int x = v.xfirst(); x <= v.xlast(); x++)
                                wprintf(L" %g", v(y, x));
                        wprintf(L"\n");
                }
        }
}
////////////////////////////////////////////////////////////////////////////////






/////////////////////operators//////////////////////////////////////////////////
void vec2D::set(float scalar)
{
        vec2D &v = *this;
        for (int y = v.yfirst(); y <= v.ylast(); y++)
                for (int x = v.xfirst(); x <= v.xlast(); x++)
                        v(y, x) = scalar;
}
//set to Rect [ left, right )
//            [ top, bottom )
void vec2D::set(float scalar, RECT& r)                              //0-offset array function
{
        vec2D &v = *this;

        if (v.xfirst() != 0 || v.yfirst() != 0)
                return;

        if (r.top < 0) r.top = 0;
        if (r.left < 0) r.left = 0;
        if ((unsigned int)r.bottom > v.height()) r.bottom = v.height();
        if ((unsigned int)r.right > v.width()) r.right = v.width();

        __m128 mscalar = _mm_load_ps1(&scalar);
        unsigned int width = r.right - r.left;

        for (unsigned int y = (unsigned int)r.top; y < (unsigned int)r.bottom; y++) {
                for (unsigned int x = 0; x < width / 4; x++)
                        _mm_storeu_ps(&v(y, r.left + 4*x), mscalar);      //store unaligned r.left%4 might be nonzero

                if (width % 4) {
                        for (unsigned int x = width - width % 4; x < width; x++)
                                v(y, r.left + x) = scalar;
                }
        }
}
void vec2D::setrand()
{
        vec2D &v = *this;

        int r;
        srand((unsigned int)time(0));
        for (int y = v.yfirst(); y <= v.ylast(); y++) {
                for (int x = v.xfirst(); x <= v.xlast(); x++) {
                        r = 0xFFF & rand();
                        r -= 0x800;
                        v(y, x) = (float)r / 2048.0f;
                }
        }
}

bool vec2D::copy(const vec2D& v, int left, int top)                                        //copy [hxw] region from top,left offset A to this
{
        //0-offset array function
        //v size >= this size !
        vec2D& pv = *this;
        if ((v.xfirst() || v.yfirst()) || (pv.xfirst() || pv.yfirst()))
                return false;

        RECT r;                           //selected rectangle
        r.left = left;                    //left,top coords
        r.top = top;
        r.right = r.left + width();       //right,bottom coords
        r.bottom = r.top + height();

        if ((r.left >= 0 && r.top >= 0) && (r.right <= (int)v.width() && r.bottom <= (int)v.height())) {       //operator()
                for (unsigned int y = 0; y < height(); y++)
                        for (unsigned int x = 0; x < width(); x++)
                                pv(y, x) = v(y + r.top, x + r.left);
        } else { //get
                for (unsigned int y = 0; y < height(); y++)
                        for (unsigned int x = 0; x < width(); x++)
                                pv(y, x) = v.get(y + r.top, x + r.left);
        }

        return true;
}
/////////////////////////////////////////////////////////////////////////////////



///////zero offset operations////////////////////////////////////////////////////
void vec2D::add(const vec2D& a, const vec2D& b)      //this = a.+b
{
        vec2D& c = *this;

        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(a.data(y, 4*x)), _mm_load_ps(b.data(y, 4*x))));

                if ((c.width() % 4) != 0) {
                        for (unsigned int x = c.width() - c.width() % 4; x < c.width(); x++)
                                c(y, x) = a(y, x) + b(y, x);
                }
        }
}
void vec2D::sub(const vec2D& a, const vec2D& b)      //this = a.-b
{
        vec2D& c = *this;
        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(a.data(y, 4*x)), _mm_load_ps(b.data(y, 4*x))));

                if ((c.width() % 4) != 0) {
                        for (unsigned int x = c.width() - c.width() % 4; x < c.width(); x++)
                                c(y, x) = a(y, x) - b(y, x);
                }
        }
}
void vec2D::mule(const vec2D& a, const vec2D& b)      //this = a.*b
{
        vec2D& c = *this;
        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(a.data(y, 4*x)), _mm_load_ps(b.data(y, 4*x))));

                if ((c.width() % 4) != 0) {
                        for (unsigned int x = c.width() - c.width() % 4; x < c.width(); x++)
                                c(y, x) = a(y, x) * b(y, x);
                }
        }
}
void vec2D::mul(const vec2D& a, const vec2D& b)      //this = a*b
{
        vec2D& c = *this;
        for (unsigned int y = 0; y < c.height(); y++) {
                for (unsigned int x = 0; x < c.width(); x++) {
                        c(y, x) = 0.0f;
                        for (unsigned int i = 0; i < a.width(); i++)
                                c(y, x) += a(y, i) * b(i, x);
                }
        }
}
void vec2D::mult(const vec2D& a, const vec2D& b)      //this = a*b'
{
        vec2D& c = *this;
        for (unsigned int y = 0; y < c.height(); y++)
                for (unsigned int x = 0; x < c.width(); x++)
                        c(y, x) = mconv(a.data(y, 0), b.data(x, 0), a.width());
}
void vec2D::div(const vec2D& a, const vec2D& b)      //this = a./b
{
        vec2D& c = *this;
        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(a.data(y, 4*x)), _mm_load_ps(b.data(y, 4*x))));

                if ((c.width() % 4) != 0) {
                        for (unsigned int x = c.width() - c.width() % 4; x < c.width(); x++)
                                c(y, x) = a(y, x) / b(y, x);
                }
        }
}