/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----
Filename = [ prob2.c]
* reluxiztion method
----+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
#include <udf.h>
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
#define TNUM 10000 /* maximum number of particle */
#define NC 50 /* mamimum number of candidate particle */
#define ND 100 /* mamimum number of neighbor particle */
#define TM 16 /* maximum displacement (radius) */
#define FORMAT_OF_INPUT 1
#if FORMAT_OF_INPUT==0
#define TN 32 /* neighbor domain size (radius) */
#define TQ 4 /* quasi-rigidity domain size (radius) */
#else
#define TN_R 10 /* neighbor domain size (number of neigbor particle) */
#define TQ_R 15 /* quasi-rigidity domain size (maximum rotating angle (degree)) */
#endif
#define Const_A 0.3
#define Const_B 4.0
#define ITER 100 /* iterative number */
#define MEAN_ENTROPY 0.15 /* mean of entropy */
#define EPS 1.0E-6 /* convergence */
#define EPS1 1.0E-9 /* convergence */
#define LOG_LIMIT 1.0E-12 /* convergence */
#define NOMATCH -1 /* matching particle doesn't exist */
#define rad(deg) ((deg)/180*M_PI) /* degree -> radian */
typedef struct {
double x, y, z;
} DBLPOINT; /* gravity center of particle */
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
FILE *FileOpen(char *fname, char *mode, int flag);
DBLPOINT *ReadData(FILE *fp, int *num);
void PTV(int num_a, DBLPOINT *data_a, int num_b, DBLPOINT *data_b, int *num_ab, int **num_ab_n, int *num_aa, int **num_aa_n, int *pair, double *prob, double *search);
void SearchCandidateParticle(int num_a, DBLPOINT *data_a, int num_b, DBLPOINT *data_b, int *num_n, int **num_nn, double *search);
void SearchNeighborParticle(int num, DBLPOINT *data, int *num_n, int **num_nn, double *search);
void SaveVelocity(FILE *fp, int num_a, DBLPOINT *data_a, DBLPOINT *data_b, int *pair, double *prb);
void SavePropability(FILE *fp, int num_a, int *pair, double *prob);
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
int main(int argc, char **argv)
{
char RFILE[64], SFILE[64], *tmp, ch;
int count, SetFileInp=FALSE, SetFileOutp=FALSE;
FILE *fp_r, *fp_w;
int num_a, num_b; /* number of particle */
DBLPOINT *data_a, *data_b; /* data of paricle coordinate */
int *num_ab, *num_aa; /* number of neighor particle */
int **num_ab_n, **num_aa_n; /* neighbor particle number */
int *pair; /* matched particle number */
double *prob; /* match probability */
#if FORMAT_OF_INPUT == 0
double search[3]={TM, TN, TQ}; /* parameters */
#else
double search[3]={TM, TN_R, TQ_R}; /* parameters */
double width=640.0, height=480.0, depth=100.0; /* image size */
double apn;
#endif
if(argc>1){
count=0;
do{
count++;
tmp=argv[count];
if(*argv[count]=='-'){
ch=*(++tmp);
switch(ch){
case 'i' :
count++;
SetFileInp=TRUE;
strcpy(RFILE,argv[count]);
printf("read argement -i %s\n", RFILE);
break;
case 'o' :
count++;
SetFileOutp=TRUE;
strcpy(SFILE,argv[count]);
printf("read argement -o %s\n", SFILE);
break;
case 's' :
count++;
search[0]=atof(argv[count]);
printf("read argement -s %f\n", search[0]);
break;
case 'n' :
count++;
search[1]=atof(argv[count]);
printf("read argement -n %f\n", search[1]);
break;
case 'q' :
count++;
search[2]=atof(argv[count]);
printf("read argement -q %f\n", search[2]);
break;
#if FORMAT_OF_INPUT == 1
case 'w' :
count++;
width=atof(argv[count]);
printf("read argement -w %f\n", width);
break;
case 'h' :
count++;
height=atof(argv[count]);
printf("read argement -h %f\n", height);
break;
case 'd' :
count++;
depth=atof(argv[count]);
printf("read argement -d %f\n", depth);
break;
#endif
default :
printf("error on command line !!\n");
exit(0);
}
}
else{
printf("error on command line !!\n");
exit(0);
}
}while(count<argc-1);
}
else{
printf("2ptv -i FileInp -o FileOutp -s search -n neighbor -q rigidity\n");
#if FORMAT_OF_INPUT == 1
printf(" -w width -h height -d depth\n");
#endif
printf("-i, -o : Input filename, Output filename\n");
#if FORMAT_OF_INPUT == 0
printf("-s : search size (radius)\n");
printf("-n : neighbor area size (radius)\n");
printf("-q : quasi-rigidity size (radius)\n");
printf("default (-s : %f; -n : %f; -q : %f)\n", search[0], search[1], search[2]);
#else
printf("-w : width of test area size (%f)\n", width);
printf("-h : height of test area size (%f)\n", height);
printf("-d : depth of test area size (%f)\n", depth);
printf("-s : search size (radius)\n");
printf("-n : neighbor area size (number of neighbor particle)\n");
printf("-q : quasi-rigidity size (maximum rorating angle)\n");
printf("default (-s : %f; -n : %f; -q : %f)\n", search[0], search[1], search[2]);
#endif
exit(0);
}
if(SetFileInp==FALSE){
printf("input file name should be inputted.\n");
exit(0);
}
if(SetFileOutp==FALSE){
printf("output file name should be inputted.\n");
exit(0);
}
printf("\n<<<2 Frame PTV program >>\n");
printf(" read filename = %s\n",RFILE);
fp_r=FileOpen(RFILE, "r", 0);
data_a=ReadData(fp_r, &num_a);
data_b=ReadData(fp_r, &num_b);
fclose(fp_r);
printf("\n< 1st > %4d < 2st > %4d\n", num_a, num_b);
#if FORMAT_OF_INPUT == 1
apn=(double)width*(double)height*(double)depth/(((double)num_a+(double)num_b)/2.0);
search[1]=pow(3.0*search[1]*apn/(4.0*M_PI), (1.0/3.0));
search[2]=search[0]*sin(rad(search[2]));
#endif
printf("a radius of search area = %f\n", search[0]);
printf("a radius of nieghbor area = %f\n", search[1]);
printf("a radius of quasi-rigidity area = %f\n", search[2]);
New(pair, num_a, int, "int *pair");
New(prob, num_a, double, "double *prob");
New(num_ab, num_a, int, "int *num_ab");
New(num_aa, num_a, int, "int *num_aa");
num_ab_n=NewInt(NC, num_a);
num_aa_n=NewInt(ND, num_a);
SearchCandidateParticle(num_a, data_a, num_b, data_b, num_ab, num_ab_n, search);
SearchNeighborParticle(num_a, data_a, num_aa, num_aa_n, search);
PTV(num_a, data_a, num_b, data_b, num_ab, num_ab_n, num_aa, num_aa_n, pair, prob, search);
/* ---------- SAVE DATA ---------- */
fp_w=FileOpen(SFILE,"w",1);
SaveVelocity(fp_w, num_a, data_a, data_b, pair, prob);
fclose(fp_w);
/*
fp_w=FileOpen(SFILE,"w",2);
SavePropability(fp_w, num_a, pair, prob);
fclose(fp_w);
*/
/* --- releace memory --- */
free(data_a);
free(data_b);
free(pair);
free(prob);
free(num_ab);
free(num_aa);
FreeInt(num_ab_n, num_a);
FreeInt(num_aa_n, num_a);
return 1;
}
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
FILE *FileOpen(char *fname, char *mode, int flag)
{
char file[128];
FILE *fp;
strcpy(file,"");
strcpy(file,fname);
if(flag==1) strcat(file,".rv");
else if(flag==2) strcat(file,".rp");
else if(flag==3) strcat(file,".re");
if((fp=fopen(file,mode))==NULL) {
printf("\ncannot open file \" %s \"\n",fname);
exit(1); /* open file in failure */
}
return fp;
}
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
DBLPOINT *ReadData(FILE *fp, int *num)
{
int i=0;
DBLPOINT *tmp;
fscanf(fp,"%d\n",num);
if(*num>TNUM){
printf("too samll TNUM : TNUM = %d\n",TNUM);
exit(1);
}
New(tmp, *num, DBLPOINT, "DBLPOINT *tmp");
for(i=0; i<*num; i++){
fscanf(fp,"%lf %lf %lf\n",&tmp[i].x, &tmp[i].y, &tmp[i].z);
// printf("X[%4d]=%10.6f Y[%4d]=%10.6f Z[%4d]=%10.6f\n", i, tmp[i].x, i, tmp[i].y, i, tmp[i].z);
// if(i%20==19) getchar();
}
return tmp;
}
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
void SaveVelocity(FILE *fp, int num_a, DBLPOINT *data_a, DBLPOINT *data_b, int *pair, double *prob)
{
int i, j=0;
double u, v, w;
for(i=0; i<num_a; i++){
if(pair[i]==NOMATCH) continue;
j++;
}
fprintf(fp," %5d\n",j);
for(i=0; i<num_a; i++){
if(pair[i]==NOMATCH) continue;
u=data_b[pair[i]].x-data_a[i].x;
v=data_b[pair[i]].y-data_a[i].y;
w=data_b[pair[i]].z-data_a[i].z;
// fprintf(fp," %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f\n", data_a[i].x, data_a[i].y, data_a[i].z, u, v, w);
fprintf(fp," %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f\n", data_a[i].x, data_a[i].y, data_a[i].z, u, v, w, prob[i]);
}
}
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
void SavePropability(FILE *fp, int num_a, int *pair, double *prob)
{
int i;
fprintf(fp," %5d\n",num_a);
for(i=0; i<num_a; i++){
fprintf(fp," %5d %5d %12.6f\n",i,pair[i],prob[i]);
}
}
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
void SearchCandidateParticle(int num_a, DBLPOINT *data_a, int num_b, DBLPOINT *data_b, int *num_n, int **num_nn, double *search)
{
int i, j;
double x, y, z, rr, r2;
r2=sqr(search[0]);
for(i=0; i<num_a; i++){
num_n[i]=0;
for(j=0; j<num_b; j++){
x=data_a[i].x-data_b[j].x;
y=data_a[i].y-data_b[j].y;
z=data_a[i].z-data_b[j].z;
rr=sqr(x)+sqr(y)+sqr(z);
// printf("(%d, %d) %f (%f)\n", i, j, rr, r2);
if(r2>rr){
num_nn[i][num_n[i]]=j;
num_n[i]++;
if(num_n[i]>=NC){
printf("too small NC : %d", NC);
exit(0);
}
}
}
// for(j=0; j<num_n[i]; j++) printf("%d %d \n", i, num_nn[i][j]);
// getchar();
}
}
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
void SearchNeighborParticle(int num, DBLPOINT *data, int *num_n, int **num_nn, double *search)
{
int i, j, k, l, itmp;
int *nn;
double x, y, z, r2, dtmp;
double *rr;
r2=sqr(search[1]);
New(nn, num, int, "*nn");
New(rr, num, double, "*rr");
for(i=0; i<num; i++){
for(j=0; j<num; j++){
nn[j]=j;
x=data[j].x-data[i].x;
y=data[j].y-data[i].y;
z=data[j].z-data[i].z;
rr[j]=sqr(x)+sqr(y)+sqr(z);
}
for(l=0; l<num-1; l++){
for(k=l+1; k<num; k++){
if(rr[l]>rr[k]){
itmp=nn[k];
nn[k]=nn[l];
nn[l]=itmp;
dtmp=rr[k];
rr[k]=rr[l];
rr[l]=dtmp;
}
}
}
// printf("%d -> ", i);
// for(j=0; j<num; j++) printf("%d ", nn[j]);
for(j=1; j<num; j++){
if(r2<rr[j]) break;
num_nn[i][j-1]=nn[j];
// printf("%d ", num_nn[i][j-1]);
}
num_n[i]=j-1;
if(num_n[i]>=ND){
printf("too small ND : %d", ND);
exit(0);
}
// printf("num[%d]=%d\n", i, num_n[i]);
}
free(nn);
free(rr);
}
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
void PTV( int num_a, DBLPOINT *data_a, int num_b, DBLPOINT *data_b,
int *num_ab, int **num_ab_n, int *num_aa, int **num_aa_n,
int *pair, double *prob, double *search)
{
int i, j, k, l, n, it;
double xij, yij, zij, xkl, ykl, zkl, rr, r2, dev;
double mh, mh_old;
double **p, **q, **p1, **p2;
int *pair1;
double *prob1;
r2=sqr(search[2]);
/* --- memory allocation --- */
New(pair1, num_a, int, "int *pair1");
New(prob1, num_a, double, "int *prob1");
p =NewDouble(NC, num_a);
p1=NewDouble(NC, num_a);
p2=NewDouble(NC, num_a);
q =NewDouble(NC, num_a);
/* --- initialization --- */
for(i=0; i<num_a; i++){
if(num_ab[i]==0) continue;
for(j=0; j<num_ab[i]+1; j++){
// P_all= 1/(M+1)
p[i][j]=1.0/((double)num_ab[i]+1.0);
// printf("%f ", p[i][j]);
}
// printf("\n" );
}
/* --- main --- */
mh_old=-1.0;
it=0;
while(it++<ITER){
printf("iter = %d\t", it);
for(i=0; i<num_a; i++){
if(i%40==39) printf("i = %d\t",i);
if(num_ab[i]==0) continue;
for(j=0; j<num_ab[i]; j++){
// printf("(i, j)=(%d, %d) \n", i, num_ab_n[i][j]);
// calculate distance between x_i and x_j
// x_i : particle on 1st image
// x_j : particle near x_i on 2nd image
xij=data_b[num_ab_n[i][j]].x-data_a[i].x;
yij=data_b[num_ab_n[i][j]].y-data_a[i].y;
zij=data_b[num_ab_n[i][j]].z-data_a[i].z;
q[i][j]=0.0;
// printf("%f -> ", q[i][j]);
if(num_aa[i]==0) continue;
for(k=0; k<num_aa[i]; k++){
if(num_ab[num_aa_n[i][k]]==0) continue;
for(l=0; l<num_ab[num_aa_n[i][k]]; l++){
// printf("(k, l)=(%d, %d) \n", num_aa_n[i][k], num_ab_n[k][l]);
// calculate distance between x_k and x_l
// x_k : particle near x_i on 1st image
// x_l : particle near x_k on 2nd image
xkl=data_b[num_ab_n[num_aa_n[i][k]][l]].x-data_a[num_aa_n[i][k]].x;
ykl=data_b[num_ab_n[num_aa_n[i][k]][l]].y-data_a[num_aa_n[i][k]].y;
zkl=data_b[num_ab_n[num_aa_n[i][k]][l]].z-data_a[num_aa_n[i][k]].z;
rr=sqr(xij-xkl)+sqr(yij-ykl)+sqr(zij-zkl);
if(rr<r2){
// Q_ij^n-1=\sum_k \sum_l P_kl^n-1
q[i][j]+=p[num_aa_n[i][k]][l];
// printf("%f -> ", q[i][j]);
}
}
}
// P_ij^n=A P_ij^n-1 + B Q_ij^n-1
p1[i][j]=Const_A*p[i][j]+Const_B*q[i][j];
// printf("p1=%f\n", p1[i][j]);
// getchar();
}
}
/* --- normalization & calculation of entropy --- */
n=0;
mh=0.0;
for(i=0; i<num_a; i++){
if(num_ab[i]==0) continue;
dev=p[i][num_ab[i]];
for(j=0; j<num_ab[i]; j++){
dev+=p1[i][j];
}
for(j=0; j<num_ab[i]; j++){
p[i][j]=max(p1[i][j]/dev, LOG_LIMIT);
mh-=p[i][j]*log(p[i][j]);
n++;
// printf("%f %f\n", p[i][j], mh);
}
p[i][num_ab[i]]=max(p[i][num_ab[i]]/dev, LOG_LIMIT);
mh-=p[i][num_ab[i]]*log(p[i][num_ab[i]]);
n++;
// printf("%f %f\n", p[i][num_ab[i]], mh);
}
if(n==0){
printf("there are not candidate particle \n");
printf("you should have neighbor area size large.\n");
exit(0);
}
mh/=(double)n;
printf("mean of entropy = %f\n",mh);
/* --- judgement of convergence --- */
if(mh<EPS) break;
if(fabs(mh_old-mh)<EPS1) break;
if(mh>MEAN_ENTROPY && it!=1){
printf("you may change too large MEAN_ENTROPY : %f \n", MEAN_ENTROPY);
break;
}
mh_old=mh;
}
/* --- determine matched particle --- */
for(i=0; i<num_a; i++){
if(num_ab[i]==0){
pair[i]=NOMATCH;
prob[i]=0.0;
continue;
}
pair[i]=NOMATCH;
prob[i]=p[i][num_ab[i]];
pair1[i]=NOMATCH;
prob1[i]=p[i][num_ab[i]];
// printf("i=%d j=%d, p=%f\n", i, -1, p[i][num_ab[i]]);
for(j=0; j<num_ab[i]; j++){
if(prob[i]<p[i][j]){
pair1[i]=pair[i];
prob1[i]=prob[i];
pair[i]=num_ab_n[i][j];
prob[i]=p[i][j];
}
else if(prob1[i]<p[i][j]){
pair1[i]=num_ab_n[i][j];
prob1[i]=p[i][j];
}
}
}
do{
n=0;
for(i=0; i<num_a; i++){
if(pair[i]==NOMATCH) continue;
for(j=0; j<num_a; j++){
if(i==j) continue;
if(pair[j]==NOMATCH) continue;
if(pair[i]!=pair[j]) continue;
if(prob[i]==prob[j]) continue;
if(prob[i]>prob[j]){
pair[j]=pair1[j];
prob[j]=prob1[j];
pair1[j]=NOMATCH;
prob1[j]=p[j][num_ab[j]];
n++;
}
else{
pair[i]=pair1[i];
prob[i]=prob1[i];
pair1[i]=NOMATCH;
prob1[i]=p[i][num_ab[i]];
n++;
}
}
}
// printf("%d ",n);
}while(n!=0);
/* --- release memory --- */
free(pair1);
free(prob1);
FreeDouble(p, num_a);
FreeDouble(p1, num_a);
FreeDouble(p2, num_a);
FreeDouble(q, num_a);
}
/*--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+--*/
使用cygwin编译时产生错误，知道的留下电话，我打过去请教一下，谢谢！
第一次使用gcc,怎么没有udf.h文件？
[招生]科锐逆向工程师培训(2024年11月15日实地，远程教学同时开班, 第51期)
