matlab中的motion,matlab里面的motion c++实现

matlab里面的motion c++实现，返回的是一个双指针矩阵。需要输入角度以及距离；

double PI=3.1415926;

int sx, sy;

float *mask=0;

float *mask2=0;

int mask_width;

int mask_height;

float half = (length-1)/2;

float phi =float((fmod(angle+180,180)/180)*PI);

float cosphi = cos(phi);

float sinphi = sin(phi);

int xsign ;

if(cosphi< 0)

{

xsign = -1;

}

else if (angle==90)

xsign = 0;

else

xsign= 1;

int psfwdt = 1;

sx = (int)fabs(half * cosphi +psfwdt*xsign -length*2.2204e-016);

sy = (int)fabs(half * sinphi + psfwdt -length*2.2204e-016);

mask_width = sx;

mask_height = sy;

// cout<

mask =(float*)malloc(sizeof(float)*(sx+1)*(sy+1));

mask2=(float*)malloc(sizeof(float)*(sx*2+1)*(sy*2+1));

float **dist2psf_matrix = (float**)malloc(sizeof(float*)*(sy+1));

float **psf = (float**)malloc(sizeof(float*)*(sy*2+1));

int i = 0,j = 0;

for(i = 0; i< sy*2+1 ;i++)

{

psf[i]=mask2+i*(sx*2+1);

//psf[i]=mask2+(sy*2 - i ) * (sx*2+1);

}

for(i = 0; i< sy+1 ; i++)

{

dist2psf_matrix[i] = mask + i * (sx+1);

//dist2psf_matrix[i] = mask + (sy - i ) * (sx+1);

}

double rad = 0;

int x_position = 0,x_start = 0;

x_start = 0;

for(i = 0; i < sy+1; i ++)

{

x_position = x_start;

for(j = 0; j < sx+1; j++)

{

dist2psf_matrix[i][j] = i * fabs(cosphi)- j * sinphi;

// cout<

rad = sqrt((float)(i * i + j * j));

if(rad >= half && fabs(dist2psf_matrix[i][j]) <= psfwdt)

{

float x2lastpix = half - fabs((x_position +dist2psf_matrix[i][j] * sinphi) / cosphi);

// cout<

dist2psf_matrix[i][j] = sqrt(dist2psf_matrix[i][j] * dist2psf_matrix[i][j] + x2lastpix * x2lastpix);

//cout<

}

//cout<

dist2psf_matrix[i][j] = psfwdt - fabs(dist2psf_matrix[i][j]);

if(dist2psf_matrix[i][j] < 0) dist2psf_matrix[i][j]=0;

x_position ++;

}

}

float sum=0;

for (i=0;i<=2*sy;++i)

{

for (j=0;j<=2*sx;++j)

{

// cout<

psf[i][j]=0;

}

}

for(i =0; i < sy+1; i ++)

{

for(j = 0; j < sx+1; j++)

{

if (i

{psf[i+sy][j]=dist2psf_matrix[i][sx-j];

sum+=dist2psf_matrix[i][sx-j];

//cout<

}

else

psf[i][j]=0;

}

//cout<

}

sum=sum*2-1;

//cout<

for (i=0;i<=sy;++i){

for (j=0;j<=sx;++j)

{

psf[i][2*sx-j]=psf[(2*sy-i)][j];

}

}

if(angle>90)

{

float temp=0;

for (i=0;i<=2*sy;++i)

{

for(j=0;j<=sx;++j)

{

temp=psf[i][j];

psf[i][j]=psf[i][2*sx-j];

psf[i][2*sx-j]=temp;

}

}

}

cout.precision(3);

for(i=0;i

{

for (j=0;j

{

psf[i][j]/=sum;

//cout<

}

//cout<

}

delete[] dist2psf_matrix;

delete[] mask;