-
Notifications
You must be signed in to change notification settings - Fork 1
/
csearch.c
135 lines (121 loc) · 3.99 KB
/
csearch.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
#include "mex.h"
#include "stdlib.h"
#include "stdio.h"
#include "math.h"
#define LOOPMAX 10000 /* maximum count of search loop */
#define SGN(x) ((x)<=0.0?-1.0:1.0)
#define ROUND(x) (floor((x)+0.5))
#define SWAP(x,y) do {double tmp_; tmp_=x; x=y; y=tmp_;} while (0)
/* new matrix ------------------------------------------------------------------
* allocate memory of matrix
* args : int n,m I number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
double *mat(int n, int m)
{
double *p;
if (n<=0||m<=0) return NULL;
if (!(p=(double *)malloc(sizeof(double)*n*m))) {
fprintf(stderr,"matrix memory allocation error: n=%d,m=%d\n",n,m);
return -1;
}
return p;
}
/* zero matrix -----------------------------------------------------------------
* generate new zero matrix
* args : int n,m I number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
double *zeros(int n, int m)
{
double *p;
#if NOCALLOC
if ((p=mat(n,m))) for (n=n*m-1;n>=0;n--) p[n]=0.0;
#else
if (n<=0||m<=0) return NULL;
if (!(p=(double *)calloc(sizeof(double),n*m))) {
fprintf(stderr,"matrix memory allocation error: n=%d,m=%d\n",n,m);
return -1;
}
#endif
return p;
}
/* modified lambda (mlambda) search (ref. [2]) -------------------------------*/
int csearch(int n, int m, const double *L, const double *D,
const double *zs, double *zn, double *s)
{
int i,j,k,c,nn=0,imax=0;
double newdist,maxdist=1E99,y;
double *S=zeros(n,n),*dist=mat(n,1),*zb=mat(n,1),*z=mat(n,1),*step=mat(n,1);
k=n-1; dist[k]=0.0;
zb[k]=zs[k];
z[k]=ROUND(zb[k]); y=zb[k]-z[k]; step[k]=SGN(y);
for (c=0;c<LOOPMAX;c++) {
newdist=dist[k]+y*y/D[k];
if (newdist<maxdist) {
if (k!=0) {
dist[--k]=newdist;
for (i=0;i<=k;i++)
S[k+i*n]=S[k+1+i*n]+(z[k+1]-zb[k+1])*L[k+1+i*n];
zb[k]=zs[k]+S[k+k*n];
z[k]=ROUND(zb[k]); y=zb[k]-z[k]; step[k]=SGN(y);
}
else {
if (nn<m) {
if (nn==0||newdist>s[imax]) imax=nn;
for (i=0;i<n;i++) zn[i+nn*n]=z[i];
s[nn++]=newdist;
}
else {
if (newdist<s[imax]) {
for (i=0;i<n;i++) zn[i+imax*n]=z[i];
s[imax]=newdist;
for (i=imax=0;i<m;i++) if (s[imax]<s[i]) imax=i;
}
maxdist=s[imax];
}
z[0]+=step[0]; y=zb[0]-z[0]; step[0]=-step[0]-SGN(step[0]);
}
}
else {
if (k==n-1) break;
else {
k++;
z[k]+=step[k]; y=zb[k]-z[k]; step[k]=-step[k]-SGN(step[k]);
}
}
}
for (i=0;i<m-1;i++) { /* sort by s */
for (j=i+1;j<m;j++) {
if (s[i]<s[j]) continue;
SWAP(s[i],s[j]);
for (k=0;k<n;k++) SWAP(zn[k+i*n],zn[k+j*n]);
}
}
free(S);
free(dist);
free(zb);
free(z);
free(step);
if (c>=LOOPMAX) {
fprintf(stderr,"%s : search loop count overflow\n",__FILE__);
return -1;
}
return 0;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double *L,*D,*zs,*s,*zn;
int n,m=2;
n = mxGetM(prhs[0]);// get cols of c
zs= mxGetPr(prhs[0]);
L = mxGetPr(prhs[1]);
D = mxGetPr(prhs[2]);
m = mxGetScalar(prhs[3]);
plhs[0] = mxCreateDoubleMatrix(n,m, mxREAL); // afixed
plhs[1] = mxCreateDoubleMatrix(1,m, mxREAL); // ratios
zn=(double*)mxGetData(plhs[0]);
s=(double*)mxGetData(plhs[1]);
csearch(n, m, L, D, zs, zn,s);
// [afixed,sqnorm]=ssearch(ahat,L,D,ncands)
}