void calculateobjhierarchy(scene *actualscene,int parentid, object *parent)
{
for (int on=0;on<actualscene->objectnum;on++)
{
object *o=&(actualscene->objects[on]);
if (o->data.primitive<100 && o->parent==parentid)
{
matrix relative,parentmatrix;
memcpy(relative,o->xformmatrix,sizeof(matrix));
m_identity(parentmatrix);
if (parentid!=-1)
{
memcpy(parentmatrix,parent->xformmatrix,sizeof(matrix));
matrix i;
m_invert(parentmatrix,i);
memcpy(parentmatrix,i,sizeof(matrix));
m_mult(relative,parentmatrix,relative);
//relative-ban az xformmatrix-ok relativ matrixa
m_mult(i,relative,relative);
m_mult(parent->currentmatrix,relative,relative);
m_mult(parent->xformmatrix,relative,relative);
memcpy(parentmatrix,parent->xformmatrix,sizeof(matrix));
}
memcpy(o->buffermatrix,o->xformmatrix,sizeof(matrix));
matrix m;
m_mult(relative,parentmatrix,m);
m_mult(m,o->currentmatrix,m);
obj_transform(o,m);
if (parentid!=-1)
{
matrix i;
//memcpy(i,o->buffermatrix,sizeof(matrix));
m_invert(o->buffermatrix,i);
m_mult(parent->currentmatrix,o->buffermatrix,m);
m_mult(m,o->currentmatrix,m);
m_mult(i,m,o->currentmatrix);
}
memcpy(o->xformmatrix,o->buffermatrix,sizeof(matrix));
calculateobjhierarchy(actualscene,o->number,o);
}
}
}
void matlib_main()
{
MATRIX *A, *B, *C;
MATRIX *At, *Bt;
long n, ni, i;
double sum;
random_1(-1);
n = query_long("dimension of arrays", 3L);
ni = query_long("number of iterations", 100L);
#ifdef VAX_VMS
init_stats();
#endif
m_alloc(&A, n, n);
m_alloc(&At, n, n);
m_alloc(&B, n, n);
m_alloc(&Bt, n, n);
m_alloc(&C, n, n);
for (i=sum=0; i<ni; i++) {
m_rand(At, -1.0L, 1.0L);
m_trans(A, At);
m_invert(B, A);
m_mult(C, A, B);
sum += fabs(m_det(C)-1.0);
}
m_free(&A);
m_free(&At);
m_free(&B);
m_free(&Bt);
m_free(&C);
printf("M.A.D.{DET{A.INV(A))}-1} = %e\n", sum/ni);
#ifdef VAX_VMS
report_stats(stdout, "stats: ");
#endif
}
long lsfn(
double *xd, double *yd, double *sy, /* data */
long nd, /* number of data points */
long nf, /* y = a_0 + a_1*x ... a_nf*x^nf */
double *coef, /* place to put co-efficients */
double *s_coef, /* and their sigmas */
double *chi, /* place to put reduced chi-squared */
double *diff /* place to put difference table */
)
{
long i, j, nt, unweighted;
double xp, *x_i, x0;
static MATRIX *X, *Y, *Yp, *C, *C_1, *Xt, *A, *Ca,
*XtC, *XtCX, *T, *Tt, *TC;
nt = nf + 1;
if (nd<nt) {
printf("error: insufficient data for requested order of fit\n");
printf("(%ld data points, %ld terms in fit)\n", nd, nt);
exit(1);
}
unweighted = 1;
if (sy)
for (i=1; i<nd; i++)
if (sy[i]!=sy[0]) {
unweighted = 0;
break;
}
/* allocate matrices */
m_alloc(&X, nd, nt);
m_alloc(&Y, nd, 1);
m_alloc(&Yp, nd, 1);
m_alloc(&Xt, nt, nd);
if (!unweighted) {
m_alloc(&C, nd, nd);
m_alloc(&C_1, nd, nd);
m_zero(C);
m_zero(C_1);
}
m_alloc(&A, nt, 1);
m_alloc(&Ca, nt, nt);
m_alloc(&XtC, nt, nd);
m_alloc(&XtCX, nt, nt);
m_alloc(&T, nt, nd);
m_alloc(&Tt, nd, nt);
m_alloc(&TC, nt, nd);
/* Compute X, Y, C, C_1. X[i][j] = (xd[i])^j. Y[i][0] = yd[i].
* C = delta(i,j)*sy[i]^2 (covariance matrix of yd)
* C_1 = INV(C)
*/
for (i=0; i<nd; i++) {
x_i = X->a[i];
x0 = xd[i];
xp = 1.0;
Y->a[i][0] = yd[i];
if (!unweighted) {
C->a[i][i] = sqr(sy[i]);
C_1->a[i][i] = 1/C->a[i][i];
}
for (j=0; j<nt; j++) {
x_i[j] = xp;
xp *= x0;
}
}
/* Compute A, the matrix of coefficients.
* Weighted least-squares solution is A = INV(Xt.INV(C).X).Xt.INV(C).y
* Unweighted solution is A = INV(Xt.X).Xt.y
*/
if (unweighted) {
/* eliminating 2 matrix operations makes this much faster than a weighted fit
* if there are many data points.
*/
if (!m_trans(Xt, X))
return(p_merror("transposing X"));
if (!m_mult(XtCX, Xt, X))
return(p_merror("multiplying Xt.X"));
if (!m_invert(XtCX, XtCX))
return(p_merror("inverting XtCX"));
if (!m_mult(T, XtCX, Xt))
return(p_merror("multiplying XtX.Xt"));
if (!m_mult(A, T, Y))
return(p_merror("multiplying T.Y"));
/* Compute covariance matrix of A, Ca = (T.Tt)*C[0][0] */
if (!m_trans(Tt, T))
return(p_merror("computing transpose of T"));
if (!m_mult(Ca, T, Tt))
return(p_merror("multiplying T.Tt"));
if (!m_scmul(Ca, Ca, sy?sqr(sy[0]):1))
return(p_merror("multiplying T.Tt by scalar"));
}
else {
if (!m_trans(Xt, X))
return(p_merror("transposing X"));
if (!m_mult(XtC, Xt, C_1))
return(p_merror("multiplying Xt.C_1"));
if (!m_mult(XtCX, XtC, X))
return(p_merror("multiplying XtC.X"));
if (!m_invert(XtCX, XtCX))
return(p_merror("inverting XtCX"));
if (!m_mult(T, XtCX, XtC))
return(p_merror("multiplying XtCX.XtC"));
if (!m_mult(A, T, Y))
return(p_merror("multiplying T.Y"));
/* Compute covariance matrix of A, Ca = T.C.Tt */
if (!m_mult(TC, T, C))
return(p_merror("multiplying T.C"));
if (!m_trans(Tt, T))
return(p_merror("computing transpose of T"));
if (!m_mult(Ca, TC, Tt))
return(p_merror("multiplying TC.Tt"));
}
for (i=0; i<nt; i++) {
coef[i] = A->a[i][0];
if (s_coef)
s_coef[i] = sqrt(Ca->a[i][i]);
}
/* Compute Yp = X.A, use to compute chi-squared */
if (chi) {
if (!m_mult(Yp, X, A))
return(p_merror("multiplying X.A"));
*chi = 0;
for (i=0; i<nd; i++) {
xp = (Yp->a[i][0] - yd[i]);
if (diff!=NULL)
diff[i] = xp;
xp /= sy?sy[i]:1;
*chi += xp*xp;
}
if (nd!=nt)
*chi /= (nd-nt);
}
m_free(&X);
m_free(&Y);
m_free(&Yp);
m_free(&Xt);
if (!unweighted) {
m_free(&C);
m_free(&C_1);
}
m_free(&A);
m_free(&Ca);
m_free(&XtC);
m_free(&XtCX);
m_free(&T);
m_free(&Tt);
m_free(&TC);
return(1);
}
void computeChromCorrectionMatrix(RUN *run, LINE_LIST *beamline, CHROM_CORRECTION *chrom)
{
VMATRIX *M;
double chromx, chromy;
double chromx0, chromy0;
double K2=0.0, *K2ptr;
ELEMENT_LIST *context;
long i, count, K2_param=0;
MATRIX *C, *Ct, *CtC, *inv_CtC;
m_alloc(&C, 2, chrom->n_families);
m_alloc(&Ct, chrom->n_families, 2);
m_alloc(&CtC, chrom->n_families, chrom->n_families);
m_alloc(&inv_CtC, chrom->n_families, chrom->n_families);
if (chrom->T)
m_free(&(chrom->T));
if (chrom->dK2)
m_free(&(chrom->dK2));
if (chrom->dchrom)
m_free(&(chrom->dchrom));
m_alloc(&(chrom->T), chrom->n_families, 2);
m_alloc(&(chrom->dK2), chrom->n_families, 1);
m_alloc(&(chrom->dchrom), 2, 1);
if (verbosityLevel>2) {
fprintf(stdout, "Computing chromaticity influence matrix for all named sextupoles.\n");
fflush(stdout);
}
computeChromaticities(&chromx0, &chromy0,
NULL, NULL, NULL, NULL,
beamline->twiss0, beamline->elast->twiss, M=beamline->matrix);
M = NULL;
for (i=0; i<chrom->n_families; i++) {
count = 0;
context = NULL;
while ((context=find_element(chrom->name[i], &context, beamline->elem_twiss))) {
if (!count && !(K2_param=confirm_parameter("K2", context->type))) {
fprintf(stdout, "error: element %s does not have K2 parameter\n",
context->name);
fflush(stdout);
exitElegant(1);
}
if (!(K2ptr = (double*)(context->p_elem + entity_description[context->type].parameter[K2_param].offset)))
bombElegant("K2ptr NULL in setup_chromaticity_correction", NULL);
if (count==0)
K2 = *K2ptr;
*K2ptr = K2 + chrom->sextupole_tweek;
if (context->matrix) {
free_matrices(context->matrix);
free(context->matrix);
context->matrix = NULL;
}
compute_matrix(context, run, NULL);
count++;
}
if (count==0) {
fprintf(stdout, "error: element %s is not in the beamline.\n", chrom->name[i]);
fflush(stdout);
exitElegant(1);
}
if (beamline->links) {
/* rebaseline_element_links(beamline->links, run, beamline); */
assert_element_links(beamline->links, run, beamline, STATIC_LINK+DYNAMIC_LINK);
}
if (M) {
free_matrices(M);
free(M);
M = NULL;
}
M = full_matrix(beamline->elem_twiss, run, 2);
computeChromaticities(&chromx, &chromy,
NULL, NULL, NULL, NULL, beamline->twiss0, beamline->elast->twiss, M);
C->a[0][i] = (chromx-chromx0)/chrom->sextupole_tweek;
C->a[1][i] = (chromy-chromy0)/chrom->sextupole_tweek;
if (C->a[0][i]==0 || C->a[1][i]==0) {
fprintf(stdout, "error: element %s does not change the chromaticity!\n", chrom->name[i]);
fflush(stdout);
exitElegant(1);
}
count = 0;
context = NULL;
while ((context=find_element(chrom->name[i], &context, beamline->elem_twiss))) {
if (!count && !(K2_param=confirm_parameter("K2", context->type))) {
fprintf(stdout, "error: element %s does not have K2 parameter\n",
context->name);
fflush(stdout);
exitElegant(1);
}
if (!(K2ptr = (double*)(context->p_elem + entity_description[context->type].parameter[K2_param].offset)))
bombElegant("K2ptr NULL in setup_chromaticity_correction", NULL);
if (!K2ptr)
bombElegant("K2ptr NULL in setup_chromaticity_correction", NULL);
*K2ptr = K2;
if (context->matrix) {
free_matrices(context->matrix);
free(context->matrix);
context->matrix = NULL;
}
compute_matrix(context, run, NULL);
count++;
}
}
if (M) {
free_matrices(M);
free(M);
M = NULL;
}
if (beamline->matrix) {
free_matrices(beamline->matrix);
free(beamline->matrix);
beamline->matrix = NULL;
}
beamline->matrix = full_matrix(beamline->elem_twiss, run, run->default_order);
if (verbosityLevel>1) {
fprintf(stdout, "\nfamily dCHROMx/dK2 dCHROMy/dK2\n");
fflush(stdout);
for (i=0; i<chrom->n_families; i++)
fprintf(stdout, "%10s: %14.7e %14.7e\n", chrom->name[i], C->a[0][i], C->a[1][i]);
fflush(stdout);
}
m_trans(Ct, C);
m_mult(CtC, Ct, C);
m_invert(inv_CtC, CtC);
m_mult(chrom->T, inv_CtC, Ct);
if (verbosityLevel>1) {
fprintf(stdout, "\nfamily dK2/dCHROMx dK2/dCHROMy\n");
fflush(stdout);
for (i=0; i<chrom->n_families; i++)
fprintf(stdout, "%10s: %14.7e %14.7e\n", chrom->name[i], chrom->T->a[i][0], chrom->T->a[i][1]);
fflush(stdout);
fprintf(stdout, "\n");
fflush(stdout);
}
m_free(&C);
m_free(&Ct);
m_free(&CtC);
m_free(&inv_CtC);
}
int interpolate_minimum(
double *fmin, /* for returning interpolated minimum */
double *zmin, /* for returning position of interpolated minimum */
double *value, /* array of function values at z_lo+dz*i */
double z_lo, double z_hi, /* z for value[0], value[n_values-1], respectively */
long n_values /* number of values in array value */
)
{
double f1, f2, f3, z1, z2, z3, dz, a, b, c;
register long i, i_min_value;
MATRIX *Z, *Zi, *A, *F;
/* first find the minimum value in the array */
f1 = DBL_MAX;
i_min_value = -1;
for (i=0; i<n_values; i++)
if ((f2=value[i])<f1) {
f1 = f2;
i_min_value = i;
}
if (i_min_value==-1)
return(0);
if (i_min_value==0 || i_min_value==n_values-1) {
*fmin = value[i_min_value];
if (n_values>=2)
*zmin = i_min_value*(z_hi-z_lo)/(n_values-1)+z_lo;
else if (i_min_value==0)
*zmin = z_lo;
else
*zmin = z_hi;
return(1);
}
/* interpolate to find minimum */
dz = (z_hi-z_lo)/(n_values-1);
f1 = value[--i_min_value];
z1 = dz*i_min_value + z_lo;
f2 = value[++i_min_value];
z2 = dz*i_min_value + z_lo;
f3 = value[++i_min_value];
z3 = dz*i_min_value + z_lo;
/* set up matrix problem to solve for parabola that fits the points
* (z1, f1), (z2, f2), (z3, f3).
*/
m_alloc(&Z, 3, 3);
m_alloc(&Zi, 3, 3);
m_alloc(&F, 3, 1);
m_alloc(&A, 3, 1);
Z->a[0][0] = z1*z1;
Z->a[0][1] = z1;
Z->a[0][2] = 1;
Z->a[1][0] = z2*z2;
Z->a[1][1] = z2;
Z->a[1][2] = 1;
Z->a[2][0] = z3*z3;
Z->a[2][1] = z3;
Z->a[2][2] = 1;
F->a[0][0] = f1;
F->a[1][0] = f2;
F->a[2][0] = f3;
m_invert(Zi, Z);
m_mult(A, Zi, F);
/* f = a.z^2 + b.z + c */
a = A->a[0][0];
b = A->a[1][0];
c = A->a[2][0];
*zmin = -b/(2*a);
*fmin = a*sqr(*zmin) + b*(*zmin) + c;
return(1);
}
void Vehicle::vehicle_model(veh_str *vehicl, double Sim_Time)
{
//char dummy[256];
double dp,dr,ds;
//double dp1;
double u[3];
static double u_old,old_phi,old_theta,old_omegac;
double heading_rads;
double head_err, depth_err, pitch_err, prop_err;
static double cosomega, sinomega;
static double costheta, sintheta, tantheta;
static double cosphi, sinphi;
static double rb2t[9]; //(3,3)
static double rt2b[9]; //(3,3)
double *pos;
double *ang;
double *vect;
double dpos[3];
double dang[3];
double dvect[6];
double dvec1[6];
double dvec2[6];
double dvec3[6];
double dvec4[6];
double dxth;
double dxuv[2];
double dxu1[2];
double dxu2[2];
int i;
(*vehicl).phi = (*vehicl).phi*PI/180; // Wei
(*vehicl).theta = (*vehicl).theta*PI/180;
if (init_flag == 1) {
m_invert(Mhinv,Mh,3);
m_mul(Ah,Mhinv,Ch,3,3,3);
m_mul(Bh,Mhinv,Dh,3,3,1);
m_invert(Mdinv,Md,4);
m_mul(Ad,Mdinv,Cd,4,4,4);
m_mul(Bd,Mdinv,Dd,4,4,1);
memset(A, 0, 36*sizeof(double));
A[0] = Xu/(mm - Xud); //(0,0)
A[7] = Ah[0]; //(1,1)=(0,0)
A[11] = Ah[2]; //(1,5)=(0,2)
A[14] = Ad[0]; //(2,2)=(0,0)
A[16] = Ad[1]; //(2,4)=(0,1)
A[21] = Kp/(Ixx - Kpd); //(3,3)
A[23] = (*vehicl).u*mm*zg; //(3,5)
A[26] = Ad[4]; //(4,2)=(1,0)
A[28] = Ad[5]; //(4,4)=(1,1)
A[31] = Ah[6]; //(5,1)=(2,0)
A[35] = Ah[8]; //(5,5)=(2,2)
memset(B, 0, 18*sizeof(double));
B[0] = Xdp/(mm-Xud); //(0,0)
B[1] = Xdr/(mm-Xud); //(0,1)
B[2] = Xds/(mm-Xud); //(0,2)
B[4] = Bh[0]; //(1,1)=(0,0)
B[8] = Bd[0]; //(2,2)=(0,0)
B[9] = Kdp/(Ixx-Kpd); //(3,0)
B[14] = Bd[1]; //(4,2)=(1,0)
B[16] = Bh[2]; //(5,1)=(2,0)
memset(C, 0, 18*sizeof(double));
C[5] = Ah[1]; //(1,2)=(0,1)
C[7] = Ad[3]; //(2,1)=(0,3)
C[9] = Kphi/(Ixx-Kpd); //(3,0)
C[13] = Ad[7]; //(4,1)=(1,3)
C[17] = Ah[7]; //(5,2)=(2,1)
for(i = 0; i < 16; i++) {
Cd1[i]=Cd[i];
}
for(i = 0; i < 9; i++) {
Ch1[i]=Ch[i];
}
d2r = PI/180.0;
rw2a[0] = 1; //(0,0)
rw2a[3] = 0; //(1,0)
rw2a[6] = 0; //(2,0)
u_old = 100;
old_phi = 100;
old_theta = 100;
old_omegac = 100;
}
if(abs((int)((*vehicl).u - u_old))>0.2){
u_old = (*vehicl).u;
Cd1[1] = (*vehicl).u*mm + Zq; //(0,1)
Cd1[4] = (*vehicl).u*(-mm)*xg + Mq; //(1,1)
Cd1[11] = -(*vehicl).u; //(2,3)
Ch1[2] = Yr - (*vehicl).u*mm; //(0,2)
Ch1[5] = Nr - (*vehicl).u*mm*xg; //(1,2)
m_mul(Ah,Mhinv,Ch1,3,3,3);
m_mul(Ad,Mdinv,Cd1,4,4,4);
A[23] = (*vehicl).u*mm*zg; //(3,5)
A[35] = Ah[8]; //(5,5)=(2,2)
A[16] = Ad[1]; //(2,4)=(0,1)
A[28] = Ad[5]; //(4,4)=(1,1)
}
heading_rads = (*vehicl).omega*d2r;
old_phi = (int)((*vehicl).phi);
old_theta = (*vehicl).theta;
old_omegac = (*vehicl).omega;
cosomega = cos(heading_rads);
costheta = cos((*vehicl).theta); //wei
sinomega = sin(heading_rads);
sintheta = sin((*vehicl).theta);
cosphi = cos((*vehicl).phi);
sinphi = sin((*vehicl).phi);
tantheta = tan((*vehicl).theta);
rt2b[0] = cosomega*costheta; //(0,0)
rt2b[1] = sinomega*costheta; //(0,1)
rt2b[2] = -sintheta; //(0,2)
rt2b[3] = -sinomega*cosphi+cosomega*sintheta*sinphi; //(1,0)
rt2b[4] = cosomega*cosphi+sinomega*sintheta*sinphi; //(1,1)
rt2b[5] = costheta*sinphi; //(1,2)
rt2b[6] = sinomega*sinphi+cosomega*sintheta*cosphi; //(2,0)
rt2b[7] = -cosomega*sinphi+sinomega*sintheta*cosphi; //(2,1)
rt2b[8] = costheta*cosphi; //(2,2)
m_transpose(rb2t,rt2b,3,3);
rw2a[1] = sinphi*tantheta; //(0,1)
rw2a[2] = cosphi*tantheta; //(0,2)
rw2a[4] = cosphi; //(1,1)
rw2a[5] = -sinphi; //(1,2)
rw2a[7] = sinphi/costheta; //(2,1)
rw2a[8] = cosphi/costheta; //(2,2)
pos = &((*vehicl).x);
ang = &((*vehicl).phi);
vect = &((*vehicl).u);
//---------------------------heading control---------------------------------
head_err = ((vehicl->omega_c) - (vehicl->omega)); // degrees
head_err = head_err*d2r; // radians
while(head_err>PI)
head_err = head_err - 2.0*PI;
while(head_err<-PI)
head_err = head_err + 2.0*PI;
dr = head_err*-0.2; // Calculate dr
//--------------------------------depth control-----------------------------
depth_err = (*vehicl).z_c - (*vehicl).z;
(*vehicl).theta_c = (-0.772*depth_err);
if((*vehicl).theta_c>0.5)
(*vehicl).theta_c = 0.5;
if((*vehicl).theta_c<-0.5)
(*vehicl).theta_c = -0.5;
pitch_err = ((*vehicl).theta_c - (*vehicl).theta);
ds = ((*vehicl).xth)*Cth + Dth*pitch_err; // Calculate ds
if(ds>0.8) // saturation
ds = 0.8;
if(ds<-0.8)
ds = -0.8;
//-----------------------------propulsion control---------------------------
prop_err = (*vehicl).vel_c - (*vehicl).u;
dp = Cu[0]*(*vehicl).xu1 + Cu[1]*(*vehicl).xu2 + prop_err*Du; // Calculate dp
u[0] = dp;
u[1] = dr;
u[2] = ds;
m_mul(dpos,rb2t,vect,3,3,1);
m_mul(dang,rw2a,(vect+3),3,3,1);
m_mul(dvec1,A,vect,6,6,1);
m_mul(dvec2,B,u,6,3,1);
m_mul(dvec3,C,ang,6,3,1);
m_add(dvec4,dvec1,dvec2,6,1);
m_add(dvect,dvec3,dvec4,6,1);
m_mul(dxu1,Au,&((*vehicl).xu1),2,2,1);
m_muls(dxu2,Bu,prop_err,2,1);
m_add(dxuv,dxu1,dxu2,2,1);
dxth = (Ath*((*vehicl).xth) + Bth*pitch_err); // jay
(*vehicl).xth += dxth*dt;
/*
for (i=0;i<3;i++) { // jay
pos[i] += dpos[i]*dt;
}
*/
pos[0] += dpos[0]*dt; // x
pos[1] += (-1)*dpos[1]*dt; // y, invert because of axis flip
pos[2] += dpos[2]*dt; // z
//
for (i=0;i<2;i++){ //jay
ang[i] += dang[i]*dt; //Wei
}
ang[2] += dang[2]*dt*180.0/PI;
(*vehicl).phi = ang[0]*180/PI; // Wei
(*vehicl).theta = ang[1]*180/PI;
(*vehicl).omega = ang[2];
for (int j=0;j<6;j++){
vect[j] += dvect[j]*dt;
}
(*vehicl).xu1 += dxuv[0]*dt;
(*vehicl).xu2 += dxuv[1]*dt;
//--------------------------------------------------------------------------
if(((*vehicl).src_find_tm) > Sim_Time){ // if not source found
(*vehicl).dsf += ((*vehicl).u)*(dt);} // integrated path length to source
if(((*vehicl).plm_find_tm) > Sim_Time){ // if not plume found
(*vehicl).dpf += ((*vehicl).u)*(dt);} // integrated path length to plume
if(!((*vehicl).src_fnd_flg)){ // if src fnd has not been declared
(*vehicl).dsd += ((*vehicl).u)*(dt);} // integrated path length to plume
while((*vehicl).omega >= 180.0)
(*vehicl).omega = (*vehicl).omega - 360.0;
while((*vehicl).omega < -180.0)
(*vehicl).omega = (*vehicl).omega + 360.0;
}
//*******************************************************
// Here is a Heat Bath update kernel...
// This routine is based on the algorithm presented by Cabbibo and
// Marrinari for updating a SU(n) link element by seperately
// updating SU(2) subgroups of that matrix using for each the
// SU(2) heat bath algorithm of creutz.
//*******************************************************
void cmhb_kernel( Float *sigma, Float *u)
{
// LRG.SetInterval(1, -1);
#if 1
// This tells which subblock is being updated:
int subblock=0;
// Here is the product of the link to be updated and the environment,
// which is the sum of the six plquettes which contain u.
Float u_sigma[MATRIXSIZE];
// Choosing an SU(2) subblock of u_sigma, it can be expressed
// in terms of four real numbers r[0] through r[3], where
// R = 1*r[0] + I*r_vector DOT sigma_vector,
// where sigma_vector refers to the three pauli matrices.
Float r[4];
// The subblock of u_sigma will not in general be SU(2), it will
// have to be seperated into a real SU(2) part and an imaginary
// SU(2), both needed to be rescaled to be SU(2). k is the
// rescaling parameter used there.
Float k;
Float alp;
Float R, R_, R__, R___, X, X_;
Float C, A, delta;
// A boltzman distributed vector which represents an SU(2) matrix.
Float bd[4];
// Variables required to get a random 3-vector on a sphere.
Float phi, sin_theta, cos_theta, sin_phi;
// and a normalization for those vectors
Float scale;
// Once a boltzman distributed SU(2) matrix has been constructed,
// it is necessary to imbed it into an SU(3) matrix (unit on the last
// diagonal)
// so that it can be multiplied by the origional link.
Float alpha[MATRIXSIZE];
Float mtx_r[MATRIXSIZE];
Float mtx_bd[MATRIXSIZE];
for(subblock=0; subblock<=1; subblock++)
{
// Construct the sum of plaquettes containing the link "u".
m_multiply3( u_sigma, u, sigma );
// Sort out the first SU2 subblock of that sum matrix and
// taking the upper 2x2 subblock of u_sigma, it can be expressed
// as: k_1 ( r[0] + r_vector DOT sigma_vector )
// + k_2 ( s[0] + s_vector DOT sigma_vector )
// extract the SU2 part called r and the coefficiant k.
if( subblock==0 )
{
r[0] = ( u_sigma[0] + u_sigma[4] ) / 2.;
r[1] = ( u_sigma[10] + u_sigma[12] ) / 2.;
r[2] = ( u_sigma[1] - u_sigma[3] ) / 2.;
r[3] = ( u_sigma[9] - u_sigma[13] ) / 2.;
}
else
{
r[0] = ( u_sigma[4] + u_sigma[8] ) / 2.;
r[1] = ( u_sigma[14] + u_sigma[16] ) / 2.;
r[2] = ( u_sigma[5] - u_sigma[7] ) / 2.;
r[3] = ( u_sigma[13] - u_sigma[17] ) / 2.;
}
k = sqrt( r[0]*r[0] + r[1]*r[1] + r[2]*r[2] + r[3]*r[3] );
r[0] = r[0]/k;
r[1] = r[1]/k;
r[2] = r[2]/k;
r[3] = r[3]/k;
// Float lambda = (2. * GJP.Beta() * k) / 3.;
// The 3 is the three of SU(3).
// Generate the zero-th component of the boltzman distributed
// using the Kennedy Pendleton algorithm: Phys Lett 156B (393).
int good_z = 0;
while( good_z == 0 )
{
Float my_pi = 3.14159265358979323846;
alp = (2./3.)* GJP.Beta() * k;
R__ = absR(LRG.Urand() );
R_ = absR(LRG.Urand() );
R = absR(LRG.Urand() );
R___ = absR(LRG.Urand() );
X = -(log(R ))/alp ;
X_ = -(log(R_))/alp ;
C = cos( 2*my_pi*R__ )*cos( 2*my_pi*R__ );
A = X * C;
delta = X_ + A;
if( (R___*R___) <= (1.-delta/2.) ) good_z = 1;
}
/* --------- For Debugging purposes
int good_z = 0;
int counter = 0;
while( good_z == 0 )
{
Float my_pi = 3.14159265358979323846;
alp = (2./3.)* GJP.Beta() * k;
R__ = absR(0.553422 );
R_ = absR(-0.098343 );
R = absR(-0.873420 );
R___ = absR(0.204563 );
X = -(log(R ))/alp ;
X_ = -(log(R_))/alp ;
C = cos( 2*my_pi*R__ )*cos( 2*my_pi*R__ );
A = X * C;
delta = X_ + A;
if(counter++ == 3) good_z = 1;
}
// -----------------------------------
*/
bd[0] = 1 - delta;
// Generate points on the surface of a sphere by directly
// generating theta and phi with the apropriate weighting.
scale = sqrt( 1. - bd[0]*bd[0] );
// Generate the three components of bd on a unit sphere.
cos_theta = LRG.Urand();
Float my_pi = 3.14159265358979323846;
phi = LRG.Urand()*my_pi; // phi e [-pi,pi)
/*
//-------------- For Debugging Purposes
cos_theta = -0.912342;
Float my_pi = 3.14159265358979323846;
phi = 0.243134*my_pi; // phi e [-pi,pi)
// ---------------------------------------------
*/
sin_theta = sqrt( 1. - cos_theta*cos_theta ); // sin(Theta) > 0
sin_phi = sqrt( 1. - cos(phi)*cos(phi) );
if( phi < 0 ) sin_phi *= -1;
// x = sin(Theta) x cos(Phi)
bd[1] = sin_theta * cos(phi);
// y = sin(Theta) x sin(Phi)
bd[2] = sin_theta * sin_phi;
// z = cos(Theta)
bd[3] = cos_theta;
bd[1]*=scale;
bd[2]*=scale;
bd[3]*=scale;
m_identity( mtx_r );
if(subblock==0)
{
*(mtx_r + 0) = r[0]; mtx_r[9] = r[3];
*(mtx_r + 1) = r[2]; mtx_r[10] = r[1];
*(mtx_r + 3) =-r[2]; mtx_r[12] = r[1];
*(mtx_r + 4) = r[0]; mtx_r[13] =-r[3];
}
else
{
*(mtx_r + 4) = r[0]; mtx_r[13] = r[3];
*(mtx_r + 5) = r[2]; mtx_r[14] = r[1];
*(mtx_r + 7) =-r[2]; mtx_r[16] = r[1];
*(mtx_r + 8) = r[0]; mtx_r[17] =-r[3];
}
m_invert( mtx_r );
m_identity( mtx_bd );
if(subblock==0)
{
*(mtx_bd + 0) = bd[0]; mtx_bd[9] = bd[3];
*(mtx_bd + 1) = bd[2]; mtx_bd[10] = bd[1];
*(mtx_bd + 3) =-bd[2]; mtx_bd[12] = bd[1];
*(mtx_bd + 4) = bd[0]; mtx_bd[13] =-bd[3];
}
else
{
*(mtx_bd + 4) = bd[0]; mtx_bd[13] = bd[3];
*(mtx_bd + 5) = bd[2]; mtx_bd[14] = bd[1];
*(mtx_bd + 7) =-bd[2]; mtx_bd[16] = bd[1];
*(mtx_bd + 8) = bd[0]; mtx_bd[17] =-bd[3];
}
m_multiply3( alpha, mtx_bd, mtx_r );
// Now that the update factor is computed multiply U by that
// factor.
m_multiply2l( u, alpha );
}
#endif
}
