/* matinv() */
void utest4(void)
{
int i,j;
double C[4];
memcpy(C,B,sizeof(double)*4);
matinv(B,2);
for (i=0;i<2;i++) for (j=0;j<2;j++) assert(fabs(B[i+j*2]-invB[i+j*2])<1E-9);
matinv(B,2);
for (i=0;i<2;i++) for (j=0;j<2;j++) assert(fabs(B[i+j*2]-C[i+j*2])<1E-9);
printf("%s utest4 : OK\n",__FILE__);
}
extern void dops(int ns, const double *azel, double elmin, double *dop)
{
double H[4 * MAXSAT], Q[16], cosel, sinel;
int i, n;
for (i = 0; i < 4; i++) dop[i] = 0.0;
for (i = n = 0; i < ns&&i < MAXSAT; i++) {
if (azel[1 + i * 2] < elmin || azel[1 + i * 2] <= 0.0) continue;
cosel = cos(azel[1 + i * 2]);
sinel = sin(azel[1 + i * 2]);
H[4 * n] = cosel*sin(azel[i * 2]);
H[1 + 4 * n] = cosel*cos(azel[i * 2]);
H[2 + 4 * n] = sinel;
H[3 + 4 * n++] = 1.0;
}
if (n < 4) return;
matmul("NT", 4, 4, n, 1.0, H, H, 0.0, Q);
if (!matinv(Q, 4)) {
dop[0] = SQRT(Q[0] + Q[5] + Q[10] + Q[15]); /* GDOP */
dop[1] = SQRT(Q[0] + Q[5] + Q[10]); /* PDOP */
dop[2] = SQRT(Q[0] + Q[5]); /* HDOP */
dop[3] = SQRT(Q[10]); /* VDOP */
}
}
void SetupAAMatrix()
{
int i,j,k;
double mr;
double sum;
double U[SQNUM_AA], V[SQNUM_AA], T1[SQNUM_AA], T2[SQNUM_AA];
k=0;
for (i=0; i<NUM_AA-1; i++) {
for (j=i+1; j<NUM_AA; j++) {
Qij[i*NUM_AA+j] = Qij[j*NUM_AA+i] = aaRelativeRate[k++];
}
}
for (i=0; i<NUM_AA; i++) {
for (j=0; j<NUM_AA; j++) {
Qij[i*NUM_AA+j] *= aaFreq[j];
}
}
mr=0;
for (i=0; i<NUM_AA; i++) {
sum = 0;
Qij[i*NUM_AA+i]=0;
for (j=0; j<NUM_AA; j++) {
sum += Qij[i*NUM_AA+j];
}
Qij[i*NUM_AA+i] = -sum;
mr += aaFreq[i] * sum;
}
abyx(1.0/mr, Qij, SQNUM_AA);
if ((k=eigen(1, Qij, NUM_AA, Root, T1, U, V, T2))!=0) {
fprintf(stderr, "\ncomplex roots in SetupAAMatrix");
exit(EXIT_FAILURE);
}
xtoy (U, V, SQNUM_AA);
matinv (V, NUM_AA, NUM_AA, T1);
for (i=0; i<NUM_AA; i++) {
for (j=0; j<NUM_AA; j++) {
for (k=0; k<NUM_AA; k++) {
Cijk[i*SQNUM_AA+j*NUM_AA+k] = U[i*NUM_AA+k]*V[k*NUM_AA+j];
}
}
}
}
int solveCircuit(int nG, double **Gm, double **Gm_inv)
{
double sum;
int i, j;
if (Verbose)
fprintf(stderr, "Calculating circuit model");
/* set diagonal entries to sum of conductances but ignore nth node */
for (i = 0; i < nG; i++) {
sum = 0.0;
for (j = 0; j < nG; j++)
if (i != j)
sum += Gm[i][j];
Gm[i][i] = -sum;
}
return matinv(Gm, Gm_inv, nG - 1);
}
/*
多変量正規分布 N(Μ,Σ)の X に対する値を計算する。
単変量の正規分布は上の関数を使うこと。
*/
double
get_multi_norm(double *mean, double **covmat, double *X, int dim)
{
double *tmp1 = NULL, *tmp2 = NULL;
double det, **invmat = NULL;
double val;
tmp1 = del_vec(X, mean, dim);
tmp2 = tmp1;
invmat = matinv(dim, covmat, &det);
tmp1 = vecxmat(tmp1, dim, invmat, dim, dim);
val = dvid(safe_exp(-0.5 * innerprod(dim, tmp1, tmp2)),
(pow(2.0 * M_PI, dim / 2.0) * sqrt(det)));
free_double_matrix(invmat);
free_double_vector(tmp1);
free_double_vector(tmp2);
return val;
}
int main()
{
int i, j, k, n;
matrix a, asave, a_inv;
double s, t;
printf("n = "); scanf("%d", &n);
a = new_matrix(n, n); asave = new_matrix(n, n);
a_inv = new_matrix(n, n);
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
a[i][j] = asave[i][j] = rnd() - rnd();
printf("A\n");
matprint(a, n, 7, "%10.6f");
s = matinv(n, a, a_inv);
printf("行列式 = %g\n", s);
printf("A^{-1}\n");
matprint(a_inv, n, 7, "%10.6f");
t = 0;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++) {
s = (i == j);
for (k = 0; k < n; k++)
s -= asave[i][k] * a_inv[k][j];
t += s * s;
}
printf("A A^{-1} の成分の二乗平均誤差 %g\n",
sqrt(t / (n * n)));
t = 0;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++) {
s = (i == j);
for (k = 0; k < n; k++)
s -= a_inv[i][k] * asave[k][j];
t += s * s;
}
printf("A^{-1} A の成分の二乗平均誤差 %g\n",
sqrt(t / (n * n)));
return EXIT_SUCCESS;
}
END_TEST
START_TEST(test_matinv)
{
double c[3][3] = {{ 1, 2, 3}, { 0, 4, 5 }, { 1, 0, 6 }};
double expected[3][3] = {{1.090909, -0.545455, -0.090909},\
{0.227273, 0.136364, -0.227273}, {-0.181818, 0.090909, 0.181818}};
int i, j;
matinv ((double*) c, 3, 3);
for (i=0; i<3; i++){
for (j=0; j<3; j++){
ck_assert_msg(fabs(c[i][j] - expected[i][j]) < EPS, "wrong item \
[%d][%d] %f instead of %f", i,j,c[i][j],expected[i][j]);
}
}
}
int linset(struct linprm *lin)
{
static const char *function = "linset";
int i, j, naxis, status;
double *pc, *piximg;
struct wcserr **err;
if (lin == 0x0) return LINERR_NULL_POINTER;
err = &(lin->err);
naxis = lin->naxis;
/* Check for a unit matrix. */
lin->unity = 1;
pc = lin->pc;
for (i = 0; i < naxis; i++) {
for (j = 0; j < naxis; j++) {
if (j == i) {
if (*(pc++) != 1.0) {
lin->unity = 0;
break;
}
} else {
if (*(pc++) != 0.0) {
lin->unity = 0;
break;
}
}
}
}
if (lin->unity) {
if (lin->flag == LINSET) {
/* Free memory that may have been allocated previously. */
if (lin->piximg) free(lin->piximg);
if (lin->imgpix) free(lin->imgpix);
}
lin->piximg = 0x0;
lin->imgpix = 0x0;
lin->i_naxis = 0;
/* Check cdelt. */
for (i = 0; i < naxis; i++) {
if (lin->cdelt[i] == 0.0) {
return wcserr_set(LIN_ERRMSG(LINERR_SINGULAR_MTX));
}
}
} else {
if (lin->flag != LINSET || lin->i_naxis < naxis) {
if (lin->flag == LINSET) {
/* Free memory that may have been allocated previously. */
if (lin->piximg) free(lin->piximg);
if (lin->imgpix) free(lin->imgpix);
}
/* Allocate memory for internal arrays. */
if ((lin->piximg = calloc(naxis*naxis, sizeof(double))) == 0x0) {
return wcserr_set(LIN_ERRMSG(LINERR_MEMORY));
}
if ((lin->imgpix = calloc(naxis*naxis, sizeof(double))) == 0x0) {
free(lin->piximg);
return wcserr_set(LIN_ERRMSG(LINERR_MEMORY));
}
lin->i_naxis = naxis;
}
/* Compute the pixel-to-image transformation matrix. */
pc = lin->pc;
piximg = lin->piximg;
for (i = 0; i < naxis; i++) {
for (j = 0; j < naxis; j++) {
if (lin->disseq == 0x0) {
/* No sequent distortions, incorporate cdelt into piximg. */
*(piximg++) = lin->cdelt[i] * (*(pc++));
} else {
*(piximg++) = *(pc++);
}
}
}
/* Compute the image-to-pixel transformation matrix. */
if ((status = matinv(naxis, lin->piximg, lin->imgpix))) {
return wcserr_set(LIN_ERRMSG(status));
}
}
/* Set up the distortion functions. */
lin->affine = 1;
if (lin->dispre) {
if ((status = disset(lin->dispre))) {
return wcserr_set(LIN_ERRMSG(lin_diserr[status]));
}
lin->affine = 0;
}
if (lin->disseq) {
if ((status = disset(lin->disseq))) {
return wcserr_set(LIN_ERRMSG(lin_diserr[status]));
}
lin->affine = 0;
}
lin->simple = lin->unity && lin->affine;
/* Create work arrays. */
if (lin->tmpcrd) free(lin->tmpcrd);
if ((lin->tmpcrd = calloc(naxis, sizeof(double))) == 0x0) {
linfree(lin);
return wcserr_set(LIN_ERRMSG(LINERR_MEMORY));
}
lin->flag = LINSET;
return 0;
}
void create_RC_matrices(flp_t *flp, int omit_lateral)
{
int i, j, k = 0, n = flp->n_units;
int **border;
double **len, *gx, *gy, **g, *c_ver, **t, *gx_sp, *gy_sp;
double r_sp1, r_sp2, r_hs; /* lateral resistances to spreader and heatsink */
/* NOTE: *_mid - the vertical R/C from CENTER nodes of spreader
* and heatsink. *_per - the vertical R/C from PERIPHERAL (n,s,e,w) nodes
*/
double r_sp_per, r_hs_mid, r_hs_per, c_sp_per, c_hs_mid, c_hs_per;
double gn_sp=0, gs_sp=0, ge_sp=0, gw_sp=0;
double w_chip = get_total_width (flp); /* x-axis */
double l_chip = get_total_height (flp); /* y-axis */
border = imatrix(n, 4);
len = matrix(n, n); /* len[i][j] = length of shared edge bet. i & j */
gx = vector(n); /* lumped conductances in x direction */
gy = vector(n); /* lumped conductances in y direction */
gx_sp = vector(n); /* lateral conductances in the spreader layer */
gy_sp = vector(n);
g = matrix(NL*n+EXTRA, NL*n+EXTRA); /* g[i][j] = conductance bet. nodes i & j */
c_ver = vector(NL*n+EXTRA); /* vertical capacitance */
b = matrix(NL*n+EXTRA, NL*n+EXTRA); /* B, C, INVA and INVB are (NL*n+EXTRA)x(NL*n+EXTRA) matrices */
c = matrix(NL*n+EXTRA, NL*n+EXTRA);
inva = matrix(NL*n+EXTRA, NL*n+EXTRA);
invb = matrix(NL*n+EXTRA, NL*n+EXTRA);
t = matrix (NL*n+EXTRA, NL*n+EXTRA); /* copy of B */
/* compute the silicon fitting factor - see pg 10 of the UVA CS tech report - CS-TR-2003-08 */
factor_chip = C_FACTOR * ((SPEC_HEAT_INT / SPEC_HEAT_SI) * (w_chip + 0.88 * t_interface) \
* (l_chip + 0.88 * t_interface) * t_interface / ( w_chip * l_chip * t_chip) + 1);
/* fitting factor for interface - same rationale as above */
factor_int = C_FACTOR * ((SPEC_HEAT_CU / SPEC_HEAT_INT) * (w_chip + 0.88 * t_spreader) \
* (l_chip + 0.88 * t_spreader) * t_spreader / ( w_chip * l_chip * t_interface) + 1);
/*printf("fitting factors : %lf, %lf\n", factor_chip, factor_int); */
/* gx's and gy's of blocks */
for (i = 0; i < n; i++) {
/* at the silicon layer */
if (omit_lateral) {
gx[i] = gy[i] = 0;
}
else {
gx[i] = 1.0/getr(K_SI, flp->units[i].height, flp->units[i].width, l_chip, t_chip);
gy[i] = 1.0/getr(K_SI, flp->units[i].width, flp->units[i].height, w_chip, t_chip);
}
/* at the spreader layer */
gx_sp[i] = 1.0/getr(K_CU, flp->units[i].height, flp->units[i].width, l_chip, t_spreader);
gy_sp[i] = 1.0/getr(K_CU, flp->units[i].width, flp->units[i].height, w_chip, t_spreader);
}
/* shared lengths between blocks */
for (i = 0; i < n; i++)
for (j = i; j < n; j++)
len[i][j] = len[j][i] = get_shared_len(flp, i, j);
/* lateral R's of spreader and sink */
r_sp1 = getr(K_CU, (s_spreader+3*w_chip)/4.0, (s_spreader-w_chip)/4.0, w_chip, t_spreader);
r_sp2 = getr(K_CU, (3*s_spreader+w_chip)/4.0, (s_spreader-w_chip)/4.0, (s_spreader+3*w_chip)/4.0, t_spreader);
r_hs = getr(K_CU, (s_sink+3*s_spreader)/4.0, (s_sink-s_spreader)/4.0, s_spreader, t_sink);
/* vertical R's and C's of spreader and sink */
r_sp_per = RHO_CU * t_spreader * 4.0 / (s_spreader * s_spreader - w_chip*l_chip);
c_sp_per = factor_pack * SPEC_HEAT_CU * t_spreader * (s_spreader * s_spreader - w_chip*l_chip) / 4.0;
r_hs_mid = RHO_CU * t_sink / (s_spreader*s_spreader);
c_hs_mid = factor_pack * SPEC_HEAT_CU * t_sink * (s_spreader * s_spreader);
r_hs_per = RHO_CU * t_sink * 4.0 / (s_sink * s_sink - s_spreader*s_spreader);
c_hs_per = factor_pack * SPEC_HEAT_CU * t_sink * (s_sink * s_sink - s_spreader*s_spreader) / 4.0;
/* short the R's from block centers to a particular chip edge */
for (i = 0; i < n; i++) {
if (eq(flp->units[i].bottomy + flp->units[i].height, l_chip)) {
gn_sp += gy_sp[i];
border[i][2] = 1; /* block is on northern border */
}
if (eq(flp->units[i].bottomy, 0)) {
gs_sp += gy_sp[i];
border[i][3] = 1; /* block is on southern border */
}
if (eq(flp->units[i].leftx + flp->units[i].width, w_chip)) {
ge_sp += gx_sp[i];
border[i][1] = 1; /* block is on eastern border */
}
if (eq(flp->units[i].leftx, 0)) {
gw_sp += gx_sp[i];
border[i][0] = 1; /* block is on western border */
}
}
/* overall R and C between nodes */
for (i = 0; i < n; i++) {
double area = (flp->units[i].height * flp->units[i].width);
/*
* amongst functional units in the various layers
* resistances in the interface layer are assumed
* to be infinite
*/
for (j = 0; j < n; j++) {
double part = 0, part_sp = 0;
if (is_horiz_adj(flp, i, j)) {
part = gx[i] / flp->units[i].height;
part_sp = gx_sp[i] / flp->units[i].height;
}
else if (is_vert_adj(flp, i,j)) {
part = gy[i] / flp->units[i].width;
part_sp = gy_sp[i] / flp->units[i].width;
}
g[i][j] = part * len[i][j];
g[HSP*n+i][HSP*n+j] = part_sp * len[i][j];
}
/* vertical g's in the silicon layer */
g[i][IFACE*n+i]=g[IFACE*n+i][i]=2.0/(RHO_SI * t_chip / area);
/* vertical g's in the interface layer */
g[IFACE*n+i][HSP*n+i]=g[HSP*n+i][IFACE*n+i]=2.0/(RHO_INT * t_interface / area);
/* vertical g's in the spreader layer */
g[HSP*n+i][NL*n+SP_B]=g[NL*n+SP_B][HSP*n+i]=2.0/(RHO_CU * t_spreader / area);
/* C's from functional units to ground */
c_ver[i] = factor_chip * SPEC_HEAT_SI * t_chip * area;
/* C's from interface portion of the functional units to ground */
c_ver[IFACE*n+i] = factor_int * SPEC_HEAT_INT * t_interface * area;
/* C's from spreader portion of the functional units to ground */
c_ver[HSP*n+i] = factor_pack * SPEC_HEAT_CU * t_spreader * area;
/* lateral g's from block center (spreader layer) to peripheral (n,s,e,w) spreader nodes */
g[HSP*n+i][NL*n+SP_N]=g[NL*n+SP_N][HSP*n+i]=2.0*border[i][2]/((1.0/gy_sp[i])+r_sp1*gn_sp/gy_sp[i]);
g[HSP*n+i][NL*n+SP_S]=g[NL*n+SP_S][HSP*n+i]=2.0*border[i][3]/((1.0/gy_sp[i])+r_sp1*gs_sp/gy_sp[i]);
g[HSP*n+i][NL*n+SP_E]=g[NL*n+SP_E][HSP*n+i]=2.0*border[i][1]/((1.0/gx_sp[i])+r_sp1*ge_sp/gx_sp[i]);
g[HSP*n+i][NL*n+SP_W]=g[NL*n+SP_W][HSP*n+i]=2.0*border[i][0]/((1.0/gx_sp[i])+r_sp1*gw_sp/gx_sp[i]);
}
/* max slope (max_power * max_vertical_R / vertical RC time constant) for silicon */
max_slope = MAX_PD / (factor_chip * t_chip * SPEC_HEAT_SI);
/* vertical g's and C's between central nodes */
/* between spreader bottom and sink bottom */
g[NL*n+SINK_B][NL*n+SP_B]=g[NL*n+SP_B][NL*n+SINK_B]=2.0/r_hs_mid;
/* from spreader bottom to ground */
c_ver[NL*n+SP_B]=c_hs_mid;
/* from sink bottom to ground */
c_ver[NL*n+SINK_B] = factor_pack * c_convec;
/* g's and C's from peripheral(n,s,e,w) nodes */
for (i = 1; i <= 4; i++) {
/* vertical g's between peripheral spreader nodes and spreader bottom */
g[NL*n+SP_B-i][NL*n+SP_B]=g[NL*n+SP_B][NL*n+SP_B-i]=2.0/r_sp_per;
/* lateral g's between peripheral spreader nodes and peripheral sink nodes */
g[NL*n+SP_B-i][NL*n+SINK_B-i]=g[NL*n+SINK_B-i][NL*n+SP_B-i]=2.0/(r_hs + r_sp2);
/* vertical g's between peripheral sink nodes and sink bottom */
g[NL*n+SINK_B-i][NL*n+SINK_B]=g[NL*n+SINK_B][NL*n+SINK_B-i]=2.0/r_hs_per;
/* from peripheral spreader nodes to ground */
c_ver[NL*n+SP_B-i]=c_sp_per;
/* from peripheral sink nodes to ground */
c_ver[NL*n+SINK_B-i]=c_hs_per;
}
/* calculate matrices A, B such that A(dT) + BT = POWER */
for (i = 0; i < NL*n+EXTRA; i++) {
for (j = 0; j < NL*n+EXTRA; j++) {
if (i==j) {
inva[i][j] = 1.0/c_ver[i];
if (i == NL*n+SINK_B) /* sink bottom */
b[i][j] += 1.0 / r_convec;
for (k = 0; k < NL*n+EXTRA; k++) {
if ((g[i][k]==0.0)||(g[k][i])==0.0)
continue;
else
/* here is why the 2.0 factor comes when calculating g[][] */
b[i][j] += 1.0/((1.0/g[i][k])+(1.0/g[k][i]));
}
} else {
inva[i][j]=0.0;
if ((g[i][j]==0.0)||(g[j][i])==0.0)
b[i][j]=0.0;
else
b[i][j]=-1.0/((1.0/g[i][j])+(1.0/g[j][i]));
}
}
}
/* we are always going to use the eqn dT + A^-1 * B T = A^-1 * POWER. so, store C = A^-1 * B */
matmult(c, inva, b, NL*n+EXTRA);
/* we will also be needing INVB so store it too */
copy_matrix(t, b, NL*n+EXTRA, NL*n+EXTRA);
matinv(invb, t, NL*n+EXTRA);
/* dump_vector(c_ver, NL*n+EXTRA); */
/* dump_matrix(g, NL*n+EXTRA, NL*n+EXTRA); */
/* dump_matrix(c, NL*n+EXTRA, NL*n+EXTRA); */
/* cleanup */
free_matrix(t, NL*n+EXTRA);
free_matrix(g, NL*n+EXTRA);
free_matrix(len, n);
free_imatrix(border, n);
free_vector(c_ver);
free_vector(gx);
free_vector(gy);
free_vector(gx_sp);
free_vector(gy_sp);
}
/*****************************************************************************
*
* Routine for constructing graph domain_dof with minimal overlap
* and computing the respective matrix inverses to be
* used in an overlapping Schwarz procedure (like smoother
* in AMG);
*
*****************************************************************************/
HYPRE_Int
hypre_AMGCreateDomainDof(hypre_CSRMatrix *A,
HYPRE_Int **i_domain_dof_pointer,
HYPRE_Int **j_domain_dof_pointer,
HYPRE_Real **domain_matrixinverse_pointer,
HYPRE_Int *num_domains_pointer)
{
HYPRE_Int *i_domain_dof, *j_domain_dof;
HYPRE_Real *domain_matrixinverse;
HYPRE_Int num_domains;
HYPRE_Int *i_dof_dof = hypre_CSRMatrixI(A);
HYPRE_Int *j_dof_dof = hypre_CSRMatrixJ(A);
HYPRE_Real *a_dof_dof = hypre_CSRMatrixData(A);
HYPRE_Int num_dofs = hypre_CSRMatrixNumRows(A);
/* HYPRE_Int *i_dof_to_accept_weight; */
HYPRE_Int *i_dof_to_prefer_weight,
*w_dof_dof, *i_dof_weight;
HYPRE_Int *i_dof_to_aggregate, *i_aggregate_dof, *j_aggregate_dof;
HYPRE_Int *i_dof_index;
HYPRE_Int ierr = 0;
HYPRE_Int i,j,k, l_loc, i_loc, j_loc;
HYPRE_Int i_dof;
HYPRE_Int *i_local_to_global;
HYPRE_Int *i_global_to_local;
HYPRE_Int local_dof_counter, max_local_dof_counter=0;
HYPRE_Int domain_dof_counter = 0, domain_matrixinverse_counter = 0;
HYPRE_Real *AE, *XE;
/* PCG arrays: --------------------------------------------------- */
/* HYPRE_Real *x, *rhs, *v, *w, *d, *aux;
HYPRE_Int max_iter; */
/* --------------------------------------------------------------------- */
/*=======================================================================*/
/* create artificial domains by agglomeration; */
/*=======================================================================*/
hypre_printf("----------- create artificials domain by agglomeration; ======\n");
i_dof_to_prefer_weight = (HYPRE_Int *) malloc(num_dofs * sizeof(HYPRE_Int));
w_dof_dof = (HYPRE_Int *) malloc(i_dof_dof[num_dofs] * sizeof(HYPRE_Int));
for (i=0; i < num_dofs; i++)
i_dof_to_prefer_weight[i] = 0;
for (i=0; i<num_dofs; i++)
for (j=i_dof_dof[i]; j< i_dof_dof[i+1]; j++)
{
if (j_dof_dof[j] == i)
w_dof_dof[j]=0;
else
w_dof_dof[j]=1;
}
hypre_printf("end computing weights for agglomeration procedure: --------\n");
i_dof_weight = (HYPRE_Int *) malloc(num_dofs * sizeof(HYPRE_Int));
i_aggregate_dof = (HYPRE_Int *) malloc(num_dofs * sizeof(HYPRE_Int));
j_aggregate_dof= (HYPRE_Int *) malloc(num_dofs * sizeof(HYPRE_Int));
ierr = hypre_AMGeAgglomerate(i_aggregate_dof, j_aggregate_dof,
i_dof_dof, j_dof_dof, w_dof_dof,
i_dof_dof, j_dof_dof,
i_dof_dof, j_dof_dof,
i_dof_to_prefer_weight,
i_dof_weight,
num_dofs, num_dofs,
&num_domains);
hypre_printf("num_dofs: %d, num_domains: %d\n", num_dofs, num_domains);
i_dof_to_aggregate = (HYPRE_Int *) malloc(num_dofs * sizeof(HYPRE_Int));
for (i=0; i < num_domains; i++)
for (j=i_aggregate_dof[i]; j < i_aggregate_dof[i+1]; j++)
i_dof_to_aggregate[j_aggregate_dof[j]] = i;
/*
hypre_printf("========================================================\n");
hypre_printf("== artificial non--overlapping domains (aggregates): ===\n");
hypre_printf("========================================================\n");
for (i=0; i < num_domains; i++)
{
hypre_printf("\n aggregate %d:\n", i);
for (j=i_aggregate_dof[i]; j < i_aggregate_dof[i+1]; j++)
hypre_printf("%d, ", j_aggregate_dof[j]);
hypre_printf("\n");
}
*/
free(i_dof_to_prefer_weight);
free(i_dof_weight);
free(w_dof_dof);
/* make domains from aggregates: *********************************/
i_domain_dof = (HYPRE_Int *) malloc((num_domains+1) * sizeof(HYPRE_Int));
i_dof_index = (HYPRE_Int *) malloc(num_dofs * sizeof(HYPRE_Int));
for (i=0; i < num_dofs; i++)
i_dof_index[i] = -1;
domain_dof_counter=0;
for (i=0; i < num_domains; i++)
{
i_domain_dof[i] = domain_dof_counter;
for (j=i_aggregate_dof[i]; j < i_aggregate_dof[i+1]; j++)
for (k=i_dof_dof[j_aggregate_dof[j]];
k<i_dof_dof[j_aggregate_dof[j]+1]; k++)
if (i_dof_to_aggregate[j_dof_dof[k]] >= i
&& i_dof_index[j_dof_dof[k]]==-1)
{
i_dof_index[j_dof_dof[k]]++;
domain_dof_counter++;
}
for (j=i_aggregate_dof[i]; j < i_aggregate_dof[i+1]; j++)
for (k=i_dof_dof[j_aggregate_dof[j]];
k<i_dof_dof[j_aggregate_dof[j]+1]; k++)
i_dof_index[j_dof_dof[k]]=-1;
}
i_domain_dof[num_domains] = domain_dof_counter;
j_domain_dof = (HYPRE_Int *) malloc(domain_dof_counter * sizeof(HYPRE_Int));
domain_dof_counter=0;
for (i=0; i < num_domains; i++)
{
for (j=i_aggregate_dof[i]; j < i_aggregate_dof[i+1]; j++)
for (k=i_dof_dof[j_aggregate_dof[j]];
k<i_dof_dof[j_aggregate_dof[j]+1]; k++)
if (i_dof_to_aggregate[j_dof_dof[k]] >= i
&& i_dof_index[j_dof_dof[k]]==-1)
{
i_dof_index[j_dof_dof[k]]++;
j_domain_dof[domain_dof_counter] = j_dof_dof[k];
domain_dof_counter++;
}
for (j=i_aggregate_dof[i]; j < i_aggregate_dof[i+1]; j++)
for (k=i_dof_dof[j_aggregate_dof[j]];
k<i_dof_dof[j_aggregate_dof[j]+1]; k++)
i_dof_index[j_dof_dof[k]]=-1;
}
free(i_aggregate_dof);
free(j_aggregate_dof);
free(i_dof_to_aggregate);
/*
i_domain_dof = i_aggregate_dof;
j_domain_dof = j_aggregate_dof;
*/
hypre_printf("END domain_dof computations: =================================\n");
domain_matrixinverse_counter = 0;
local_dof_counter = 0;
for (i=0; i < num_domains; i++)
{
local_dof_counter = i_domain_dof[i+1]-i_domain_dof[i];
domain_matrixinverse_counter+= local_dof_counter * local_dof_counter;
if (local_dof_counter > max_local_dof_counter)
max_local_dof_counter = local_dof_counter;
}
domain_matrixinverse = hypre_CTAlloc(HYPRE_Real, domain_matrixinverse_counter);
i_local_to_global = hypre_CTAlloc(HYPRE_Int, max_local_dof_counter);
AE = hypre_CTAlloc(HYPRE_Real, max_local_dof_counter *
max_local_dof_counter);
XE = hypre_CTAlloc(HYPRE_Real, max_local_dof_counter *
max_local_dof_counter);
/* i_dof_index = (HYPRE_Int *) malloc(num_dofs * sizeof(HYPRE_Int)); */
i_global_to_local = i_dof_index;
for (i=0; i < num_dofs; i++)
i_global_to_local[i] = -1;
domain_matrixinverse_counter = 0;
for (i=0; i < num_domains; i++)
{
local_dof_counter = 0;
for (j=i_domain_dof[i]; j < i_domain_dof[i+1]; j++)
{
i_global_to_local[j_domain_dof[j]] = local_dof_counter;
i_local_to_global[local_dof_counter] = j_domain_dof[j];
local_dof_counter++;
}
/* get local matrix in AE: ======================================== */
for (i_loc=0; i_loc < local_dof_counter; i_loc++)
for (j_loc=0; j_loc < local_dof_counter; j_loc++)
AE[i_loc + j_loc * local_dof_counter] = 0.e0;
for (i_loc=0; i_loc < local_dof_counter; i_loc++)
{
i_dof = i_local_to_global[i_loc];
for (j=i_dof_dof[i_dof]; j < i_dof_dof[i_dof+1]; j++)
{
j_loc = i_global_to_local[j_dof_dof[j]];
if (j_loc >=0)
AE[i_loc + j_loc * local_dof_counter] = a_dof_dof[j];
}
}
/* get block for Schwarz smoother: ============================= */
ierr = matinv(XE, AE, local_dof_counter);
/* hypre_printf("ierr_AE_inv: %d\n", ierr); */
for (i_loc=0; i_loc < local_dof_counter; i_loc++)
for (j_loc=0; j_loc < local_dof_counter; j_loc++)
domain_matrixinverse[domain_matrixinverse_counter
+ i_loc + j_loc * local_dof_counter]
= XE[i_loc + j_loc * local_dof_counter];
domain_matrixinverse_counter+=local_dof_counter*local_dof_counter;
for (l_loc=0; l_loc < local_dof_counter; l_loc++)
i_global_to_local[i_local_to_global[l_loc]] = -1;
}
hypre_TFree(i_local_to_global);
hypre_TFree(AE);
hypre_TFree(XE);
hypre_TFree(i_dof_index);
*i_domain_dof_pointer = i_domain_dof;
*j_domain_dof_pointer = j_domain_dof;
*num_domains_pointer = num_domains;
*domain_matrixinverse_pointer = domain_matrixinverse;
/*
x = hypre_CTAlloc(HYPRE_Real, num_dofs);
rhs = hypre_CTAlloc(HYPRE_Real, num_dofs);
v = hypre_CTAlloc(HYPRE_Real, num_dofs);
w = hypre_CTAlloc(HYPRE_Real, num_dofs);
d = hypre_CTAlloc(HYPRE_Real, num_dofs);
aux = hypre_CTAlloc(HYPRE_Real, num_dofs);
for (i=0; i < num_dofs; i++)
x[i] = 0.e0;
for (i=0; i < num_dofs; i++)
rhs[i] = rand();
max_iter = 1000;
hypre_printf("\nenter SchwarzPCG: =======================================\n");
ierr = hypre_Schwarzpcg(x, rhs,
a_dof_dof,
i_dof_dof, j_dof_dof,
i_domain_dof, j_domain_dof,
domain_matrixinverse,
num_domains,
v, w, d, aux,
max_iter,
num_dofs);
hypre_printf("\n\n=======================================================\n");
hypre_printf(" END test PCG solve: \n");
hypre_printf("===========================================================\n");
hypre_TFree(x);
hypre_TFree(rhs);
hypre_TFree(aux);
hypre_TFree(v);
hypre_TFree(w);
hypre_TFree(d);
hypre_TFree(i_domain_dof);
hypre_TFree(j_domain_dof);
hypre_TFree(domain_matrixinverse);
*/
return ierr;
}
HYPRE_Int
hypre_AMGNodalSchwarzSmoother( hypre_CSRMatrix *A,
HYPRE_Int *dof_func,
HYPRE_Int num_functions,
HYPRE_Int option,
HYPRE_Int **i_domain_dof_pointer,
HYPRE_Int **j_domain_dof_pointer,
HYPRE_Real **domain_matrixinverse_pointer,
HYPRE_Int *num_domains_pointer)
{
/* option = 0: nodal symGS;
1: next to nodal symGS (overlapping Schwarz) */
HYPRE_Int *i_domain_dof, *j_domain_dof;
HYPRE_Real *domain_matrixinverse;
HYPRE_Int num_domains;
HYPRE_Int *i_dof_node, *j_dof_node;
HYPRE_Int *i_node_dof, *j_node_dof;
HYPRE_Int *i_node_dof_dof, *j_node_dof_dof;
HYPRE_Int *i_node_node, *j_node_node;
HYPRE_Int num_nodes;
HYPRE_Int *i_dof_dof = hypre_CSRMatrixI(A);
HYPRE_Int *j_dof_dof = hypre_CSRMatrixJ(A);
HYPRE_Real *a_dof_dof = hypre_CSRMatrixData(A);
HYPRE_Int num_dofs = hypre_CSRMatrixNumRows(A);
HYPRE_Int ierr = 0;
HYPRE_Int i,j,k, l_loc, i_loc, j_loc;
HYPRE_Int i_dof, j_dof;
HYPRE_Int *i_local_to_global;
HYPRE_Int *i_global_to_local;
HYPRE_Int *i_int;
HYPRE_Int *i_int_to_local;
HYPRE_Int int_dof_counter, local_dof_counter, max_local_dof_counter=0;
HYPRE_Int domain_dof_counter = 0, domain_matrixinverse_counter = 0;
HYPRE_Real *AE, *XE;
/* PCG arrays: ---------------------------------------------------
HYPRE_Real *x, *rhs, *v, *w, *d, *aux;
HYPRE_Int max_iter;
------------------------------------------------------------------ */
/* build dof_node graph: ----------------------------------------- */
num_nodes = num_dofs / num_functions;
hypre_printf("\nnum_nodes: %d, num_dofs: %d = %d x %d\n", num_nodes, num_dofs,
num_nodes, num_functions);
i_dof_node = hypre_CTAlloc(HYPRE_Int, num_dofs+1);
j_dof_node = hypre_CTAlloc(HYPRE_Int, num_dofs);
for (i=0; i < num_dofs+1; i++)
i_dof_node[i] = i;
for (j = 0; j < num_nodes; j++)
for (k = 0; k < num_functions; k++)
j_dof_node[j*num_functions+k] = j;
/* build node_dof graph: ----------------------------------------- */
ierr = transpose_matrix_create(&i_node_dof, &j_node_dof,
i_dof_node, j_dof_node,
num_dofs, num_nodes);
/* build node_node graph: ----------------------------------------- */
ierr = matrix_matrix_product(&i_node_dof_dof,
&j_node_dof_dof,
i_node_dof, j_node_dof,
i_dof_dof, j_dof_dof,
num_nodes, num_dofs, num_dofs);
ierr = matrix_matrix_product(&i_node_node,
&j_node_node,
i_node_dof_dof,
j_node_dof_dof,
i_dof_node, j_dof_node,
num_nodes, num_dofs, num_nodes);
hypre_TFree(i_node_dof_dof);
hypre_TFree(j_node_dof_dof);
/* compute for each node the local information: -------------------- */
i_global_to_local = i_dof_node;
for (i_dof =0; i_dof < num_dofs; i_dof++)
i_global_to_local[i_dof] = -1;
domain_matrixinverse_counter = 0;
domain_dof_counter = 0;
for (i=0; i < num_nodes; i++)
{
local_dof_counter = 0;
for (j=i_node_node[i]; j < i_node_node[i+1]; j++)
for (k=i_node_dof[j_node_node[j]];
k<i_node_dof[j_node_node[j]+1]; k++)
{
j_dof = j_node_dof[k];
if (i_global_to_local[j_dof] < 0)
{
i_global_to_local[j_dof] = local_dof_counter;
local_dof_counter++;
}
}
domain_matrixinverse_counter += local_dof_counter*local_dof_counter;
domain_dof_counter += local_dof_counter;
if (local_dof_counter > max_local_dof_counter)
max_local_dof_counter = local_dof_counter;
for (j=i_node_node[i]; j < i_node_node[i+1]; j++)
for (k=i_node_dof[j_node_node[j]];
k<i_node_dof[j_node_node[j]+1]; k++)
{
j_dof = j_node_dof[k];
i_global_to_local[j_dof] = -1;
}
}
num_domains = num_nodes;
i_domain_dof = hypre_CTAlloc(HYPRE_Int, num_domains+1);
if (option == 1)
j_domain_dof = hypre_CTAlloc(HYPRE_Int, domain_dof_counter);
else
j_domain_dof = hypre_CTAlloc(HYPRE_Int, num_dofs);
if (option == 1)
domain_matrixinverse = hypre_CTAlloc(HYPRE_Real, domain_matrixinverse_counter);
else
domain_matrixinverse = hypre_CTAlloc(HYPRE_Real, num_dofs * num_functions);
i_local_to_global = hypre_CTAlloc(HYPRE_Int, max_local_dof_counter);
AE = hypre_CTAlloc(HYPRE_Real, max_local_dof_counter *
max_local_dof_counter);
XE = hypre_CTAlloc(HYPRE_Real, max_local_dof_counter *
max_local_dof_counter);
i_int_to_local = hypre_CTAlloc(HYPRE_Int, max_local_dof_counter);
i_int = hypre_CTAlloc(HYPRE_Int, max_local_dof_counter);
for (l_loc=0; l_loc < max_local_dof_counter; l_loc++)
i_int[l_loc] = -1;
domain_dof_counter = 0;
domain_matrixinverse_counter = 0;
for (i=0; i < num_nodes; i++)
{
i_domain_dof[i] = domain_dof_counter;
local_dof_counter = 0;
for (j=i_node_node[i]; j < i_node_node[i+1]; j++)
for (k=i_node_dof[j_node_node[j]];
k<i_node_dof[j_node_node[j]+1]; k++)
{
j_dof = j_node_dof[k];
if (i_global_to_local[j_dof] < 0)
{
i_global_to_local[j_dof] = local_dof_counter;
i_local_to_global[local_dof_counter] = j_dof;
local_dof_counter++;
}
}
for (j=i_node_dof[i]; j < i_node_dof[i+1]; j++)
for (k=i_dof_dof[j_node_dof[j]]; k < i_dof_dof[j_node_dof[j]+1]; k++)
if (i_global_to_local[j_dof_dof[k]] < 0)
hypre_printf("WRONG local indexing: ====================== \n");
int_dof_counter = 0;
for (k=i_node_dof[i]; k < i_node_dof[i+1]; k++)
{
i_dof = j_node_dof[k];
i_loc = i_global_to_local[i_dof];
i_int[i_loc] = int_dof_counter;
i_int_to_local[int_dof_counter] = i_loc;
int_dof_counter++;
}
/* get local matrix AE: ======================================== */
if (option == 1)
{
for (i_loc=0; i_loc < local_dof_counter; i_loc++)
for (j_loc=0; j_loc < local_dof_counter; j_loc++)
AE[i_loc + j_loc * local_dof_counter] = 0.e0;
for (i_loc=0; i_loc < local_dof_counter; i_loc++)
{
i_dof = i_local_to_global[i_loc];
for (j=i_dof_dof[i_dof]; j < i_dof_dof[i_dof+1]; j++)
{
j_loc = i_global_to_local[j_dof_dof[j]];
if (j_loc >=0)
AE[i_loc + j_loc * local_dof_counter] = a_dof_dof[j];
}
}
/* get block for Schwarz smoother: ============================= */
ierr = matinv(XE, AE, local_dof_counter);
/* hypre_printf("ierr_AE_inv: %d\n", ierr); */
}
if (option == 1)
for (i_loc=0; i_loc < local_dof_counter; i_loc++)
j_domain_dof[domain_dof_counter+i_loc]
= i_local_to_global[i_loc];
if (option == 1)
for (i_loc=0; i_loc < local_dof_counter; i_loc++)
for (j_loc=0; j_loc < local_dof_counter; j_loc++)
domain_matrixinverse[domain_matrixinverse_counter
+ i_loc + j_loc * local_dof_counter]
= XE[i_loc + j_loc * local_dof_counter];
if (option == 0)
{
for (i_loc=0; i_loc < int_dof_counter; i_loc++)
for (j_loc=0; j_loc < int_dof_counter; j_loc++)
AE[i_loc + j_loc * int_dof_counter] = 0.e0;
for (l_loc=0; l_loc < int_dof_counter; l_loc++)
{
i_loc = i_int_to_local[l_loc];
i_dof = i_local_to_global[i_loc];
for (j=i_dof_dof[i_dof]; j < i_dof_dof[i_dof+1]; j++)
{
j_loc = i_global_to_local[j_dof_dof[j]];
if (j_loc >=0)
if (i_int[j_loc] >=0)
AE[i_loc + i_int[j_loc] * int_dof_counter]
= a_dof_dof[j];
}
}
ierr = matinv(XE, AE, int_dof_counter);
for (i_loc=0; i_loc < int_dof_counter; i_loc++)
{
j_domain_dof[domain_dof_counter + i_loc] =
i_local_to_global[i_int_to_local[i_loc]];
for (j_loc=0; j_loc < int_dof_counter; j_loc++)
domain_matrixinverse[domain_matrixinverse_counter
+ i_loc + j_loc * int_dof_counter]
= XE[i_loc + j_loc * int_dof_counter];
}
domain_dof_counter+=int_dof_counter;
domain_matrixinverse_counter+=int_dof_counter*int_dof_counter;
}
else
{
domain_dof_counter+=local_dof_counter;
domain_matrixinverse_counter+=local_dof_counter*local_dof_counter;
}
for (l_loc=0; l_loc < local_dof_counter; l_loc++)
{
i_int[l_loc] = -1;
i_global_to_local[i_local_to_global[l_loc]] = -1;
}
}
i_domain_dof[num_nodes] = domain_dof_counter;
hypre_TFree(i_dof_node);
hypre_TFree(j_dof_node);
hypre_TFree(i_node_dof);
hypre_TFree(j_node_dof);
hypre_TFree(i_node_node);
hypre_TFree(j_node_node);
hypre_TFree(i_int);
hypre_TFree(i_int_to_local);
hypre_TFree(i_local_to_global);
hypre_TFree(AE);
hypre_TFree(XE);
*i_domain_dof_pointer = i_domain_dof;
*j_domain_dof_pointer = j_domain_dof;
*num_domains_pointer = num_domains;
*domain_matrixinverse_pointer = domain_matrixinverse;
/* hypre_printf("exit *Schwarz*: ===============================\n\n"); */
/* -----------------------------------------------------------------
x = hypre_CTAlloc(HYPRE_Real, num_dofs);
rhs = hypre_CTAlloc(HYPRE_Real, num_dofs);
v = hypre_CTAlloc(HYPRE_Real, num_dofs);
w = hypre_CTAlloc(HYPRE_Real, num_dofs);
d = hypre_CTAlloc(HYPRE_Real, num_dofs);
aux = hypre_CTAlloc(HYPRE_Real, num_dofs);
for (i=0; i < num_dofs; i++)
x[i] = 0.e0;
for (i=0; i < num_dofs; i++)
rhs[i] = rand();
max_iter = 1000;
hypre_printf("\nenter SchwarzPCG: =======================================\n");
ierr = hypre_Schwarzpcg(x, rhs,
a_dof_dof,
i_dof_dof, j_dof_dof,
i_domain_dof, j_domain_dof,
domain_matrixinverse,
num_domains,
v, w, d, aux,
max_iter,
num_dofs);
hypre_printf("\n\n=======================================================\n");
hypre_printf(" END test PCG solve: \n");
hypre_printf("===========================================================\n");
hypre_TFree(x);
hypre_TFree(rhs);
hypre_TFree(aux);
hypre_TFree(v);
hypre_TFree(w);
hypre_TFree(d);
----------------------------------------------------------------------- */
return ierr;
}
int raw_orient (Calibration* cal, control_par *cpar, int nfix, vec3d fix[], target pix[])
{
double X[10][6], y[10], XPX[6][6], XPy[6], beta[6];
int i, j, n, itnum, stopflag;
double dm = 0.0001, drad = 0.0001;
double xp, yp, xc, yc;
vec3d pos;
/* init X, y (set to zero) */
for (i = 0; i < 10; i++) {
for (j = 0; j < 6; j++)
X[i][j] = 0;
y[i] = 0;
}
cal->added_par.k1 = 0;
cal->added_par.k2 = 0;
cal->added_par.k3 = 0;
cal->added_par.p1 = 0;
cal->added_par.p2 = 0;
cal->added_par.scx = 1;
cal->added_par.she = 0;
/* main loop, program runs through it, until none of the beta values
comes over a threshold and no more points are thrown out
because of their residuals */
itnum = 0;
stopflag = 0;
while ((stopflag == 0) && (itnum < 20)) {
++itnum;
for (i = 0, n = 0; i < nfix; i++) {
/* we do not check the order - trust the user to click the points
in the correct order of appearance in man_ori and in the calibration
parameters GUI
*/
pixel_to_metric (&xc, &yc, pix[i].x, pix[i].y, cpar);
/* no corrections as additional parameters are neglected
correct_brown_affin (xc, yc, cal->added_par, &xc, &yc);
*/
/* every calibration dot is projected to the mm position, xp, yp */
vec_set(pos, fix[i][0], fix[i][1], fix[i][2]);
rotation_matrix(&(cal->ext_par));
img_coord (pos, cal, cpar->mm, &xp, &yp);
/* numeric derivatives of internal camera coefficients */
num_deriv_exterior(cal, cpar, dm, drad, pos, X[n], X[n + 1]);
y[n] = xc - xp;
y[n+1] = yc - yp;
n += 2;
}
/* Gauss Markoff Model */
ata ((double *) X, (double *) XPX, n, 6, 6);
matinv ((double *) XPX, 6, 6);
atl ((double *) XPy, (double *) X, y, n, 6, 6);
matmul ((double *) beta, (double *) XPX, (double *) XPy, 6,6,1,6,6);
stopflag = 1;
for (i = 0; i < 6; i++) {
if (fabs (beta[i]) > 0.1 )
stopflag = 0;
}
cal->ext_par.x0 += beta[0];
cal->ext_par.y0 += beta[1];
cal->ext_par.z0 += beta[2];
cal->ext_par.omega += beta[3];
cal->ext_par.phi += beta[4];
cal->ext_par.kappa += beta[5];
}
if (stopflag) {
rotation_matrix(&(cal->ext_par));
}
return stopflag;
}
void SimOneMVN_mxIW(double *nu, double *Lbdinvhlf, double *f1f2, int *pd,
int *pnreps, int *pN, double *es, double *YY)
{
int i, j, k, l, d, d2, N, nreps, mxnreps, Jrand;
int *lbuff;
double xd, sm, tstnu, nu_1, nu_2, zz, lambda, f_1, f_2;
double *df, *pW, *SgmHlf, *Y, *xbuff, *Sigma, *sig, *SigInv;
N = *pN;
d = *pd;
xd = (double) d;
d2 = d*d;
mxnreps=0;
for(l=0;l<N;l++) if(mxnreps < *(pnreps+l)) mxnreps = *(pnreps+l);
lbuff = (int *)S_alloc( 1,sizeof(int));
df = (double *)S_alloc( 1, sizeof(double));
pW = (double *)S_alloc( d2, sizeof(double));
xbuff = (double *)S_alloc( d, sizeof(double));
SgmHlf = (double *)S_alloc( d2, sizeof(double));
Y = (double *)S_alloc(mxnreps*d, sizeof(double));
Sigma = (double *)S_alloc( d2, sizeof(double));
SigInv = (double *)S_alloc( d2, sizeof(double));
sig = (double *)S_alloc( d, sizeof(double));
f_1 = *f1f2;
f_2 = *(f1f2+1);
lambda = *nu/(2 * xd + 2.0);
nu_1 = (2.0 * xd + 2.0)*(1.0 + (lambda - 1.0) *(1.0 - f_1)/(1-f_1/f_2));
nu_2 = (2.0 * xd + 2.0)*(1.0 + (lambda - 1.0) * f_2 );
tstnu = f_1/(nu_2 - (2.0*xd+2.0)) + (1.0-f_1)/(nu_1 - (2.0*xd+2.0));
tstnu = 1.0/tstnu + 2.0*xd + 2.0;
Rprintf("nu_1=%g, nu_2=%g, nu ?= %g", nu_1, nu_2, tstnu);
GetRNGstate();
/* NOTE: */
/* this block computes the average std dev over genes from the model */
/* its diagonal elements, passed to the pointer, sig (of size 3) */
/* are used for the purposes of assigning mean value to Y's under the alternative */
/* */
for(i=0;i<d;i++)
for(j=0;j<d;j++){
sm = 0.0;
for(k=0;k<d;k++)
sm += *(Lbdinvhlf + d*i + k) * (*(Lbdinvhlf + d*j + k));
*(SigInv + d*j + i) = sm;
}
matinv(SigInv, Sigma, pd);
for(i=0;i<d;i++) *(sig + i) = pow((*(Sigma + d*i + i))/(*nu - 2.0*xd - 2.0), 0.5);
for(l=0;l<N;l++){
/* Pick J = 1 w.p. 1-f_1, J= 2 w.p. f_1. */
/* Then draw an InvWish_d(nu_J, Lambda) matrix. This is done */
/* using the result: if Sigma^(-1) ~ Wish_d(nu-d-1, Lambda^(-1)) then */
/* Sigma ~ InvWish_d(nu, Lambda). I simulate N i.i.d. Wish_d(nu-d-1,Lambda^(-1)) */
/* matrices and then invert to get Sigma. One more catch, my rwishart routine */
/* uses the cholesky square root of the parameter matrix, Lambda instead of Lambda */
/* itself. Since I want the parameter matrix in the Wishart to be Lambda^(-1) then */
/* I should pass its cholesky square root which is Lbdinvhlf, e.g. the cholesky */
/* square root of Lambda inverse. That is computed in the calling R script and */
/* passed in. Notice the need to check that Lambda is nonsingular and that */
/* nu > 2*d + 2 (required so that the expected value of the inverse wishart */
/* is finite.) */
/* */
zz = unif_rand();
Jrand = 1*(zz < f_1);
*df = (1-Jrand)*nu_1 + Jrand*nu_2 - xd - 1.0;
rwishart1(df, pd, Lbdinvhlf, pW);
matinv(pW, Sigma, pd);
/* */
/* Sigma ~ (1-f_1)*InvWish_d(nu_1, Lambda) + f_1*InvWish_d(nu_2, Lambda) */
/* */
/* Next, use Sigma to simulate i.i.d. N(0_d, Sigma)'s */
nreps = *(pnreps + l);
*lbuff = nreps*d;
rnormn(lbuff, Y);
chol(Sigma, SgmHlf, pd);
for(i=0;i<nreps;i++){
for(j=0;j<d;j++){
sm = 0.0;
for(k=0;k<d;k++) sm += (*(SgmHlf +d*j +k))*(*(Y +d*i +k));
*(xbuff+j) = sm;
}
for(j=0;j<d;j++) *(Y + d*i + j) = *(xbuff + j) + *(es + l)*(*(sig + j));
}
for(i=0;i<(nreps*d);i++) *(YY + mxnreps*d*l + i) = *(Y+i);
}
PutRNGstate();
}
void RateMatrix::SetupMatrix(bool transition_probability_independent_sites_setup)
{
unsigned int i,j,k;
double mr;
double sum;
double U[numStates_squared+1], V[numStates_squared+1], T1[numStates_squared+1], T2[numStates_squared+1];
double Qij2pass[numStates_squared+1];
//////////
/// transition_probability_independent_sites_setup:
/// If we are calculating the transition probabilities as independent sites models, then we need
/// to run the code in the if statements. However, if we are doing the pseudo-dependent sites
/// calculations, we want to skip those code segments.
/// Segment #1:
/// * Sets the Qij entries to be the input substitution model.
/// -> For pseudo-dependent sites, we excise the Q matrix from the 4^N X 4^N sequence model.
/// Segment #2:
/// * Sets Qij so that branch length corresponds to expected number of changes.
/// -> pseudo-dependent sites branch lengths correspond not to 1 change per site, but whatever
/// the dependent sites model constrains it to.
//////////
CheckFrequencies();
k=0;
if (transition_probability_independent_sites_setup) {
for (i=0; i<numStates-1; i++) {
for (j=i+1; j<numStates; j++) {
Qij.at(i*numStates+j) = Qij.at(j*numStates+i) = Sij.at(k++);
}
}
for (i=0; i<numStates; i++) {
for (j=0; j<numStates; j++) {
Qij.at(i*numStates+j) *= pi.at(j);
}
}
mr=0;
for (i=0; i<numStates; i++) {
sum = 0;
Qij.at(i*numStates+i)=0;
for (j=0; j<numStates; j++) {
sum += Qij.at(i*numStates+j);
}
Qij.at(i*numStates+i) = -sum;
mr += pi.at(i) * sum;
}
//////////
/// Multiplies every element of Qij by 1.0/mr. This is abyx.
//////////
for (i=0; i<numStates_squared; Qij.at(i)*=1.0/mr,i++) ;
}
// Testcode: Print Qij.
//if (!transition_probability_independent_sites_setup) printQij();
//Testcode: does sum = 1?
sum = 0;
double partial = 0;
for (i = 0; i < numStates; i++) {
partial = 0;
for (j = 0; j < numStates; j++) {
if (i != j)
partial += Qij.at(i*numStates+j);
}
sum += pi.at(i) * partial;
}
//if (!transition_probability_independent_sites_setup) cerr << "Sum: " << sum << endl;
for (i = 0; i < numStates_squared; i++) Qij2pass[i]=Qij.at(i);
if ((k=eigen(1, Qij2pass, numStates, Root, T1, U, V, T2))!=0) {
fprintf(stderr, "\ncomplex roots in SetupMatrix");
exit(EXIT_FAILURE);
}
xtoy (U, V, numStates_squared);
matinv (V, numStates, numStates, T1);
vector<unsigned int> squared (numStates, 0), power_1 (numStates, 0);
for (i=0; i<numStates; i++) { squared[i]=i*numStates_squared; power_1[i]=i*numStates; }
for (i=0; i<numStates; i++) {
for (j=0; j<numStates; j++) {
for (k=0; k<numStates; k++) {
Cijk.at(squared[i]+power_1[j]+k) = U[power_1[i]+k]*V[power_1[k]+j];
}
}
}
}
/* creates 3 matrices: invA, B, C: dT + A^-1*BT = A^-1*Power,
* C = A^-1 * B. note that A is a diagonal matrix (no lateral
* capacitances. all capacitances are to ground). so, inva[i][i]
* (= 1/a[i][i]) is just enough.
*
* NOTE: EXTRA nodes: 1 chip bottom, 5 spreader and 5 heat sink nodes
* (north, south, east, west and bottom).
*/
void create_RC_matrices(flp_t *flp, int omit_lateral)
{
int i, j, k = 0, n = flp->n_units;
int **border;
double **len, *gx, *gy, **g, *c_ver, **t;
double r_sp1, r_sp2, r_hs; /* lateral resistances to spreader and heatsink */
/* NOTE: *_mid - the vertical R/C from center nodes of spreader
* and heatsink. *_ver - the vertical R/C from peripheral (n,s,e,w) nodes
*/
double r_sp_mid, r_sp_ver, r_hs_mid, r_hs_ver, c_sp_mid, c_sp_ver, c_hs_mid, c_hs_ver;
double gn=0, gs=0, ge=0, gw=0;
double w_chip = get_total_width (flp); /* x-axis */
double l_chip = get_total_height (flp); /* y-axis */
FILE *fp_b,*fp_c,*fp_inva,*fp_invb;
fp_b=fopen("B","w");
fp_c=fopen("C","w");
fp_invb=fopen("invB","w");
fp_inva=fopen("invA","w");
border = imatrix(n, 4);
len = matrix(n, n); /* len[i][j] = length of shared edge bet. i & j */
gx = vector(n); /* lumped conductances in x direction */
gy = vector(n); /* lumped conductances in y direction */
g = matrix(n+EXTRA, n+EXTRA); /* g[i][j] = conductance bet. nodes i & j */
c_ver = vector(n+EXTRA); /* vertical capacitance */
b = matrix(n+EXTRA, n+EXTRA); /* B, C, INVA and INVB are (n+EXTRA)x(n+EXTRA) matrices */
c = matrix(n+EXTRA, n+EXTRA);
inva = matrix(n+EXTRA, n+EXTRA);
invb = matrix(n+EXTRA, n+EXTRA);
t = matrix (n+EXTRA, n+EXTRA); /* copy of B */
/* compute the silicon fitting factor - see pg 10 of the UVA CS tech report - CS-TR-2003-08 */
factor_chip = C_FACTOR * ((SPEC_HEAT_CU / SPEC_HEAT_SI) * (w_chip + 0.88 * t_spreader) \
* (l_chip + 0.88 * t_spreader) * t_spreader / ( w_chip * l_chip * t_chip) + 1);
/* gx's and gy's of blocks */
for (i = 0; i < n; i++) {
gx[i] = 1.0/getr(K_SI, flp->units[i].height, flp->units[i].width, l_chip);
gy[i] = 1.0/getr(K_SI, flp->units[i].width, flp->units[i].height, w_chip);
}
/* shared lengths between blocks */
for (i = 0; i < n; i++)
for (j = i; j < n; j++)
len[i][j] = len[j][i] = get_shared_len(flp, i, j);
/* lateral R's of spreader and sink */
r_sp1 = getr(K_CU, (s_spreader+3*w_chip)/4.0, (s_spreader-w_chip)/4.0, w_chip);
r_sp2 = getr(K_CU, (3*s_spreader+w_chip)/4.0, (s_spreader-w_chip)/4.0, (s_spreader+3*w_chip)/4.0);
r_hs = getr(K_CU, (s_sink+3*s_spreader)/4.0, (s_sink-s_spreader)/4.0, s_spreader);
/* vertical R's and C's of spreader and sink */
r_sp_mid = RHO_CU * t_spreader / (w_chip * l_chip);
c_sp_mid = factor_pack * SPEC_HEAT_CU * t_spreader * (w_chip * l_chip);
r_sp_ver = RHO_CU * t_spreader * 4.0 / (s_spreader * s_spreader - w_chip*l_chip);
c_sp_ver = factor_pack * SPEC_HEAT_CU * t_spreader * (s_spreader * s_spreader - w_chip*l_chip) / 4.0;
r_hs_mid = RHO_CU * t_sink / (s_spreader*s_spreader);
c_hs_mid = factor_pack * SPEC_HEAT_CU * t_sink * (s_spreader * s_spreader);
r_hs_ver = RHO_CU * t_sink * 4.0 / (s_sink * s_sink - s_spreader*s_spreader);
c_hs_ver = factor_pack * SPEC_HEAT_CU * t_sink * (s_sink * s_sink - s_spreader*s_spreader) / 4.0;
/* short the R's from block centers to a particular chip edge */
for (i = 0; i < n; i++) {
if (eq(flp->units[i].bottomy + flp->units[i].height, l_chip)) {
gn += gy[i];
border[i][2] = 1; /* block is on northern border */
}
if (eq(flp->units[i].bottomy, 0)) {
gs += gy[i];
border[i][3] = 1; /* block is on southern border */
}
if (eq(flp->units[i].leftx + flp->units[i].width, w_chip)) {
ge += gx[i];
border[i][1] = 1; /* block is on eastern border */
}
if (eq(flp->units[i].leftx, 0)) {
gw += gx[i];
border[i][0] = 1; /* block is on western border */
}
}
/* overall R and C between nodes */
for (i = 0; i < n; i++) {
/* amongst functional units */
for (j = 0; j < n; j++) {
double part = 0;
if (!omit_lateral) {
if (is_horiz_adj(flp, i, j)){
part = gx[i] / flp->units[i].height;
printf("%d %d horiz adj\n",i,j);
}
else if (is_vert_adj(flp, i,j)) {
part = gy[i] / flp->units[i].width;
printf("%d %d vert adj\n",i,j);
}
}
g[i][j] = part * len[i][j];
}
/* C's from functional units to ground */
c_ver[i] = factor_chip * SPEC_HEAT_SI * t_chip * flp->units[i].height * flp->units[i].width;
/* lateral g's from block center to peripheral (n,s,e,w) spreader nodes */
g[i][n+SP_N]=g[n+SP_N][i]=2.0*border[i][2]/((1.0/gy[i])+r_sp1*gn/gy[i]);
g[i][n+SP_S]=g[n+SP_S][i]=2.0*border[i][3]/((1.0/gy[i])+r_sp1*gs/gy[i]);
g[i][n+SP_E]=g[n+SP_E][i]=2.0*border[i][1]/((1.0/gx[i])+r_sp1*ge/gx[i]);
g[i][n+SP_W]=g[n+SP_W][i]=2.0*border[i][0]/((1.0/gx[i])+r_sp1*gw/gx[i]);
/* vertical g's from block center to chip bottom */
g[i][n+CHIP_B]=g[n+CHIP_B][i]=2.0/(RHO_SI * t_chip / (flp->units[i].height * flp->units[i].width));
}
/* max slope (1/vertical RC time constant) for silicon */
max_slope = 1.0 / (factor_chip * t_chip * t_chip * RHO_SI * SPEC_HEAT_SI);
/* vertical g's and C's between central nodes */
/* between chip bottom and spreader bottom */
g[n+CHIP_B][n+SP_B]=g[n+SP_B][n+CHIP_B]=2.0/r_sp_mid;
/* from chip bottom to ground */
c_ver[n+CHIP_B]=c_sp_mid;
/* between spreader bottom and sink bottom */
g[n+SINK_B][n+SP_B]=g[n+SP_B][n+SINK_B]=2.0/r_hs_mid;
/* from spreader bottom to ground */
c_ver[n+SP_B]=c_hs_mid;
/* from sink bottom to ground */
c_ver[n+SINK_B]=c_convec;
/* g's and C's from peripheral(n,s,e,w) nodes */
for (i = 1; i <= 4; i++) {
/* vertical g's between peripheral spreader nodes and spreader bottom */
g[n+SP_B-i][n+SP_B]=g[n+SP_B][n+SP_B-i]=2.0/r_sp_ver;
/* lateral g's between peripheral spreader nodes and peripheral sink nodes */
g[n+SP_B-i][n+SINK_B-i]=g[n+SINK_B-i][n+SP_B-i]=2.0/(r_hs + r_sp2);
/* vertical g's between peripheral sink nodes and sink bottom */
g[n+SINK_B-i][n+SINK_B]=g[n+SINK_B][n+SINK_B-i]=2.0/r_hs_ver;
/* from peripheral spreader nodes to ground */
c_ver[n+SP_B-i]=c_sp_ver;
/* from peripheral sink nodes to ground */
c_ver[n+SINK_B-i]=c_hs_ver;
}
/* calculate matrices A, B such that A(dT) + BT = POWER */
for (i = 0; i < n+EXTRA; i++) {
for (j = 0; j < n+EXTRA; j++) {
if (i==j) {
inva[i][j] = 1.0/c_ver[i];
if (i == n+SINK_B) /* sink bottom */
b[i][j] += 1.0 / r_convec;
for (k = 0; k < n+EXTRA; k++) {
if ((g[i][k]==0.0)||(g[k][i])==0.0)
continue;
else
/* here is why the 2.0 factor comes when calculating g[][] */
b[i][j] += 1.0/((1.0/g[i][k])+(1.0/g[k][i]));
}
} else {
inva[i][j]=0.0;
if ((g[i][j]==0.0)||(g[j][i])==0.0)
b[i][j]=0.0;
else
b[i][j]=-1.0/((1.0/g[i][j])+(1.0/g[j][i]));
}
}
}
/* we are always going to use the eqn dT + A^-1 * B T = A^-1 * POWER. so, store C = A^-1 * B */
matmult(c, inva, b, n+EXTRA);
/* we will also be needing INVB so store it too */
copy_matrix(t, b, n+EXTRA, n+EXTRA);
matinv(invb, t, n+EXTRA);
for (i = 0; i < n+EXTRA; i++) {
for (j = 0; j < n+EXTRA; j++) {
fprintf(fp_inva,"%f ",inva[i][j]);
fprintf(fp_invb,"%f ",invb[i][j]);
fprintf(fp_c,"%f ",c[i][j]);
fprintf(fp_b,"%f ",b[i][j]);
}
fprintf(fp_inva, "\n");
fprintf(fp_invb, "\n");
fprintf(fp_c , "\n");
fprintf(fp_b, "\n");
}
fclose(fp_inva);
fclose(fp_b);
fclose(fp_c);
fclose(fp_invb);
/* dump_vector(c_ver, n+EXTRA); */
/* dump_matrix(invb, n+EXTRA, n+EXTRA); */
/* dump_matrix(c, n+EXTRA, n+EXTRA); */
/* cleanup */
free_matrix(t, n+EXTRA);
free_matrix(g, n+EXTRA);
free_matrix(len, n);
free_imatrix(border, n);
free_vector(c_ver);
free_vector(gx);
free_vector(gy);
}
/* resolve integer ambiguity by LAMBDA ---------------------------------------*/
static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa,char **msg)
{
prcopt_t *opt=&rtk->opt;
int i,j,ny,nb,info,nx=rtk->nx,na=rtk->na;
double *D,*DP,*y,*Qy,*b,*db,*Qb,*Qab,*QQ,s[2];
rtk->sol.ratio=0.0;
/* single to double-difference transformation matrix (D') */
D=zeros(nx,nx);
if (!D)
{
#ifdef _DEBUG_MSG
*msg+=sprintf(*msg,"resamb_LAMBDA mat error1\n");
#endif
return 0;
}
if ((nb=ddmat(rtk,D,msg))<=0) {
#ifdef _DEBUG_MSG
*msg+=sprintf(*msg,"no valid double-difference\n");
#endif
free(D);
return 0;
}
ny=na+nb;
y=mat(ny,1);
Qy=mat(ny,ny);
DP=mat(ny,nx);
b=mat(nb,2);
db=mat(nb,1);
Qb=mat(nb,nb);
Qab=mat(na,nb);
QQ=mat(na,nb);
if ((!y)||(!Qy)||(!DP)||(!b)||(!db)||(!Qb)||(!Qab)||(!QQ))
{
#ifdef _DEBUG_MSG
*msg+=sprintf(*msg,"resamb_LAMBDA mat error2\n");
#endif
free(D);
free(y);
free(Qy);
free(DP);
free(b);
free(db);
free(Qb);
free(Qab);
free(QQ);
return 0;
}
/* transform single to double-differenced phase-bias (y=D'*x, Qy=D'*P*D) */
matmul("TN",ny, 1,nx,1.0,D ,rtk->x,0.0,y );
matmul("TN",ny,nx,nx,1.0,D ,rtk->P,0.0,DP);
matmul("NN",ny,ny,nx,1.0,DP,D ,0.0,Qy);
/* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */
for (i=0; i<nb; i++) for (j=0; j<nb; j++) Qb [i+j*nb]=Qy[na+i+(na+j)*ny];
for (i=0; i<na; i++) for (j=0; j<nb; j++) Qab[i+j*na]=Qy[ i+(na+j)*ny];
/* lambda/mlambda integer least-square estimation */
if (!(info=lambda(nb,2,y+na,Qb,b,s))) {
rtk->sol.ratio=s[0]>0?(float)(s[1]/s[0]):0.0f;
if (rtk->sol.ratio>999.9) rtk->sol.ratio=999.9f;
/* validation by popular ratio-test */
if (s[0]<=0.0||s[1]/s[0]>=opt->thresar[0]) {
/* transform float to fixed solution (xa=xa-Qab*Qb\(b0-b)) */
for (i=0; i<na; i++) {
rtk->xa[i]=rtk->x[i];
for (j=0; j<na; j++) rtk->Pa[i+j*na]=rtk->P[i+j*nx];
}
for (i=0; i<nb; i++) {
bias[i]=b[i];
y[na+i]-=b[i];
}
if (!matinv(Qb,nb)) {
matmul("NN",nb,1,nb, 1.0,Qb ,y+na,0.0,db);
matmul("NN",na,1,nb,-1.0,Qab,db ,1.0,rtk->xa);
/* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */
matmul("NN",na,nb,nb, 1.0,Qab,Qb ,0.0,QQ);
matmul("NT",na,na,nb,-1.0,QQ ,Qab,1.0,rtk->Pa);
/* restore single-differenced ambiguity */
restamb(rtk,bias,nb,xa);
}
else nb=0;
}
else { /* validation failed */
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"ambiguity validation failed (nb=%d ratio=%.2f)\n",
nb,s[1]/s[0]);
#endif
nb=0;
}
}
else {
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"lambda error (info=%d)\n",info);
#endif
}
free(D);
free(y);
free(Qy);
free(DP);
free(b);
free(db);
free(Qb);
free(Qab);
free(QQ);
return nb; /* number of ambiguities */
}
/* orient() calculates orientation of the camera, updating its calibration
structure using the definitions and algorithms well described in [1].
Arguments:
Calibration* cal_in - camera calibration object
control_par *cpar - control parameters
int nfix - number of 3D known points
vec3d fix[] - each of nfix items is one 3D position of known point on
the calibration object.
target pix[] - image coordinates corresponding to each point in ``fix``.
can be obtained from the set of detected 2D points using
sortgrid(). The points which are associated with fix[] have real
pointer (.pnr attribute), others have -999.
orient_par flags - structure of all the flags of the parameters to be
(un)changed, read from orient.par parameter file using
read_orient_par(), defaults are zeros except for x_scale which is
by default 1.
Output:
Calibration *cal_in - if the orientation routine converged, this structure
is updated, otherwise, returned untouched. The routine works on a copy of
the calibration structure, cal.
double sigmabeta[] - array of deviations for each of the interior and
exterior parameters and glass interface vector (19 in total).
Returns:
On success, a pointer to an array of residuals. For each observation point
i = 0..n-1, residual 2*i is the Gauss-Markof residual for the x coordinate
and residual 2*i + 1 is for the y. Then come 10 cells with the delta
between initial guess and final solution for internal and distortion
parameters, which are also part of the G-M model and described in it.
On failure returns NULL.
*/
double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
target pix[], orient_par *flags, double sigmabeta[20])
{
int i,j,n, itnum, stopflag, n_obs=0, maxsize;
double ident[IDT], XPX[NPAR][NPAR], XPy[NPAR], beta[NPAR], omega=0;
double xp, yp, xpd, ypd, xc, yc, r, qq, p, sumP;
int numbers;
double al,be,ga,nGl,e1_x,e1_y,e1_z,e2_x,e2_y,e2_z,safety_x,safety_y,safety_z;
double *P, *y, *yh, *Xbeta, *resi;
vec3d glass_dir, tmp_vec, e1, e2;
Calibration *cal;
/* small perturbation for translation/rotation in meters and in radians */
double dm = 0.00001, drad = 0.0000001;
cal = malloc (sizeof (Calibration));
memcpy(cal, cal_in, sizeof (Calibration));
maxsize = nfix*2 + IDT;
P = (double *) calloc(maxsize, sizeof(double));
y = (double *) calloc(maxsize, sizeof(double));
yh = (double *) calloc(maxsize, sizeof(double));
Xbeta = (double *) calloc(maxsize, sizeof(double));
resi = (double *) calloc(maxsize, sizeof(double));
double (*X)[NPAR] = malloc(sizeof (*X) * maxsize);
double (*Xh)[NPAR] = malloc(sizeof (*Xh) * maxsize);
for(i = 0; i < maxsize; i++) {
for(j = 0; j < NPAR; j++) {
X[i][j] = 0.0;
Xh[i][j] = 0.0;
}
y[i] = 0;
P[i] = 1;
}
for(i = 0; i < NPAR; i++)
sigmabeta[j] = 0.0;
if(flags->interfflag){
numbers = 18;
} else{
numbers = 16;
}
vec_set(glass_dir,
cal->glass_par.vec_x, cal->glass_par.vec_y, cal->glass_par.vec_z);
nGl = vec_norm(glass_dir);
e1_x = 2*cal->glass_par.vec_z - 3*cal->glass_par.vec_x;
e1_y = 3*cal->glass_par.vec_x - 1*cal->glass_par.vec_z;
e1_z = 1*cal->glass_par.vec_y - 2*cal->glass_par.vec_y;
vec_set(tmp_vec, e1_x, e1_y, e1_z);
unit_vector(tmp_vec, e1);
e2_x = e1_y*cal->glass_par.vec_z - e1_z*cal->glass_par.vec_x;
e2_y = e1_z*cal->glass_par.vec_x - e1_x*cal->glass_par.vec_z;
e2_z = e1_x*cal->glass_par.vec_y - e1_y*cal->glass_par.vec_y;
vec_set(tmp_vec, e2_x, e2_y, e2_z);
unit_vector(tmp_vec, e2);
al = 0;
be = 0;
ga = 0;
/* init identities */
ident[0] = cal->int_par.cc;
ident[1] = cal->int_par.xh;
ident[2] = cal->int_par.yh;
ident[3] = cal->added_par.k1;
ident[4] = cal->added_par.k2;
ident[5] = cal->added_par.k3;
ident[6] = cal->added_par.p1;
ident[7] = cal->added_par.p2;
ident[8] = cal->added_par.scx;
ident[9] = cal->added_par.she;
safety_x = cal->glass_par.vec_x;
safety_y = cal->glass_par.vec_y;
safety_z = cal->glass_par.vec_z;
/* main loop, program runs through it, until none of the beta values
comes over a threshold and no more points are thrown out
because of their residuals */
itnum = 0;
stopflag = 0;
while ((stopflag == 0) && (itnum < NUM_ITER)) {
itnum++;
for (i = 0, n = 0; i < nfix; i++) {
/* check for correct correspondence
note that we do not use anymore pointer in fix, the points are read by
the order of appearance and if we want to use every other point
we use 'i', just check it is not -999 */
if(pix[i].pnr != i) continue;
switch (flags->useflag) {
case 1: if ((i % 2) == 0) continue; break;
case 2: if ((i % 2) != 0) continue; break;
case 3: if ((i % 3) == 0) continue; break;
}
/* get metric flat-image coordinates of the detected point */
pixel_to_metric (&xc, &yc, pix[i].x, pix[i].y, cpar);
correct_brown_affin (xc, yc, cal->added_par, &xc, &yc);
/* Projected 2D position on sensor of corresponding known point */
rotation_matrix(&(cal->ext_par));
img_coord (fix[i], cal, cpar->mm, &xp, &yp);
/* derivatives of distortion parameters */
r = sqrt (xp*xp + yp*yp);
X[n][7] = cal->added_par.scx;
X[n+1][7] = sin(cal->added_par.she);
X[n][8] = 0;
X[n+1][8] = 1;
X[n][9] = cal->added_par.scx * xp * r*r;
X[n+1][9] = yp * r*r;
X[n][10] = cal->added_par.scx * xp * pow(r,4.0);
X[n+1][10] = yp * pow(r,4.0);
X[n][11] = cal->added_par.scx * xp * pow(r,6.0);
X[n+1][11] = yp * pow(r,6.0);
X[n][12] = cal->added_par.scx * (2*xp*xp + r*r);
X[n+1][12] = 2 * xp * yp;
X[n][13] = 2 * cal->added_par.scx * xp * yp;
X[n+1][13] = 2*yp*yp + r*r;
qq = cal->added_par.k1*r*r; qq += cal->added_par.k2*pow(r,4.0);
qq += cal->added_par.k3*pow(r,6.0);
qq += 1;
X[n][14] = xp * qq + cal->added_par.p1 * (r*r + 2*xp*xp) + \
2*cal->added_par.p2*xp*yp;
X[n+1][14] = 0;
X[n][15] = -cos(cal->added_par.she) * yp;
X[n+1][15] = -sin(cal->added_par.she) * yp;
/* numeric derivatives of projection coordinates over external
parameters, 3D position and the angles */
num_deriv_exterior(cal, cpar, dm, drad, fix[i], X[n], X[n + 1]);
/* Num. deriv. of projection coords over sensor distance from PP */
cal->int_par.cc += dm;
rotation_matrix(&(cal->ext_par));
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][6] = (xpd - xp) / dm;
X[n+1][6] = (ypd - yp) / dm;
cal->int_par.cc -= dm;
/* ditto, over water-glass-air interface position vector */
al += dm;
cal->glass_par.vec_x += e1[0]*nGl*al;
cal->glass_par.vec_y += e1[1]*nGl*al;
cal->glass_par.vec_z += e1[2]*nGl*al;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][16] = (xpd - xp) / dm;
X[n+1][16] = (ypd - yp) / dm;
al -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
be += dm;
cal->glass_par.vec_x += e2[0]*nGl*be;
cal->glass_par.vec_y += e2[1]*nGl*be;
cal->glass_par.vec_z += e2[2]*nGl*be;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][17] = (xpd - xp) / dm;
X[n+1][17] = (ypd - yp) / dm;
be -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
ga += dm;
cal->glass_par.vec_x += cal->glass_par.vec_x*nGl*ga;
cal->glass_par.vec_y += cal->glass_par.vec_y*nGl*ga;
cal->glass_par.vec_z += cal->glass_par.vec_z*nGl*ga;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][18] = (xpd - xp) / dm;
X[n+1][18] = (ypd - yp) / dm;
ga -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
y[n] = xc - xp;
y[n+1] = yc - yp;
n += 2;
}
n_obs = n;
/* identities */
for (i = 0; i < IDT; i++)
X[n_obs + i][6 + i] = 1;
y[n_obs+0] = ident[0] - cal->int_par.cc;
y[n_obs+1] = ident[1] - cal->int_par.xh;
y[n_obs+2] = ident[2] - cal->int_par.yh;
y[n_obs+3] = ident[3] - cal->added_par.k1;
y[n_obs+4] = ident[4] - cal->added_par.k2;
y[n_obs+5] = ident[5] - cal->added_par.k3;
y[n_obs+6] = ident[6] - cal->added_par.p1;
y[n_obs+7] = ident[7] - cal->added_par.p2;
y[n_obs+8] = ident[8] - cal->added_par.scx;
y[n_obs+9] = ident[9] - cal->added_par.she;
/* weights */
for (i = 0; i < n_obs; i++)
P[i] = 1;
P[n_obs+0] = ( ! flags->ccflag) ? POS_INF : 1;
P[n_obs+1] = ( ! flags->xhflag) ? POS_INF : 1;
P[n_obs+2] = ( ! flags->yhflag) ? POS_INF : 1;
P[n_obs+3] = ( ! flags->k1flag) ? POS_INF : 1;
P[n_obs+4] = ( ! flags->k2flag) ? POS_INF : 1;
P[n_obs+5] = ( ! flags->k3flag) ? POS_INF : 1;
P[n_obs+6] = ( ! flags->p1flag) ? POS_INF : 1;
P[n_obs+7] = ( ! flags->p2flag) ? POS_INF : 1;
P[n_obs+8] = ( ! flags->scxflag) ? POS_INF : 1;
P[n_obs+9] = ( ! flags->sheflag) ? POS_INF : 1;
n_obs += IDT;
sumP = 0;
for (i = 0; i < n_obs; i++) { /* homogenize */
p = sqrt (P[i]);
for (j = 0; j < NPAR; j++)
Xh[i][j] = p * X[i][j];
yh[i] = p * y[i];
sumP += P[i];
}
/* Gauss Markoff Model it is the least square adjustment
of the redundant information contained both in the spatial
intersection and the resection, see [1], eq. 23 */
ata ((double *) Xh, (double *) XPX, n_obs, numbers, NPAR );
matinv ((double *) XPX, numbers, NPAR);
atl ((double *) XPy, (double *) Xh, yh, n_obs, numbers, NPAR);
matmul ((double *) beta, (double *) XPX, (double *) XPy,
numbers, numbers,1, NPAR, NPAR);
stopflag = 1;
for (i = 0; i < numbers; i++) {
if (fabs (beta[i]) > CONVERGENCE) stopflag = 0;
}
if ( ! flags->ccflag) beta[6] = 0.0;
if ( ! flags->xhflag) beta[7] = 0.0;
if ( ! flags->yhflag) beta[8] = 0.0;
if ( ! flags->k1flag) beta[9] = 0.0;
if ( ! flags->k2flag) beta[10] = 0.0;
if ( ! flags->k3flag) beta[11] = 0.0;
if ( ! flags->p1flag) beta[12] = 0.0;
if ( ! flags->p2flag) beta[13] = 0.0;
if ( ! flags->scxflag)beta[14] = 0.0;
if ( ! flags->sheflag) beta[15] = 0.0;
cal->ext_par.x0 += beta[0];
cal->ext_par.y0 += beta[1];
cal->ext_par.z0 += beta[2];
cal->ext_par.omega += beta[3];
cal->ext_par.phi += beta[4];
cal->ext_par.kappa += beta[5];
cal->int_par.cc += beta[6];
cal->int_par.xh += beta[7];
cal->int_par.yh += beta[8];
cal->added_par.k1 += beta[9];
cal->added_par.k2 += beta[10];
cal->added_par.k3 += beta[11];
cal->added_par.p1 += beta[12];
cal->added_par.p2 += beta[13];
cal->added_par.scx += beta[14];
cal->added_par.she += beta[15];
if (flags->interfflag) {
cal->glass_par.vec_x += e1[0]*nGl*beta[16];
cal->glass_par.vec_y += e1[1]*nGl*beta[16];
cal->glass_par.vec_z += e1[2]*nGl*beta[16];
cal->glass_par.vec_x += e2[0]*nGl*beta[17];
cal->glass_par.vec_y += e2[1]*nGl*beta[17];
cal->glass_par.vec_z += e2[2]*nGl*beta[17];
}
}
/* compute residuals etc. */
matmul ( (double *) Xbeta, (double *) X, (double *) beta, n_obs,
numbers, 1, n_obs, NPAR);
omega = 0;
for (i = 0; i < n_obs; i++) {
resi[i] = Xbeta[i] - y[i];
omega += resi[i] * P[i] * resi[i];
}
sigmabeta[NPAR] = sqrt (omega / (n_obs - numbers));
for (i = 0; i < numbers; i++) {
sigmabeta[i] = sigmabeta[NPAR] * sqrt(XPX[i][i]);
}
free(X);
free(P);
free(y);
free(Xbeta);
free(Xh);
if (stopflag){
rotation_matrix(&(cal->ext_par));
memcpy(cal_in, cal, sizeof (Calibration));
return resi;
}
else {
free(resi);
return NULL;
}
}
