📄 pcrb.c
字号:
r = l*nm;
t = r*m;
lm = l*m;
fx_hy = (map[indexYhy + r]*(1 - Yx) + map[indexYhy + 1 + r ]*Yx)*(1 - Yhy) + (map[indexYhy + nR + r]*(1 - Yx) + map[indexYhy + nR + 1 + r]*Yx)*Yhy;
fx_yh = (map[indexYyh + r]*(1 - Yx) + map[indexYyh + 1 + r]*Yx)*(1 - Yyh) + (map[indexYyh + nR + r]*(1 - Yx) + map[indexYyh + nR + 1 + r]*Yx)*Yyh;
fhx_y = (map[indexYhx + r]*(1 - Yhx) + map[indexYhx + 1 + r]*Yhx)*(1 - Yy) + (map[indexYhx + nR + r]*(1 - Yhx) + map[indexYhx + nR + 1 + r]*Yhx)*Yy;
fxh_y = (map[indexYxh + r]*(1 - Yxh) + map[indexYxh + 1 + r]*Yxh)*(1 - Yy) + (map[indexYxh + nR + r]*(1 - Yxh) + map[indexYxh + nR + 1 + r]*Yxh)*Yy;
/* Gradient Evaluation */
grad_Zi[0 + i2] = (fx_yh - fx_hy)*hyy;
grad_Zi[1 + i2] = (fxh_y - fhx_y)*hxx;
/* Covariance measurement */
for (k = 0 ; k < m ; k++)
{
v = k*nm + t;
Ri[k + lm] = (R[indexY + v]*(1 - Yx) + R[indexY + 1 + v]*Yx)*(1 - Yy) + (R[indexY + nR + v]*(1 - Yx) + R[indexY + nR + 1 + v ]*Yx)*Yy;
}
}
/*------ Compute Grad_Zi*R(^-1)*Grad_Zi^T ------*/
YRinvYt(grad_Zi , Ri ,
gRiinvgt ,
Riinv , gRiinv , vect , vv , indx , d2 , m);
/* gRi (2 x m) = grad_Zi*Ri (2 x m)*(m x m)*/
// BLASCALL(dgemm)("N", "N", &d2, &m, &m, &one, grad_Zi, &d2, Ri, &m, &zero, gRi, &d2);
// k p n k n k
/* gRigt (2 x 2) = gRi*gt (2 x m)*(m x 2) */
// BLASCALL(dgemm)("N", "T", &d2, &d2, &m, &one, gRi, &d2 , grad_Zi , &d2 , &zero , gRigt, &d2);
// k p n k n k
for (l = 0 ; l < 4 ; l++)
{
EgRiinvgt[l] += gRiinvgt[l];
}
}
}
if (compteur != 0.0)
{
cte_compteur = 1.0/compteur;
}
for (l = 0 ; l < 4 ; l++)
{
EgRiinvgt[l] *= cte_compteur;
}
/*------ Compute Hp^T*(Grad_Zi*R(^-1)*Grad_Zi^T)*Hp = E_PI ------*/
HptEgRiinvgtHp(Hp , EgRiinvgt , E_PI , HptEgRiinvgt , d2 , d);
return compteur;
}
/*-------------------------------------------------------------------------*/
void HptEgRiinvgtHp(double *Hp , double *EgRiinvgt , double *E_PI ,
double *HptEgRiinvgt , int d2 , int d)
{
int i , t , l , lm , td2 ;
register double temp;
/* a ) --------- HptEgRiinvgt = Hpt*EgRiinvgt (d x 2) -----------*/
for (t = 0 ; t < d ; t++)
{
td2 = t*d2;
for(l = 0 ; l < d2 ; l++)
{
lm = l*d2;
temp = 0.0;
for(i = 0 ; i < d2 ; i++)
{
temp += Hp[i + td2 ]*EgRiinvgt[i + lm];
}
HptEgRiinvgt[t + l*d] = temp;
}
}
/* b ) --------- gRiinvgt = gRiinv*grad_Zi^T (d x d) -----------*/
for (t = 0 ; t < d ; t++)
{
for(l = 0 ; l < d ; l++)
{
lm = l*d2;
temp = 0.0;
for(i = 0 ; i < d2 ; i++)
{
temp += HptEgRiinvgt[t + i*d ]*Hp[i + lm];
}
E_PI[t + l*d] = temp;
}
}
}
/*-------------------------------------------------------------------------*/
void YRinvYt(double *grad_Zi , double *Ri , double *gRiinvgt ,
double *Riinv , double *gRiinv , double *vect , double *vv ,
int *indx , int d2 , int m)
{
double d , temp;
int i , j , t , l , jm , lm , i2;
/* a) --------- Riinv = Ri^(-1) (m x m) -----------*/
if (m == 1)
{
Riinv[0] = 1.0/Ri[0];
}
else
{
if(ludcmp(Ri, m , indx, &d , vv))
{
for(j = 0; j < m; j++)
{
for(i = 0; i < m; i++)
{
vect[i] = 0.0;
}
jm = j*m;
vect[j] = 1.0;
lubksb(Ri, m , indx, vect);
for(i = 0; i < m ; i++)
{
Riinv[i + jm] = vect[i];
}
}
}
}
/* b ) --------- gRiinv = grad_Zi*Riinv (2 x m) -----------*/
for (t = 0 ; t < d2 ; t++)
{
for(l = 0 ; l < m ; l++)
{
lm = l*m;
temp = 0.0;
for(i = 0 ; i < m ; i++)
{
temp += grad_Zi[t + i*d2 ]*Riinv[i + lm];
}
gRiinv[t + l*d2] = temp;
}
}
/* c ) --------- gRiinvgt = gRiinv*grad_Zi^T (2 x 2) -----------*/
for (t = 0 ; t < d2 ; t++)
{
for(l = 0 ; l < d2 ; l++)
{
temp = 0.0;
for(i = 0 ; i < m ; i++)
{
i2 = i*d2;
temp += gRiinv[t + i2]*grad_Zi[l + i2];
}
gRiinvgt[t + l*d2] = temp;
}
}
}
/*-------------------------------------------------------------------------------------------------*/
void mvg(double *C, double *X, int d , int M)
{
// Generate M multivariate gaussian samples of (d x 1)
// X = C'*N(0,1), where C = chol(Sigma) is cholesky factorization (d x d), a upper triangular matrix;
int l , j , i, ld, jd;
register double temp;
for(l = 0 ; l < M ; l++)
{
ld = l*d;
for(j = 0 ; j < d ; j++)
{
jd = j*d;
temp = 0.0;
for(i = 0 ; i <= j ; i++)
{
temp += (C[i + jd]*randn());
}
X[j + ld] = temp;
}
}
}
/* ----------------------------------------------------------------------------------------------- */
void inv(double *R , double *invR , int *indx , double *vect , double *vv , int d)
{
int i , j , jd;
double dd;
if(ludcmp(R , d , indx, &dd , vv))
{
for(j = 0; j < d; j++)
{
for(i = 0; i < d; i++)
{
vect[i] = 0.0;
}
jd = j*d;
vect[j] = 1.0;
lubksb(R, d , indx, vect);
for(i = 0; i < d ; i++)
{
invR[i + jd] = vect[i];
}
}
}
}
/* ----------------------------------------------------------------------------------------------- */
void lubksb(double *m, int n, int *indx, double *b)
{
int i, ii = -1, ip, j , in;
double sum;
for(i = 0; i < n; i++)
{
ip = indx[i];
sum = b[ip];
b[ip] = b[i];
if(ii > -1)
{
for(j = ii; j <= i - 1; j++)
{
sum -= m[i + j*n] * b[j];
}
}
else if(sum)
{
ii = i;
}
b[i] = sum;
}
for(i = n - 1; i >= 0; i--)
{
sum = b[i];
in = i*n;
for(j = i + 1; j < n; j++)
{
sum -= m[i + j*n] * b[j];
}
b[i] = sum / m[i + in];
}
}
/* ----------------------------------------------------------------------------------------------- */
int ludcmp(double *m, int n, int *indx, double *d , double *vv)
{
int i, imax, j, k , jn , kn , n1 = n - 1;
double big, dum, sum, temp;
*d = 1.0;
for(i = 0; i < n; i++)
{
big = 0.0;
for(j = 0; j < n; j++)
{
if((temp = fabs(m[i + j*n])) > big)
{
big = temp;
}
}
if(big == 0.0)
{
return 0;
}
vv[i] = 1.0 / big;
}
for(j = 0; j < n; j++)
{
jn = j*n;
for(i = 0; i < j; i++)
{
sum = m[i + jn];
for(k = 0; k < i; k++)
{
sum -= m[i + k*n ] * m[k + jn];
}
m[i + jn] = sum;
}
big = 0.0;
for(i = j; i < n; i++)
{
sum = m[i + jn];
for(k = 0; k < j; k++)
{
sum -= m[i + k*n] * m[k + jn];
}
m[i + jn] = sum;
if((dum = vv[i] * fabs(sum)) >= big)
{
big = dum;
imax = i;
}
}
if(j != imax)
{
for(k = 0; k < n; k++)
{
kn = k*n;
dum = m[imax + kn];
m[imax + kn] = m[j + kn];
m[j + kn] = dum;
}
*d = -(*d);
vv[imax] = vv[j];
}
indx[j] = imax;
if(m[j + jn] == 0.0)
{
m[j + jn] = NUMERICS_FLOAT_MIN;
}
if(j != n1)
{
dum = 1.0 / (m[j + jn]);
for(i = j + 1; i < n; i++)
{
m[i + jn] *= dum;
}
}
}
return 1;
};
/* --------------------------------------------------------------------------- */
void randini(void)
{
/* SHR3 Seed initialization */
jsrseed = (UL) time( NULL );
jsr ^= jsrseed;
/* KISS Seed initialization */
z = (UL) time( NULL );
w = (UL) time( NULL );
jcong = (UL) time( NULL );
mix(z , w , jcong);
}
/* --------------------------------------------------------------------------- */
void randnini(void)
{
register const double m1 = 2147483648.0;
register double invm1;
register double dn = 3.442619855899 , tn = dn , vn = 9.91256303526217e-3 , q;
int i;
/* Ziggurat tables for randn */
invm1 = 1.0/m1;
q = vn/exp(-0.5*dn*dn);
kn[0] = (dn/q)*m1;
kn[1] = 0;
wn[0] = q*invm1;
wn[zigstep - 1 ] = dn*invm1;
fn[0] = 1.0;
fn[zigstep - 1] = exp(-0.5*dn*dn);
for(i = (zigstep - 2) ; i >= 1 ; i--)
{
dn = sqrt(-2.*log(vn/dn + exp(-0.5*dn*dn)));
kn[i+1] = (dn/tn)*m1;
tn = dn;
fn[i] = exp(-0.5*dn*dn);
wn[i] = dn*invm1;
}
}
/* --------------------------------------------------------------------------- */
double nfix(void)
{
const double r = 3.442620f; /* The starting of the right tail */
static double x, y;
for(;;)
{
x = hz*wn[iz];
if(iz == 0)
{ /* iz==0, handle the base strip */
do
{
x = -log(rand())*0.2904764; /* .2904764 is 1/r */
y = -log(rand());
}
while( (y + y) < (x*x));
return (hz > 0) ? (r + x) : (-r - x);
}
if( (fn[iz] + rand()*(fn[iz-1] - fn[iz])) < ( exp(-0.5*x*x) ) )
{
return x;
}
hz = randint;
iz = (hz & (zigstep - 1));
if(abs(hz) < kn[iz])
{
return (hz*wn[iz]);
}
}
}
/* --------------------------------------------------------------------------- */
double randn(void)
{
hz = randint;
iz = (hz & (zigstep - 1));
return (abs(hz) < kn[iz]) ? (hz*wn[iz]) : ( nfix() );
};
/* ----------------------------------------------------------------------------------------------- */
void transpose(double *A, double * B , int d)
{
int i , j , jd;
for (j = 0 ; j<d ; j++)
{
jd = j*d;
for(i = 0 ; i<d ; i++)
{
B[j + i*d] = A[i + jd];
}
}
}
/* ----------------------------------------------------------------------------------------------- */
void mult_trans(double *Qt, double *X , double *Y , int k , int n , int p)
{
// Y (k x p) = Qt*X, Qt(n x k), X(n x p)
int t , j , i , tn , tk, jn;
double temp;
for (t = 0 ; t < p ; t++)
{
tn = t*n;
tk = t*k;
for(j = 0 ; j < k ; j++)
{
jn = j*n;
temp = 0.0;
for(i = 0 ; i < n ; i++)
{
temp += Qt[i + jn]*X[i + tn];
}
Y[j + tk] = temp;
}
}
}
/* ----------------------------------------------------------------------------------------------- */
int chol(double *A , double *B , int n)
{
int i, i1 , j , k , jn , in , nn = n*n , n1 = n - 1 , kn;
double sum = 0 , p = 1 , inv_p;
for (i = 0 ; i<nn ; i++)
{
B[i] = A[i];
}
p = sqrt(B[0]);
inv_p = 1.0/p;
B[0] = p;
for(i = 1; i < n; i++)
{
B[n*i] *= inv_p;
}
for(i = 1; i < n; i++)
{
in = i*n;
i1 = i - 1;
sum = B[i + in]; //sum = B[i][i]
for(k = 0; k < i; ++k)
{
kn = in + k;
sum -= B[kn]*B[kn];
}
if(sum <= 0.0)
{
return 0;
}
p = sqrt(sum);
inv_p = 1.0/p;
for(j = n1; j > i ; --j)
{
jn = j*n;
sum = B[jn + i];
for(k = 0; k < i; ++k)
{
sum -= B[jn + k]*B[in + k];
}
B[jn + i] = sum*inv_p;
}
B[i + in] = p;
for(k = n1 ; k>i1 ; k--)
{
B[k + i1*n] = 0.0;
}
}
return 1;
}
/* ----------------------------------------------------------------------------------------------- */
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -