int main (int argc, char * argv[]) {
FILE * fp=NULL;
int nsteps = 10000;
vec * L=NULL;
double * dist;
int i, w;
int nwalkers = 100;
L=(vec*)malloc(nsteps*sizeof(vec));
dist=(double*)malloc(nsteps*sizeof(vec));
for (i=1;i<nsteps;i++) {
dist[i]=0.0;
}
L[0].x=L[0].y=L[0].z=0.0;
for (w=0;w<nwalkers;w++) {
fprintf(stdout,"Walker %d...\n",w);
for (i=1;i<nsteps;i++) {
random_step(L,i,1.0);
dist[i]+=diffnorm(&L[i],&L[0]);
}
}
for (i=1;i<nsteps;i++) {
dist[i]/=nwalkers;
}
fp=fopen("out.dat","w");
for (i=0;i<nsteps;i++) {
fprintf(fp,"%i %.5lf\n",i,dist[i]);
}
fclose(fp);
}
static
void ScanMatchingB(double *xyt_scan,double *x_jk,double* A,double* B,
int nrangesA,int nrangesB){
double T0[3];
double T1[3] = {0.5,0.5,0.5};
int mi[500];
int wi[500];
memcpy(T0,x_jk,sizeof(double)*3);
double vec_b[3],vec_a[3],vec_ac[3];
double dmax = 0.05;
double m[3];
int total_match = 0;
while(diffnorm(T0,T1,3) > 1e-6){
#ifdef DEBUG
printf("diff error = %f, [%f %f %f]\n",diffnorm(T0,T1,3),T0[0],T0[1],T0[2]);
#endif
memset(mi,0,250*sizeof(int));
memset(wi,0,250*sizeof(int));
total_match = 0;
for(int i=0;i<nrangesB;i++){
double x_b = B[i*2+0];
double y_b = B[i*2+1];
vec_b[0] = x_b; vec_b[1] = y_b; vec_b[2] = 1;
MatVecProd(T0,vec_b,vec_ac);
double min_val = 1e6;
for(int j=0;j<nrangesA;j++){
double x_a = A[j*2 + 0];
double y_a = A[j*2 + 1];
vec_a[0]=x_a; vec_a[1] = y_a; vec_a[2] = 1;
double dist = diffnorm(vec_ac,vec_a,3);
if(dist < min_val){
min_val = dist;
mi[i] = j;
}
}
m[0] = A[ mi[i]*2 + 0 ];
m[1] = A[ mi[i]*2 + 1 ];
m[2] = 1;
if(diffnorm(m,vec_ac,2) <= dmax)
wi[i] = 1;
else
wi[i] = 0;
total_match += wi[i];
}
memcpy(T1,T0,sizeof(double)*3);
double Sxk = 0, Syk = 0;
double Sxk1 = 0, Syk1 = 0;
double Sxkxk1 =0, Sxkyk1 = 0;
double Sykxk1 = 0, Sykyk1 = 0;
for(int i=0;i<nrangesB;i++){
double x_k = A[mi[i]*2 + 0];
double y_k = A[mi[i]*2 + 1];;
double x_k1 = B[i*2+0];
double y_k1 = B[i*2+1];
Sxk += wi[i]*x_k;
Syk += wi[i]*y_k;
Sxk1 += wi[i]*x_k1;
Syk1 += wi[i]*y_k1;
Sxkxk1 += wi[i] * x_k*x_k1;
Sykyk1 += wi[i] * y_k*y_k1;
Sxkyk1 += wi[i] * x_k*y_k1;
Sykxk1 += wi[i] * y_k*x_k1;
}
double tan_deltNr = Sxk*Syk1 + total_match*Sykxk1 - total_match*Sxkyk1 - Sxk1*Syk;
double tan_deltDr = total_match*Sxkxk1 + total_match*Sykyk1 - Sxk*Sxk1 - Syk*Syk1;
//double tan_delt = tan_deltNr/tan_deltDr;
double del_t = atan2(tan_deltNr,tan_deltDr);
double del_x = (Sxk - cos(del_t)*Sxk1 + sin(del_t)*Syk1)/total_match;
double del_y = (Syk - sin(del_t)*Sxk1 - cos(del_t)*Syk1)/total_match;
T0[0] = del_x;
T0[1] = del_y;
T0[2] = del_t;
}
memcpy(xyt_scan,T0,sizeof(double)*3);
}
