MAT *ms_mltadd(const MAT *A1, const MAT *A2, double s, MAT *out)
#endif
{
/* register Real *A1_e, *A2_e, *out_e; */
/* register int j; */
int i, m, n;
if ( ! A1 || ! A2 )
error(E_NULL,"ms_mltadd");
if ( A1->m != A2->m || A1->n != A2->n )
error(E_SIZES,"ms_mltadd");
if ( out != A1 && out != A2 )
out = m_resize(out,A1->m,A1->n);
if ( s == 0.0 )
return m_copy(A1,out);
if ( s == 1.0 )
return m_add(A1,A2,out);
tracecatch(out = m_copy(A1,out),"ms_mltadd");
m = A1->m; n = A1->n;
for ( i = 0; i < m; i++ )
{
__mltadd__(out->me[i],A2->me[i],s,(int)n);
/**************************************************
A1_e = A1->me[i];
A2_e = A2->me[i];
out_e = out->me[i];
for ( j = 0; j < n; j++ )
out_e[j] = A1_e[j] + s*A2_e[j];
**************************************************/
}
return out;
}
struct mbuf *
compat_70_unp_addsockcred(struct lwp *l, struct mbuf *control)
{
struct sockcred70 *sc;
struct mbuf *m;
void *p;
m = sbcreatecontrol1(&p, SOCKCRED70SIZE(kauth_cred_ngroups(l->l_cred)),
SCM_OCREDS, SOL_SOCKET, M_WAITOK);
if (m == NULL)
return control;
sc = p;
sc->sc_uid = kauth_cred_getuid(l->l_cred);
sc->sc_euid = kauth_cred_geteuid(l->l_cred);
sc->sc_gid = kauth_cred_getgid(l->l_cred);
sc->sc_egid = kauth_cred_getegid(l->l_cred);
sc->sc_ngroups = kauth_cred_ngroups(l->l_cred);
for (int i = 0; i < sc->sc_ngroups; i++)
sc->sc_groups[i] = kauth_cred_group(l->l_cred, i);
return m_add(control, m);
}
static void reduce_thread_forces(int n, rvec *f,
tensor vir,
real *Vcorr,
int efpt_ind, real *dvdl,
int nthreads, f_thread_t *f_t)
{
int t, i;
/* This reduction can run over any number of threads */
#pragma omp parallel for num_threads(gmx_omp_nthreads_get(emntBonded)) private(t) schedule(static)
for (i = 0; i < n; i++)
{
for (t = 1; t < nthreads; t++)
{
rvec_inc(f[i], f_t[t].f[i]);
}
}
for (t = 1; t < nthreads; t++)
{
*Vcorr += f_t[t].Vcorr;
*dvdl += f_t[t].dvdl[efpt_ind];
m_add(vir, f_t[t].vir, vir);
}
}
void compute_globals(FILE *fplog, gmx_global_stat_t gstat, t_commrec *cr, t_inputrec *ir,
t_forcerec *fr, gmx_ekindata_t *ekind,
t_state *state, t_state *state_global, t_mdatoms *mdatoms,
t_nrnb *nrnb, t_vcm *vcm, gmx_wallcycle_t wcycle,
gmx_enerdata_t *enerd, tensor force_vir, tensor shake_vir, tensor total_vir,
tensor pres, rvec mu_tot, gmx_constr_t constr,
globsig_t *gs, gmx_bool bInterSimGS,
matrix box, gmx_mtop_t *top_global,
gmx_bool *bSumEkinhOld, int flags)
{
int i, gsi;
real gs_buf[eglsNR];
tensor corr_vir, corr_pres;
gmx_bool bEner, bPres, bTemp;
gmx_bool bStopCM, bGStat,
bReadEkin, bEkinAveVel, bScaleEkin, bConstrain;
real prescorr, enercorr, dvdlcorr, dvdl_ekin;
/* translate CGLO flags to gmx_booleans */
bStopCM = flags & CGLO_STOPCM;
bGStat = flags & CGLO_GSTAT;
bReadEkin = (flags & CGLO_READEKIN);
bScaleEkin = (flags & CGLO_SCALEEKIN);
bEner = flags & CGLO_ENERGY;
bTemp = flags & CGLO_TEMPERATURE;
bPres = (flags & CGLO_PRESSURE);
bConstrain = (flags & CGLO_CONSTRAINT);
/* we calculate a full state kinetic energy either with full-step velocity verlet
or half step where we need the pressure */
bEkinAveVel = (ir->eI == eiVV || (ir->eI == eiVVAK && bPres) || bReadEkin);
/* in initalization, it sums the shake virial in vv, and to
sums ekinh_old in leapfrog (or if we are calculating ekinh_old) for other reasons */
/* ########## Kinetic energy ############## */
if (bTemp)
{
/* Non-equilibrium MD: this is parallellized, but only does communication
* when there really is NEMD.
*/
if (PAR(cr) && (ekind->bNEMD))
{
accumulate_u(cr, &(ir->opts), ekind);
}
debug_gmx();
if (bReadEkin)
{
restore_ekinstate_from_state(cr, ekind, &state_global->ekinstate);
}
else
{
calc_ke_part(state, &(ir->opts), mdatoms, ekind, nrnb, bEkinAveVel);
}
debug_gmx();
}
/* Calculate center of mass velocity if necessary, also parallellized */
if (bStopCM)
{
calc_vcm_grp(0, mdatoms->homenr, mdatoms,
state->x, state->v, vcm);
}
if (bTemp || bStopCM || bPres || bEner || bConstrain)
{
if (!bGStat)
{
/* We will not sum ekinh_old,
* so signal that we still have to do it.
*/
*bSumEkinhOld = TRUE;
}
else
{
if (gs != NULL)
{
for (i = 0; i < eglsNR; i++)
{
gs_buf[i] = gs->sig[i];
}
}
if (PAR(cr))
{
wallcycle_start(wcycle, ewcMoveE);
global_stat(fplog, gstat, cr, enerd, force_vir, shake_vir, mu_tot,
ir, ekind, constr, bStopCM ? vcm : NULL,
gs != NULL ? eglsNR : 0, gs_buf,
top_global, state,
*bSumEkinhOld, flags);
wallcycle_stop(wcycle, ewcMoveE);
}
if (gs != NULL)
{
if (MULTISIM(cr) && bInterSimGS)
{
if (MASTER(cr))
{
/* Communicate the signals between the simulations */
gmx_sum_sim(eglsNR, gs_buf, cr->ms);
}
/* Communicate the signals form the master to the others */
gmx_bcast(eglsNR*sizeof(gs_buf[0]), gs_buf, cr);
}
for (i = 0; i < eglsNR; i++)
{
if (bInterSimGS || gs_simlocal[i])
{
/* Set the communicated signal only when it is non-zero,
* since signals might not be processed at each MD step.
*/
gsi = (gs_buf[i] >= 0 ?
(int)(gs_buf[i] + 0.5) :
(int)(gs_buf[i] - 0.5));
if (gsi != 0)
{
gs->set[i] = gsi;
}
/* Turn off the local signal */
gs->sig[i] = 0;
}
}
}
*bSumEkinhOld = FALSE;
}
}
if (!ekind->bNEMD && debug && bTemp && (vcm->nr > 0))
{
correct_ekin(debug,
0, mdatoms->homenr,
state->v, vcm->group_p[0],
mdatoms->massT, mdatoms->tmass, ekind->ekin);
}
/* Do center of mass motion removal */
if (bStopCM)
{
check_cm_grp(fplog, vcm, ir, 1);
do_stopcm_grp(0, mdatoms->homenr, mdatoms->cVCM,
state->x, state->v, vcm);
inc_nrnb(nrnb, eNR_STOPCM, mdatoms->homenr);
}
if (bEner)
{
/* Calculate the amplitude of the cosine velocity profile */
ekind->cosacc.vcos = ekind->cosacc.mvcos/mdatoms->tmass;
}
if (bTemp)
{
/* Sum the kinetic energies of the groups & calc temp */
/* compute full step kinetic energies if vv, or if vv-avek and we are computing the pressure with IR_NPT_TROTTER */
/* three maincase: VV with AveVel (md-vv), vv with AveEkin (md-vv-avek), leap with AveEkin (md).
Leap with AveVel is not supported; it's not clear that it will actually work.
bEkinAveVel: If TRUE, we simply multiply ekin by ekinscale to get a full step kinetic energy.
If FALSE, we average ekinh_old and ekinh*ekinscale_nhc to get an averaged half step kinetic energy.
*/
enerd->term[F_TEMP] = sum_ekin(&(ir->opts), ekind, &dvdl_ekin,
bEkinAveVel, bScaleEkin);
enerd->dvdl_lin[efptMASS] = (double) dvdl_ekin;
enerd->term[F_EKIN] = trace(ekind->ekin);
}
/* ########## Long range energy information ###### */
if (bEner || bPres || bConstrain)
{
calc_dispcorr(ir, fr, top_global->natoms, box, state->lambda[efptVDW],
corr_pres, corr_vir, &prescorr, &enercorr, &dvdlcorr);
}
if (bEner)
{
enerd->term[F_DISPCORR] = enercorr;
enerd->term[F_EPOT] += enercorr;
enerd->term[F_DVDL_VDW] += dvdlcorr;
}
/* ########## Now pressure ############## */
if (bPres || bConstrain)
{
m_add(force_vir, shake_vir, total_vir);
/* Calculate pressure and apply LR correction if PPPM is used.
* Use the box from last timestep since we already called update().
*/
enerd->term[F_PRES] = calc_pres(fr->ePBC, ir->nwall, box, ekind->ekin, total_vir, pres);
/* Calculate long range corrections to pressure and energy */
/* this adds to enerd->term[F_PRES] and enerd->term[F_ETOT],
and computes enerd->term[F_DISPCORR]. Also modifies the
total_vir and pres tesors */
m_add(total_vir, corr_vir, total_vir);
m_add(pres, corr_pres, pres);
enerd->term[F_PDISPCORR] = prescorr;
enerd->term[F_PRES] += prescorr;
}
}
gmx_bool constrain(FILE *fplog, gmx_bool bLog, gmx_bool bEner,
struct gmx_constr *constr,
t_idef *idef, t_inputrec *ir, gmx_ekindata_t *ekind,
t_commrec *cr,
gmx_int64_t step, int delta_step,
t_mdatoms *md,
rvec *x, rvec *xprime, rvec *min_proj,
gmx_bool bMolPBC, matrix box,
real lambda, real *dvdlambda,
rvec *v, tensor *vir,
t_nrnb *nrnb, int econq, gmx_bool bPscal,
real veta, real vetanew)
{
gmx_bool bOK, bDump;
int start, homenr, nrend;
int i, j, d;
int ncons, settle_error;
tensor vir_r_m_dr;
rvec *vstor;
real invdt, vir_fac, t;
t_ilist *settle;
int nsettle;
t_pbc pbc, *pbc_null;
char buf[22];
t_vetavars vetavar;
int nth, th;
if (econq == econqForceDispl && !EI_ENERGY_MINIMIZATION(ir->eI))
{
gmx_incons("constrain called for forces displacements while not doing energy minimization, can not do this while the LINCS and SETTLE constraint connection matrices are mass weighted");
}
bOK = TRUE;
bDump = FALSE;
start = 0;
homenr = md->homenr;
nrend = start+homenr;
/* set constants for pressure control integration */
init_vetavars(&vetavar, econq != econqCoord,
veta, vetanew, ir, ekind, bPscal);
if (ir->delta_t == 0)
{
invdt = 0;
}
else
{
invdt = 1/ir->delta_t;
}
if (ir->efep != efepNO && EI_DYNAMICS(ir->eI))
{
/* Set the constraint lengths for the step at which this configuration
* is meant to be. The invmasses should not be changed.
*/
lambda += delta_step*ir->fepvals->delta_lambda;
}
if (vir != NULL)
{
clear_mat(vir_r_m_dr);
}
where();
settle = &idef->il[F_SETTLE];
nsettle = settle->nr/(1+NRAL(F_SETTLE));
if (nsettle > 0)
{
nth = gmx_omp_nthreads_get(emntSETTLE);
}
else
{
nth = 1;
}
if (nth > 1 && constr->vir_r_m_dr_th == NULL)
{
snew(constr->vir_r_m_dr_th, nth);
snew(constr->settle_error, nth);
}
settle_error = -1;
/* We do not need full pbc when constraints do not cross charge groups,
* i.e. when dd->constraint_comm==NULL.
* Note that PBC for constraints is different from PBC for bondeds.
* For constraints there is both forward and backward communication.
*/
if (ir->ePBC != epbcNONE &&
(cr->dd || bMolPBC) && !(cr->dd && cr->dd->constraint_comm == NULL))
{
/* With pbc=screw the screw has been changed to a shift
* by the constraint coordinate communication routine,
* so that here we can use normal pbc.
*/
pbc_null = set_pbc_dd(&pbc, ir->ePBC, cr->dd, FALSE, box);
}
else
{
pbc_null = NULL;
}
/* Communicate the coordinates required for the non-local constraints
* for LINCS and/or SETTLE.
*/
if (cr->dd)
{
dd_move_x_constraints(cr->dd, box, x, xprime, econq == econqCoord);
}
if (constr->lincsd != NULL)
{
bOK = constrain_lincs(fplog, bLog, bEner, ir, step, constr->lincsd, md, cr,
x, xprime, min_proj,
box, pbc_null, lambda, dvdlambda,
invdt, v, vir != NULL, vir_r_m_dr,
econq, nrnb,
constr->maxwarn, &constr->warncount_lincs);
if (!bOK && constr->maxwarn >= 0)
{
if (fplog != NULL)
{
fprintf(fplog, "Constraint error in algorithm %s at step %s\n",
econstr_names[econtLINCS], gmx_step_str(step, buf));
}
bDump = TRUE;
}
}
if (constr->nblocks > 0)
{
switch (econq)
{
case (econqCoord):
bOK = bshakef(fplog, constr->shaked,
md->invmass, constr->nblocks, constr->sblock,
idef, ir, x, xprime, nrnb,
constr->lagr, lambda, dvdlambda,
invdt, v, vir != NULL, vir_r_m_dr,
constr->maxwarn >= 0, econq, &vetavar);
break;
case (econqVeloc):
bOK = bshakef(fplog, constr->shaked,
md->invmass, constr->nblocks, constr->sblock,
idef, ir, x, min_proj, nrnb,
constr->lagr, lambda, dvdlambda,
invdt, NULL, vir != NULL, vir_r_m_dr,
constr->maxwarn >= 0, econq, &vetavar);
break;
default:
gmx_fatal(FARGS, "Internal error, SHAKE called for constraining something else than coordinates");
break;
}
if (!bOK && constr->maxwarn >= 0)
{
if (fplog != NULL)
{
fprintf(fplog, "Constraint error in algorithm %s at step %s\n",
econstr_names[econtSHAKE], gmx_step_str(step, buf));
}
bDump = TRUE;
}
}
if (nsettle > 0)
{
int calcvir_atom_end;
if (vir == NULL)
{
calcvir_atom_end = 0;
}
else
{
calcvir_atom_end = md->homenr;
}
switch (econq)
{
case econqCoord:
#pragma omp parallel for num_threads(nth) schedule(static)
for (th = 0; th < nth; th++)
{
int start_th, end_th;
if (th > 0)
{
clear_mat(constr->vir_r_m_dr_th[th]);
}
start_th = (nsettle* th )/nth;
end_th = (nsettle*(th+1))/nth;
if (start_th >= 0 && end_th - start_th > 0)
{
csettle(constr->settled,
end_th-start_th,
settle->iatoms+start_th*(1+NRAL(F_SETTLE)),
pbc_null,
x[0], xprime[0],
invdt, v ? v[0] : NULL, calcvir_atom_end,
th == 0 ? vir_r_m_dr : constr->vir_r_m_dr_th[th],
th == 0 ? &settle_error : &constr->settle_error[th],
&vetavar);
}
}
inc_nrnb(nrnb, eNR_SETTLE, nsettle);
if (v != NULL)
{
inc_nrnb(nrnb, eNR_CONSTR_V, nsettle*3);
}
if (vir != NULL)
{
inc_nrnb(nrnb, eNR_CONSTR_VIR, nsettle*3);
}
break;
case econqVeloc:
case econqDeriv:
case econqForce:
case econqForceDispl:
#pragma omp parallel for num_threads(nth) schedule(static)
for (th = 0; th < nth; th++)
{
int start_th, end_th;
if (th > 0)
{
clear_mat(constr->vir_r_m_dr_th[th]);
}
start_th = (nsettle* th )/nth;
end_th = (nsettle*(th+1))/nth;
if (start_th >= 0 && end_th - start_th > 0)
{
settle_proj(constr->settled, econq,
end_th-start_th,
settle->iatoms+start_th*(1+NRAL(F_SETTLE)),
pbc_null,
x,
xprime, min_proj, calcvir_atom_end,
th == 0 ? vir_r_m_dr : constr->vir_r_m_dr_th[th],
&vetavar);
}
}
/* This is an overestimate */
inc_nrnb(nrnb, eNR_SETTLE, nsettle);
break;
case econqDeriv_FlexCon:
/* Nothing to do, since the are no flexible constraints in settles */
break;
default:
gmx_incons("Unknown constraint quantity for settle");
}
}
if (settle->nr > 0)
{
/* Combine virial and error info of the other threads */
for (i = 1; i < nth; i++)
{
m_add(vir_r_m_dr, constr->vir_r_m_dr_th[i], vir_r_m_dr);
settle_error = constr->settle_error[i];
}
if (econq == econqCoord && settle_error >= 0)
{
bOK = FALSE;
if (constr->maxwarn >= 0)
{
char buf[256];
sprintf(buf,
"\nstep " "%"GMX_PRId64 ": Water molecule starting at atom %d can not be "
"settled.\nCheck for bad contacts and/or reduce the timestep if appropriate.\n",
step, ddglatnr(cr->dd, settle->iatoms[settle_error*(1+NRAL(F_SETTLE))+1]));
if (fplog)
{
fprintf(fplog, "%s", buf);
}
fprintf(stderr, "%s", buf);
constr->warncount_settle++;
if (constr->warncount_settle > constr->maxwarn)
{
too_many_constraint_warnings(-1, constr->warncount_settle);
}
bDump = TRUE;
}
}
}
free_vetavars(&vetavar);
if (vir != NULL)
{
switch (econq)
{
case econqCoord:
vir_fac = 0.5/(ir->delta_t*ir->delta_t);
break;
case econqVeloc:
vir_fac = 0.5/ir->delta_t;
break;
case econqForce:
case econqForceDispl:
vir_fac = 0.5;
break;
default:
vir_fac = 0;
gmx_incons("Unsupported constraint quantity for virial");
}
if (EI_VV(ir->eI))
{
vir_fac *= 2; /* only constraining over half the distance here */
}
for (i = 0; i < DIM; i++)
{
for (j = 0; j < DIM; j++)
{
(*vir)[i][j] = vir_fac*vir_r_m_dr[i][j];
}
}
}
if (bDump)
{
dump_confs(fplog, step, constr->warn_mtop, start, homenr, cr, x, xprime, box);
}
if (econq == econqCoord)
{
if (ir->ePull == epullCONSTRAINT)
{
if (EI_DYNAMICS(ir->eI))
{
t = ir->init_t + (step + delta_step)*ir->delta_t;
}
else
{
t = ir->init_t;
}
set_pbc(&pbc, ir->ePBC, box);
pull_constraint(ir->pull, md, &pbc, cr, ir->delta_t, t, x, xprime, v, *vir);
}
if (constr->ed && delta_step > 0)
{
/* apply the essential dynamcs constraints here */
do_edsam(ir, step, cr, xprime, v, box, constr->ed);
}
}
return bOK;
}
int gmx_vanhove(int argc,char *argv[])
{
const char *desc[] = {
"g_vanhove computes the Van Hove correlation function.",
"The Van Hove G(r,t) is the probability that a particle that is at r0",
"at time zero can be found at position r0+r at time t.",
"g_vanhove determines G not for a vector r, but for the length of r.",
"Thus it gives the probability that a particle moves a distance of r",
"in time t.",
"Jumps across the periodic boundaries are removed.",
"Corrections are made for scaling due to isotropic",
"or anisotropic pressure coupling.",
"[PAR]",
"With option [TT]-om[tt] the whole matrix can be written as a function",
"of t and r or as a function of sqrt(t) and r (option [TT]-sqrt[tt]).",
"[PAR]",
"With option [TT]-or[tt] the Van Hove function is plotted for one",
"or more values of t. Option [TT]-nr[tt] sets the number of times,",
"option [TT]-fr[tt] the number spacing between the times.",
"The binwidth is set with option [TT]-rbin[tt]. The number of bins",
"is determined automatically.",
"[PAR]",
"With option [TT]-ot[tt] the integral up to a certain distance",
"(option [TT]-rt[tt]) is plotted as a function of time.",
"[PAR]",
"For all frames that are read the coordinates of the selected particles",
"are stored in memory. Therefore the program may use a lot of memory.",
"For options [TT]-om[tt] and [TT]-ot[tt] the program may be slow.",
"This is because the calculation scales as the number of frames times",
"[TT]-fm[tt] or [TT]-ft[tt].",
"Note that with the [TT]-dt[tt] option the memory usage and calculation",
"time can be reduced."
};
static int fmmax=0,ftmax=0,nlev=81,nr=1,fshift=0;
static real sbin=0,rmax=2,rbin=0.01,mmax=0,rint=0;
t_pargs pa[] = {
{ "-sqrt", FALSE, etREAL,{&sbin},
"Use sqrt(t) on the matrix axis which binspacing # in sqrt(ps)" },
{ "-fm", FALSE, etINT, {&fmmax},
"Number of frames in the matrix, 0 is plot all" },
{ "-rmax", FALSE, etREAL, {&rmax},
"Maximum r in the matrix (nm)" },
{ "-rbin", FALSE, etREAL, {&rbin},
"Binwidth in the matrix and for -or (nm)" },
{ "-mmax", FALSE, etREAL, {&mmax},
"Maximum density in the matrix, 0 is calculate (1/nm)" },
{ "-nlevels" ,FALSE, etINT, {&nlev},
"Number of levels in the matrix" },
{ "-nr", FALSE, etINT, {&nr},
"Number of curves for the -or output" },
{ "-fr", FALSE, etINT, {&fshift},
"Frame spacing for the -or output" },
{ "-rt", FALSE, etREAL, {&rint},
"Integration limit for the -ot output (nm)" },
{ "-ft", FALSE, etINT, {&ftmax},
"Number of frames in the -ot output, 0 is plot all" }
};
#define NPA asize(pa)
t_filenm fnm[] = {
{ efTRX, NULL, NULL, ffREAD },
{ efTPS, NULL, NULL, ffREAD },
{ efNDX, NULL, NULL, ffOPTRD },
{ efXPM, "-om", "vanhove", ffOPTWR },
{ efXVG, "-or", "vanhove_r", ffOPTWR },
{ efXVG, "-ot", "vanhove_t", ffOPTWR }
};
#define NFILE asize(fnm)
output_env_t oenv;
const char *matfile,*otfile,*orfile;
char title[256];
t_topology top;
int ePBC;
matrix boxtop,box,*sbox,avbox,corr;
rvec *xtop,*x,**sx;
int isize,nalloc,nallocn,natom;
t_trxstatus *status;
atom_id *index;
char *grpname;
int nfr,f,ff,i,m,mat_nx=0,nbin=0,bin,mbin,fbin;
real *time,t,invbin=0,rmax2=0,rint2=0,d2;
real invsbin=0,matmax,normfac,dt,*tickx,*ticky;
char buf[STRLEN],**legend;
real **mat=NULL;
int *pt=NULL,**pr=NULL,*mcount=NULL,*tcount=NULL,*rcount=NULL;
FILE *fp;
t_rgb rlo={1,1,1}, rhi={0,0,0};
CopyRight(stderr,argv[0]);
parse_common_args(&argc,argv,PCA_CAN_VIEW | PCA_CAN_TIME | PCA_BE_NICE,
NFILE,fnm,asize(pa),pa,asize(desc),desc,0,NULL,&oenv);
matfile = opt2fn_null("-om",NFILE,fnm);
if (opt2parg_bSet("-fr",NPA,pa))
orfile = opt2fn("-or",NFILE,fnm);
else
orfile = opt2fn_null("-or",NFILE,fnm);
if (opt2parg_bSet("-rt",NPA,pa))
otfile = opt2fn("-ot",NFILE,fnm);
else
otfile = opt2fn_null("-ot",NFILE,fnm);
if (!matfile && !otfile && !orfile) {
fprintf(stderr,
"For output set one (or more) of the output file options\n");
exit(0);
}
read_tps_conf(ftp2fn(efTPS,NFILE,fnm),title,&top,&ePBC,&xtop,NULL,boxtop,
FALSE);
get_index(&top.atoms,ftp2fn_null(efNDX,NFILE,fnm),1,&isize,&index,&grpname);
nalloc = 0;
time = NULL;
sbox = NULL;
sx = NULL;
clear_mat(avbox);
natom=read_first_x(oenv,&status,ftp2fn(efTRX,NFILE,fnm),&t,&x,box);
nfr = 0;
do {
if (nfr >= nalloc) {
nalloc += 100;
srenew(time,nalloc);
srenew(sbox,nalloc);
srenew(sx,nalloc);
}
time[nfr] = t;
copy_mat(box,sbox[nfr]);
/* This assumes that the off-diagonal box elements
* are not affected by jumps across the periodic boundaries.
*/
m_add(avbox,box,avbox);
snew(sx[nfr],isize);
for(i=0; i<isize; i++)
copy_rvec(x[index[i]],sx[nfr][i]);
nfr++;
} while (read_next_x(oenv,status,&t,natom,x,box));
/* clean up */
sfree(x);
close_trj(status);
fprintf(stderr,"Read %d frames\n",nfr);
dt = (time[nfr-1] - time[0])/(nfr - 1);
/* Some ugly rounding to get nice nice times in the output */
dt = (int)(10000.0*dt + 0.5)/10000.0;
invbin = 1.0/rbin;
if (matfile) {
if (fmmax <= 0 || fmmax >= nfr)
fmmax = nfr - 1;
snew(mcount,fmmax);
nbin = (int)(rmax*invbin + 0.5);
if (sbin == 0) {
mat_nx = fmmax + 1;
} else {
invsbin = 1.0/sbin;
mat_nx = sqrt(fmmax*dt)*invsbin + 1;
}
snew(mat,mat_nx);
for(f=0; f<mat_nx; f++)
snew(mat[f],nbin);
rmax2 = sqr(nbin*rbin);
/* Initialize time zero */
mat[0][0] = nfr*isize;
mcount[0] += nfr;
} else {
fmmax = 0;
}
if (orfile) {
snew(pr,nr);
nalloc = 0;
snew(rcount,nr);
}
if (otfile) {
if (ftmax <= 0)
ftmax = nfr - 1;
snew(tcount,ftmax);
snew(pt,nfr);
rint2 = rint*rint;
/* Initialize time zero */
pt[0] = nfr*isize;
tcount[0] += nfr;
} else {
ftmax = 0;
}
msmul(avbox,1.0/nfr,avbox);
for(f=0; f<nfr; f++) {
if (f % 100 == 0)
fprintf(stderr,"\rProcessing frame %d",f);
/* Scale all the configuration to the average box */
m_inv_ur0(sbox[f],corr);
mmul_ur0(avbox,corr,corr);
for(i=0; i<isize; i++) {
mvmul_ur0(corr,sx[f][i],sx[f][i]);
if (f > 0) {
/* Correct for periodic jumps */
for(m=DIM-1; m>=0; m--) {
while(sx[f][i][m] - sx[f-1][i][m] > 0.5*avbox[m][m])
rvec_dec(sx[f][i],avbox[m]);
while(sx[f][i][m] - sx[f-1][i][m] <= -0.5*avbox[m][m])
rvec_inc(sx[f][i],avbox[m]);
}
}
}
for(ff=0; ff<f; ff++) {
fbin = f - ff;
if (fbin <= fmmax || fbin <= ftmax) {
if (sbin == 0)
mbin = fbin;
else
mbin = (int)(sqrt(fbin*dt)*invsbin + 0.5);
for(i=0; i<isize; i++) {
d2 = distance2(sx[f][i],sx[ff][i]);
if (mbin < mat_nx && d2 < rmax2) {
bin = (int)(sqrt(d2)*invbin + 0.5);
if (bin < nbin) {
mat[mbin][bin] += 1;
}
}
if (fbin <= ftmax && d2 <= rint2)
pt[fbin]++;
}
if (matfile)
mcount[mbin]++;
if (otfile)
tcount[fbin]++;
}
}
if (orfile) {
for(fbin=0; fbin<nr; fbin++) {
ff = f - (fbin + 1)*fshift;
if (ff >= 0) {
for(i=0; i<isize; i++) {
d2 = distance2(sx[f][i],sx[ff][i]);
bin = (int)(sqrt(d2)*invbin);
if (bin >= nalloc) {
nallocn = 10*(bin/10) + 11;
for(m=0; m<nr; m++) {
srenew(pr[m],nallocn);
for(i=nalloc; i<nallocn; i++)
pr[m][i] = 0;
}
nalloc = nallocn;
}
pr[fbin][bin]++;
}
rcount[fbin]++;
}
}
}
}
fprintf(stderr,"\n");
if (matfile) {
matmax = 0;
for(f=0; f<mat_nx; f++) {
normfac = 1.0/(mcount[f]*isize*rbin);
for(i=0; i<nbin; i++) {
mat[f][i] *= normfac;
if (mat[f][i] > matmax && (f!=0 || i!=0))
matmax = mat[f][i];
}
}
fprintf(stdout,"Value at (0,0): %.3f, maximum of the rest %.3f\n",
mat[0][0],matmax);
if (mmax > 0)
matmax = mmax;
snew(tickx,mat_nx);
for(f=0; f<mat_nx; f++) {
if (sbin == 0)
tickx[f] = f*dt;
else
tickx[f] = f*sbin;
}
snew(ticky,nbin+1);
for(i=0; i<=nbin; i++)
ticky[i] = i*rbin;
fp = ffopen(matfile,"w");
write_xpm(fp,MAT_SPATIAL_Y,"Van Hove function","G (1/nm)",
sbin==0 ? "time (ps)" : "sqrt(time) (ps^1/2)","r (nm)",
mat_nx,nbin,tickx,ticky,mat,0,matmax,rlo,rhi,&nlev);
ffclose(fp);
}
if (orfile) {
fp = xvgropen(orfile,"Van Hove function","r (nm)","G (nm\\S-1\\N)",oenv);
fprintf(fp,"@ subtitle \"for particles in group %s\"\n",grpname);
snew(legend,nr);
for(fbin=0; fbin<nr; fbin++) {
sprintf(buf,"%g ps",(fbin + 1)*fshift*dt);
legend[fbin] = strdup(buf);
}
xvgr_legend(fp,nr,(const char**)legend,oenv);
for(i=0; i<nalloc; i++) {
fprintf(fp,"%g",i*rbin);
for(fbin=0; fbin<nr; fbin++)
fprintf(fp," %g",
(real)pr[fbin][i]/(rcount[fbin]*isize*rbin*(i==0 ? 0.5 : 1)));
fprintf(fp,"\n");
}
ffclose(fp);
}
if (otfile) {
sprintf(buf,"Probability of moving less than %g nm",rint);
fp = xvgropen(otfile,buf,"t (ps)","",oenv);
fprintf(fp,"@ subtitle \"for particles in group %s\"\n",grpname);
for(f=0; f<=ftmax; f++)
fprintf(fp,"%g %g\n",f*dt,(real)pt[f]/(tcount[f]*isize));
ffclose(fp);
}
do_view(oenv, matfile,NULL);
do_view(oenv, orfile,NULL);
do_view(oenv, otfile,NULL);
thanx(stderr);
return 0;
}
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;
}
void
m_add_mailaddr(struct mproc *m, const struct mailaddr *maddr)
{
m_add(m, maddr, sizeof(*maddr));
}
/* TODO Specialize this routine into init-time and loop-time versions?
e.g. bReadEkin is only true when restoring from checkpoint */
void compute_globals(FILE *fplog, gmx_global_stat *gstat, t_commrec *cr, t_inputrec *ir,
t_forcerec *fr, gmx_ekindata_t *ekind,
t_state *state, t_mdatoms *mdatoms,
t_nrnb *nrnb, t_vcm *vcm, gmx_wallcycle_t wcycle,
gmx_enerdata_t *enerd, tensor force_vir, tensor shake_vir, tensor total_vir,
tensor pres, rvec mu_tot, gmx_constr_t constr,
gmx::SimulationSignaller *signalCoordinator,
matrix box, int *totalNumberOfBondedInteractions,
gmx_bool *bSumEkinhOld, int flags)
{
tensor corr_vir, corr_pres;
gmx_bool bEner, bPres, bTemp;
gmx_bool bStopCM, bGStat,
bReadEkin, bEkinAveVel, bScaleEkin, bConstrain;
real prescorr, enercorr, dvdlcorr, dvdl_ekin;
/* translate CGLO flags to gmx_booleans */
bStopCM = flags & CGLO_STOPCM;
bGStat = flags & CGLO_GSTAT;
bReadEkin = (flags & CGLO_READEKIN);
bScaleEkin = (flags & CGLO_SCALEEKIN);
bEner = flags & CGLO_ENERGY;
bTemp = flags & CGLO_TEMPERATURE;
bPres = (flags & CGLO_PRESSURE);
bConstrain = (flags & CGLO_CONSTRAINT);
/* we calculate a full state kinetic energy either with full-step velocity verlet
or half step where we need the pressure */
bEkinAveVel = (ir->eI == eiVV || (ir->eI == eiVVAK && bPres) || bReadEkin);
/* in initalization, it sums the shake virial in vv, and to
sums ekinh_old in leapfrog (or if we are calculating ekinh_old) for other reasons */
/* ########## Kinetic energy ############## */
if (bTemp)
{
/* Non-equilibrium MD: this is parallellized, but only does communication
* when there really is NEMD.
*/
if (PAR(cr) && (ekind->bNEMD))
{
accumulate_u(cr, &(ir->opts), ekind);
}
if (!bReadEkin)
{
calc_ke_part(state, &(ir->opts), mdatoms, ekind, nrnb, bEkinAveVel);
}
}
/* Calculate center of mass velocity if necessary, also parallellized */
if (bStopCM)
{
calc_vcm_grp(0, mdatoms->homenr, mdatoms,
state->x, state->v, vcm);
}
if (bTemp || bStopCM || bPres || bEner || bConstrain)
{
if (!bGStat)
{
/* We will not sum ekinh_old,
* so signal that we still have to do it.
*/
*bSumEkinhOld = TRUE;
}
else
{
gmx::ArrayRef<real> signalBuffer = signalCoordinator->getCommunicationBuffer();
if (PAR(cr))
{
wallcycle_start(wcycle, ewcMoveE);
global_stat(gstat, cr, enerd, force_vir, shake_vir, mu_tot,
ir, ekind, constr, bStopCM ? vcm : NULL,
signalBuffer.size(), signalBuffer.data(),
totalNumberOfBondedInteractions,
*bSumEkinhOld, flags);
wallcycle_stop(wcycle, ewcMoveE);
}
signalCoordinator->finalizeSignals();
*bSumEkinhOld = FALSE;
}
}
if (!ekind->bNEMD && debug && bTemp && (vcm->nr > 0))
{
correct_ekin(debug,
0, mdatoms->homenr,
state->v, vcm->group_p[0],
mdatoms->massT, mdatoms->tmass, ekind->ekin);
}
/* Do center of mass motion removal */
if (bStopCM)
{
check_cm_grp(fplog, vcm, ir, 1);
do_stopcm_grp(0, mdatoms->homenr, mdatoms->cVCM,
state->x, state->v, vcm);
inc_nrnb(nrnb, eNR_STOPCM, mdatoms->homenr);
}
if (bEner)
{
/* Calculate the amplitude of the cosine velocity profile */
ekind->cosacc.vcos = ekind->cosacc.mvcos/mdatoms->tmass;
}
if (bTemp)
{
/* Sum the kinetic energies of the groups & calc temp */
/* compute full step kinetic energies if vv, or if vv-avek and we are computing the pressure with inputrecNptTrotter */
/* three maincase: VV with AveVel (md-vv), vv with AveEkin (md-vv-avek), leap with AveEkin (md).
Leap with AveVel is not supported; it's not clear that it will actually work.
bEkinAveVel: If TRUE, we simply multiply ekin by ekinscale to get a full step kinetic energy.
If FALSE, we average ekinh_old and ekinh*ekinscale_nhc to get an averaged half step kinetic energy.
*/
enerd->term[F_TEMP] = sum_ekin(&(ir->opts), ekind, &dvdl_ekin,
bEkinAveVel, bScaleEkin);
enerd->dvdl_lin[efptMASS] = (double) dvdl_ekin;
enerd->term[F_EKIN] = trace(ekind->ekin);
}
/* ########## Long range energy information ###### */
if (bEner || bPres || bConstrain)
{
calc_dispcorr(ir, fr, box, state->lambda[efptVDW],
corr_pres, corr_vir, &prescorr, &enercorr, &dvdlcorr);
}
if (bEner)
{
enerd->term[F_DISPCORR] = enercorr;
enerd->term[F_EPOT] += enercorr;
enerd->term[F_DVDL_VDW] += dvdlcorr;
}
/* ########## Now pressure ############## */
if (bPres || bConstrain)
{
m_add(force_vir, shake_vir, total_vir);
/* Calculate pressure and apply LR correction if PPPM is used.
* Use the box from last timestep since we already called update().
*/
enerd->term[F_PRES] = calc_pres(fr->ePBC, ir->nwall, box, ekind->ekin, total_vir, pres);
/* Calculate long range corrections to pressure and energy */
/* this adds to enerd->term[F_PRES] and enerd->term[F_ETOT],
and computes enerd->term[F_DISPCORR]. Also modifies the
total_vir and pres tesors */
m_add(total_vir, corr_vir, total_vir);
m_add(pres, corr_pres, pres);
enerd->term[F_PDISPCORR] = prescorr;
enerd->term[F_PRES] += prescorr;
}
}
// default implementation calling low-level _sys->continuous()
//--------------------------------------------------------------------------
void Omu_IntODE::syseq(double t, const VECP y, const VECP u,
VECP f)
{
#ifdef OMU_WITH_ADOLC
if (!_sys->has_low_level_continuous()) {
// call faster version if no user defined low-level continuous
syseq_forward(t, y, u, f);
return;
}
#endif
int i, j;
Omu_StateVec &xt = *_xt_ptr;
Omu_DependentVec &Ft = *_Ft_ptr;
//
// prepare call arguments
//
for (i = 0; i < _nd; i++) {
xt[i] = u[i];
}
for (i = 0; i < _n; i++) {
xt[_nd + i] = y[i];
}
for (i = 0; i < _nu; i++) {
_ut[i] = u[_nd + i];
}
if (_sa) {
for (i = _nd; i < _nxt; i++) {
for (j = 0; j < _nx; j++) {
xt.Sx[i][j] = y[(1 + j) * _n + i - _nd];
}
for (j = 0; j < _nu; j++) {
xt.Su[i][j] = y[(1 + _nx + j) * _n + i - _nd];
}
}
}
//
// evaluate residual
//
Ft.set_required_J(_sa); // an integrator may request the Jacobian
_sys->continuous(_kk, t, xt, _ut, _dxt, Ft);
for (i = _nd; i < _nxt; i++) {
// f = F * -(dF/ddx)^(-1)
f[i - _nd] = Ft[i] / -Ft.Jdx[i][i];
}
if (_sa) {
m_mlt(Ft.Jx, xt.Sx, _Yx);
m_mlt(Ft.Jx, xt.Su, _Yu);
m_add(_Yu, Ft.Ju, _Yu);
for (i = _nd; i < _nxt; i++) {
for (j = 0; j < _nx; j++) {
f[(1 + j) * _n + i - _nd] = _Yx[i][j] / -Ft.Jdx[i][i];
}
for (j = 0; j < _nu; j++) {
f[(1 + _nx + j) * _n + i - _nd] = _Yu[i][j] / -Ft.Jdx[i][i];
}
}
}
_res_evals++;
if (_sa)
_sen_evals++;
}
void do_force(FILE *fplog,t_commrec *cr,
t_inputrec *inputrec,
int step,t_nrnb *nrnb,gmx_wallcycle_t wcycle,
gmx_localtop_t *top,
gmx_groups_t *groups,
matrix box,rvec x[],history_t *hist,
rvec f[],rvec buf[],
tensor vir_force,
t_mdatoms *mdatoms,
gmx_enerdata_t *enerd,t_fcdata *fcd,
real lambda,t_graph *graph,
t_forcerec *fr,gmx_vsite_t *vsite,rvec mu_tot,
real t,FILE *field,gmx_edsam_t ed,
int flags)
{
static rvec box_size;
int cg0,cg1,i,j;
int start,homenr;
static double mu[2*DIM];
rvec mu_tot_AB[2];
bool bSepDVDL,bStateChanged,bNS,bFillGrid,bCalcCGCM,bBS,bDoForces;
matrix boxs;
real e,v,dvdl;
t_pbc pbc;
float cycles_ppdpme,cycles_pme,cycles_force;
start = mdatoms->start;
homenr = mdatoms->homenr;
bSepDVDL = (fr->bSepDVDL && do_per_step(step,inputrec->nstlog));
clear_mat(vir_force);
if (PARTDECOMP(cr)) {
pd_cg_range(cr,&cg0,&cg1);
} else {
cg0 = 0;
if (DOMAINDECOMP(cr))
cg1 = cr->dd->ncg_tot;
else
cg1 = top->cgs.nr;
if (fr->n_tpi > 0)
cg1--;
}
bStateChanged = (flags & GMX_FORCE_STATECHANGED);
bNS = (flags & GMX_FORCE_NS);
bFillGrid = (bNS && bStateChanged);
bCalcCGCM = (bFillGrid && !DOMAINDECOMP(cr));
bDoForces = (flags & GMX_FORCE_FORCES);
if (bStateChanged) {
update_forcerec(fplog,fr,box);
/* Calculate total (local) dipole moment in a temporary common array.
* This makes it possible to sum them over nodes faster.
*/
calc_mu(start,homenr,
x,mdatoms->chargeA,mdatoms->chargeB,mdatoms->nChargePerturbed,
mu,mu+DIM);
}
if (fr->ePBC != epbcNONE) {
/* Compute shift vectors every step,
* because of pressure coupling or box deformation!
*/
if (DYNAMIC_BOX(*inputrec) && bStateChanged)
calc_shifts(box,fr->shift_vec);
if (bCalcCGCM) {
put_charge_groups_in_box(fplog,cg0,cg1,fr->ePBC,box,
&(top->cgs),x,fr->cg_cm);
inc_nrnb(nrnb,eNR_CGCM,homenr);
inc_nrnb(nrnb,eNR_RESETX,cg1-cg0);
}
else if (EI_ENERGY_MINIMIZATION(inputrec->eI) && graph) {
unshift_self(graph,box,x);
}
}
else if (bCalcCGCM) {
calc_cgcm(fplog,cg0,cg1,&(top->cgs),x,fr->cg_cm);
inc_nrnb(nrnb,eNR_CGCM,homenr);
}
if (bCalcCGCM) {
if (PAR(cr)) {
move_cgcm(fplog,cr,fr->cg_cm);
}
if (gmx_debug_at)
pr_rvecs(debug,0,"cgcm",fr->cg_cm,top->cgs.nr);
}
#ifdef GMX_MPI
if (!(cr->duty & DUTY_PME)) {
/* Send particle coordinates to the pme nodes.
* Since this is only implemented for domain decomposition
* and domain decomposition does not use the graph,
* we do not need to worry about shifting.
*/
wallcycle_start(wcycle,ewcPP_PMESENDX);
GMX_MPE_LOG(ev_send_coordinates_start);
bBS = (inputrec->nwall == 2);
if (bBS) {
copy_mat(box,boxs);
svmul(inputrec->wall_ewald_zfac,boxs[ZZ],boxs[ZZ]);
}
gmx_pme_send_x(cr,bBS ? boxs : box,x,mdatoms->nChargePerturbed,lambda);
GMX_MPE_LOG(ev_send_coordinates_finish);
wallcycle_stop(wcycle,ewcPP_PMESENDX);
}
#endif /* GMX_MPI */
/* Communicate coordinates and sum dipole if necessary */
if (PAR(cr)) {
wallcycle_start(wcycle,ewcMOVEX);
if (DOMAINDECOMP(cr)) {
dd_move_x(cr->dd,box,x,buf);
} else {
move_x(fplog,cr,GMX_LEFT,GMX_RIGHT,x,nrnb);
}
/* When we don't need the total dipole we sum it in global_stat */
if (NEED_MUTOT(*inputrec))
gmx_sumd(2*DIM,mu,cr);
wallcycle_stop(wcycle,ewcMOVEX);
}
for(i=0; i<2; i++)
for(j=0;j<DIM;j++)
mu_tot_AB[i][j] = mu[i*DIM + j];
if (fr->efep == efepNO)
copy_rvec(mu_tot_AB[0],mu_tot);
else
for(j=0; j<DIM; j++)
mu_tot[j] = (1.0 - lambda)*mu_tot_AB[0][j] + lambda*mu_tot_AB[1][j];
/* Reset energies */
reset_energies(&(inputrec->opts),fr,bNS,enerd,MASTER(cr));
if (bNS) {
wallcycle_start(wcycle,ewcNS);
if (graph && bStateChanged)
/* Calculate intramolecular shift vectors to make molecules whole */
mk_mshift(fplog,graph,fr->ePBC,box,x);
/* Reset long range forces if necessary */
if (fr->bTwinRange) {
clear_rvecs(fr->f_twin_n,fr->f_twin);
clear_rvecs(SHIFTS,fr->fshift_twin);
}
/* Do the actual neighbour searching and if twin range electrostatics
* also do the calculation of long range forces and energies.
*/
dvdl = 0;
ns(fplog,fr,x,f,box,groups,&(inputrec->opts),top,mdatoms,
cr,nrnb,step,lambda,&dvdl,&enerd->grpp,bFillGrid,bDoForces);
if (bSepDVDL)
fprintf(fplog,sepdvdlformat,"LR non-bonded",0,dvdl);
enerd->dvdl_lr = dvdl;
enerd->term[F_DVDL] += dvdl;
wallcycle_stop(wcycle,ewcNS);
}
if (DOMAINDECOMP(cr)) {
if (!(cr->duty & DUTY_PME)) {
wallcycle_start(wcycle,ewcPPDURINGPME);
dd_force_flop_start(cr->dd,nrnb);
}
}
/* Start the force cycle counter.
* This counter is stopped in do_forcelow_level.
* No parallel communication should occur while this counter is running,
* since that will interfere with the dynamic load balancing.
*/
wallcycle_start(wcycle,ewcFORCE);
if (bDoForces) {
/* Reset PME/Ewald forces if necessary */
if (fr->bF_NoVirSum)
{
GMX_BARRIER(cr->mpi_comm_mygroup);
if (fr->bDomDec)
clear_rvecs(fr->f_novirsum_n,fr->f_novirsum);
else
clear_rvecs(homenr,fr->f_novirsum+start);
GMX_BARRIER(cr->mpi_comm_mygroup);
}
/* Copy long range forces into normal buffers */
if (fr->bTwinRange) {
for(i=0; i<fr->f_twin_n; i++)
copy_rvec(fr->f_twin[i],f[i]);
for(i=0; i<SHIFTS; i++)
copy_rvec(fr->fshift_twin[i],fr->fshift[i]);
}
else {
if (DOMAINDECOMP(cr))
clear_rvecs(cr->dd->nat_tot,f);
else
clear_rvecs(mdatoms->nr,f);
clear_rvecs(SHIFTS,fr->fshift);
}
clear_rvec(fr->vir_diag_posres);
GMX_BARRIER(cr->mpi_comm_mygroup);
}
if (inputrec->ePull == epullCONSTRAINT)
clear_pull_forces(inputrec->pull);
/* update QMMMrec, if necessary */
if(fr->bQMMM)
update_QMMMrec(cr,fr,x,mdatoms,box,top);
if ((flags & GMX_FORCE_BONDED) && top->idef.il[F_POSRES].nr > 0) {
/* Position restraints always require full pbc */
set_pbc(&pbc,inputrec->ePBC,box);
v = posres(top->idef.il[F_POSRES].nr,top->idef.il[F_POSRES].iatoms,
top->idef.iparams_posres,
(const rvec*)x,fr->f_novirsum,fr->vir_diag_posres,
inputrec->ePBC==epbcNONE ? NULL : &pbc,lambda,&dvdl,
fr->rc_scaling,fr->ePBC,fr->posres_com,fr->posres_comB);
if (bSepDVDL) {
fprintf(fplog,sepdvdlformat,
interaction_function[F_POSRES].longname,v,dvdl);
}
enerd->term[F_POSRES] += v;
enerd->term[F_DVDL] += dvdl;
inc_nrnb(nrnb,eNR_POSRES,top->idef.il[F_POSRES].nr/2);
}
/* Compute the bonded and non-bonded forces */
do_force_lowlevel(fplog,step,fr,inputrec,&(top->idef),
cr,nrnb,wcycle,mdatoms,&(inputrec->opts),
x,hist,f,enerd,fcd,box,lambda,graph,&(top->excls),mu_tot_AB,
flags,&cycles_force);
GMX_BARRIER(cr->mpi_comm_mygroup);
if (ed) {
do_flood(fplog,cr,x,f,ed,box,step);
}
if (DOMAINDECOMP(cr)) {
dd_force_flop_stop(cr->dd,nrnb);
if (wcycle)
dd_cycles_add(cr->dd,cycles_force,ddCyclF);
}
if (bDoForces) {
/* Compute forces due to electric field */
calc_f_el(MASTER(cr) ? field : NULL,
start,homenr,mdatoms->chargeA,x,f,inputrec->ex,inputrec->et,t);
/* When using PME/Ewald we compute the long range virial there.
* otherwise we do it based on long range forces from twin range
* cut-off based calculation (or not at all).
*/
/* Communicate the forces */
if (PAR(cr)) {
wallcycle_start(wcycle,ewcMOVEF);
if (DOMAINDECOMP(cr)) {
dd_move_f(cr->dd,f,buf,fr->fshift);
/* Position restraint do not introduce inter-cg forces */
if (EEL_FULL(fr->eeltype) && cr->dd->n_intercg_excl)
dd_move_f(cr->dd,fr->f_novirsum,buf,NULL);
} else {
move_f(fplog,cr,GMX_LEFT,GMX_RIGHT,f,buf,nrnb);
}
wallcycle_stop(wcycle,ewcMOVEF);
}
}
if (bDoForces) {
if (vsite) {
wallcycle_start(wcycle,ewcVSITESPREAD);
spread_vsite_f(fplog,vsite,x,f,fr->fshift,nrnb,
&top->idef,fr->ePBC,fr->bMolPBC,graph,box,cr);
wallcycle_stop(wcycle,ewcVSITESPREAD);
}
/* Calculation of the virial must be done after vsites! */
calc_virial(fplog,mdatoms->start,mdatoms->homenr,x,f,
vir_force,graph,box,nrnb,fr,inputrec->ePBC);
}
if (inputrec->ePull == epullUMBRELLA || inputrec->ePull == epullCONST_F) {
/* Calculate the center of mass forces, this requires communication,
* which is why pull_potential is called close to other communication.
* The virial contribution is calculated directly,
* which is why we call pull_potential after calc_virial.
*/
set_pbc(&pbc,inputrec->ePBC,box);
dvdl = 0;
enerd->term[F_COM_PULL] =
pull_potential(inputrec->ePull,inputrec->pull,mdatoms,&pbc,
cr,t,lambda,x,f,vir_force,&dvdl);
if (bSepDVDL)
fprintf(fplog,sepdvdlformat,"Com pull",enerd->term[F_COM_PULL],dvdl);
enerd->term[F_DVDL] += dvdl;
}
if (!(cr->duty & DUTY_PME)) {
cycles_ppdpme = wallcycle_stop(wcycle,ewcPPDURINGPME);
dd_cycles_add(cr->dd,cycles_ppdpme,ddCyclPPduringPME);
}
#ifdef GMX_MPI
if (PAR(cr) && !(cr->duty & DUTY_PME)) {
/* In case of node-splitting, the PP nodes receive the long-range
* forces, virial and energy from the PME nodes here.
*/
wallcycle_start(wcycle,ewcPP_PMEWAITRECVF);
dvdl = 0;
gmx_pme_receive_f(cr,fr->f_novirsum,fr->vir_el_recip,&e,&dvdl,
&cycles_pme);
if (bSepDVDL)
fprintf(fplog,sepdvdlformat,"PME mesh",e,dvdl);
enerd->term[F_COUL_RECIP] += e;
enerd->term[F_DVDL] += dvdl;
if (wcycle)
dd_cycles_add(cr->dd,cycles_pme,ddCyclPME);
wallcycle_stop(wcycle,ewcPP_PMEWAITRECVF);
}
#endif
if (bDoForces && fr->bF_NoVirSum) {
if (vsite) {
/* Spread the mesh force on virtual sites to the other particles...
* This is parallellized. MPI communication is performed
* if the constructing atoms aren't local.
*/
wallcycle_start(wcycle,ewcVSITESPREAD);
spread_vsite_f(fplog,vsite,x,fr->f_novirsum,NULL,nrnb,
&top->idef,fr->ePBC,fr->bMolPBC,graph,box,cr);
wallcycle_stop(wcycle,ewcVSITESPREAD);
}
/* Now add the forces, this is local */
if (fr->bDomDec) {
sum_forces(0,fr->f_novirsum_n,f,fr->f_novirsum);
} else {
sum_forces(start,start+homenr,f,fr->f_novirsum);
}
if (EEL_FULL(fr->eeltype)) {
/* Add the mesh contribution to the virial */
m_add(vir_force,fr->vir_el_recip,vir_force);
}
if (debug)
pr_rvecs(debug,0,"vir_force",vir_force,DIM);
}
/* Sum the potential energy terms from group contributions */
sum_epot(&(inputrec->opts),enerd);
if (fr->print_force >= 0 && bDoForces)
print_large_forces(stderr,mdatoms,cr,step,fr->print_force,x,f);
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
gmx_bool constrain_lincs(FILE *fplog,gmx_bool bLog,gmx_bool bEner,
t_inputrec *ir,
gmx_large_int_t step,
struct gmx_lincsdata *lincsd,t_mdatoms *md,
t_commrec *cr,
rvec *x,rvec *xprime,rvec *min_proj,
matrix box,t_pbc *pbc,
real lambda,real *dvdlambda,
real invdt,rvec *v,
gmx_bool bCalcVir,tensor vir_r_m_dr,
int econq,
t_nrnb *nrnb,
int maxwarn,int *warncount)
{
char buf[STRLEN],buf2[22],buf3[STRLEN];
int i,warn,p_imax,error;
real ncons_loc,p_ssd,p_max=0;
rvec dx;
gmx_bool bOK;
bOK = TRUE;
if (lincsd->nc == 0 && cr->dd == NULL)
{
if (bLog || bEner)
{
lincsd->rmsd_data[0] = 0;
if (ir->eI == eiSD2 && v == NULL)
{
i = 2;
}
else
{
i = 1;
}
lincsd->rmsd_data[i] = 0;
}
return bOK;
}
if (econq == econqCoord)
{
if (ir->efep != efepNO)
{
if (md->nMassPerturbed && lincsd->matlam != md->lambda)
{
set_lincs_matrix(lincsd,md->invmass,md->lambda);
}
for(i=0; i<lincsd->nc; i++)
{
lincsd->bllen[i] = lincsd->bllen0[i] + lambda*lincsd->ddist[i];
}
}
if (lincsd->ncg_flex)
{
/* Set the flexible constraint lengths to the old lengths */
if (pbc != NULL)
{
for(i=0; i<lincsd->nc; i++)
{
if (lincsd->bllen[i] == 0) {
pbc_dx_aiuc(pbc,x[lincsd->bla[2*i]],x[lincsd->bla[2*i+1]],dx);
lincsd->bllen[i] = norm(dx);
}
}
}
else
{
for(i=0; i<lincsd->nc; i++)
{
if (lincsd->bllen[i] == 0)
{
lincsd->bllen[i] =
sqrt(distance2(x[lincsd->bla[2*i]],
x[lincsd->bla[2*i+1]]));
}
}
}
}
if (bLog && fplog)
{
cconerr(cr->dd,lincsd->nc,lincsd->bla,lincsd->bllen,xprime,pbc,
&ncons_loc,&p_ssd,&p_max,&p_imax);
}
/* This warn var can be updated by multiple threads
* at the same time. But as we only need to detect
* if a warning occured or not, this is not an issue.
*/
warn = -1;
/* The OpenMP parallel region of constrain_lincs for coords */
#pragma omp parallel num_threads(lincsd->nth)
{
int th=gmx_omp_get_thread_num();
clear_mat(lincsd->th[th].vir_r_m_dr);
do_lincs(x,xprime,box,pbc,lincsd,th,
md->invmass,cr,
bCalcVir || (ir->efep != efepNO),
ir->LincsWarnAngle,&warn,
invdt,v,bCalcVir,
th==0 ? vir_r_m_dr : lincsd->th[th].vir_r_m_dr);
}
if (ir->efep != efepNO)
{
real dt_2,dvdl=0;
dt_2 = 1.0/(ir->delta_t*ir->delta_t);
for(i=0; (i<lincsd->nc); i++)
{
dvdl -= lincsd->mlambda[i]*dt_2*lincsd->ddist[i];
}
*dvdlambda += dvdl;
}
if (bLog && fplog && lincsd->nc > 0)
{
fprintf(fplog," Rel. Constraint Deviation: RMS MAX between atoms\n");
fprintf(fplog," Before LINCS %.6f %.6f %6d %6d\n",
sqrt(p_ssd/ncons_loc),p_max,
ddglatnr(cr->dd,lincsd->bla[2*p_imax]),
ddglatnr(cr->dd,lincsd->bla[2*p_imax+1]));
}
if (bLog || bEner)
{
cconerr(cr->dd,lincsd->nc,lincsd->bla,lincsd->bllen,xprime,pbc,
&ncons_loc,&p_ssd,&p_max,&p_imax);
/* Check if we are doing the second part of SD */
if (ir->eI == eiSD2 && v == NULL)
{
i = 2;
}
else
{
i = 1;
}
lincsd->rmsd_data[0] = ncons_loc;
lincsd->rmsd_data[i] = p_ssd;
}
else
{
lincsd->rmsd_data[0] = 0;
lincsd->rmsd_data[1] = 0;
lincsd->rmsd_data[2] = 0;
}
if (bLog && fplog && lincsd->nc > 0)
{
fprintf(fplog,
" After LINCS %.6f %.6f %6d %6d\n\n",
sqrt(p_ssd/ncons_loc),p_max,
ddglatnr(cr->dd,lincsd->bla[2*p_imax]),
ddglatnr(cr->dd,lincsd->bla[2*p_imax+1]));
}
if (warn >= 0)
{
if (maxwarn >= 0)
{
cconerr(cr->dd,lincsd->nc,lincsd->bla,lincsd->bllen,xprime,pbc,
&ncons_loc,&p_ssd,&p_max,&p_imax);
if (MULTISIM(cr))
{
sprintf(buf3," in simulation %d", cr->ms->sim);
}
else
{
buf3[0] = 0;
}
sprintf(buf,"\nStep %s, time %g (ps) LINCS WARNING%s\n"
"relative constraint deviation after LINCS:\n"
"rms %.6f, max %.6f (between atoms %d and %d)\n",
gmx_step_str(step,buf2),ir->init_t+step*ir->delta_t,
buf3,
sqrt(p_ssd/ncons_loc),p_max,
ddglatnr(cr->dd,lincsd->bla[2*p_imax]),
ddglatnr(cr->dd,lincsd->bla[2*p_imax+1]));
if (fplog)
{
fprintf(fplog,"%s",buf);
}
fprintf(stderr,"%s",buf);
lincs_warning(fplog,cr->dd,x,xprime,pbc,
lincsd->nc,lincsd->bla,lincsd->bllen,
ir->LincsWarnAngle,maxwarn,warncount);
}
bOK = (p_max < 0.5);
}
if (lincsd->ncg_flex) {
for(i=0; (i<lincsd->nc); i++)
if (lincsd->bllen0[i] == 0 && lincsd->ddist[i] == 0)
lincsd->bllen[i] = 0;
}
}
else
{
/* The OpenMP parallel region of constrain_lincs for derivatives */
#pragma omp parallel num_threads(lincsd->nth)
{
int th=gmx_omp_get_thread_num();
do_lincsp(x,xprime,min_proj,pbc,lincsd,th,
md->invmass,econq,ir->efep != efepNO ? dvdlambda : NULL,
bCalcVir,th==0 ? vir_r_m_dr : lincsd->th[th].vir_r_m_dr);
}
}
if (bCalcVir && lincsd->nth > 1)
{
for(i=1; i<lincsd->nth; i++)
{
m_add(vir_r_m_dr,lincsd->th[i].vir_r_m_dr,vir_r_m_dr);
}
}
/* count assuming nit=1 */
inc_nrnb(nrnb,eNR_LINCS,lincsd->nc);
inc_nrnb(nrnb,eNR_LINCSMAT,(2+lincsd->nOrder)*lincsd->ncc);
if (lincsd->ntriangle > 0)
{
inc_nrnb(nrnb,eNR_LINCSMAT,lincsd->nOrder*lincsd->ncc_triangle);
}
if (v)
{
inc_nrnb(nrnb,eNR_CONSTR_V,lincsd->nc*2);
}
if (bCalcVir)
{
inc_nrnb(nrnb,eNR_CONSTR_VIR,lincsd->nc);
}
return bOK;
}
int * breakout(double * ts, int n, int min_size, double beta, int degree, int *ol){
if (!ts || min_size < 5 || n < 2 * min_size || !ol){
return NULL;
}
double (*G)(double);
switch(degree){
case 1 : G = Linear;
break;
case 2 : G = Quadratic;
break;
default : G = Const;
break;
}
int * prev = (int*)malloc(sizeof(int) * (n + 1));
memset(prev, 0, sizeof(int) * (n + 1));
int * num = (int*)malloc(sizeof(int) * (n + 1));
memset(num, 0, sizeof(int) * (n + 1));
double * F = (double*)malloc(sizeof(double) * (n + 1));
memset(F, 0, sizeof(double) * (n + 1));
MTrace * m1 = m_create((CMP_FN)cmp_fn, NULL);
MTrace * m2 = m_create((CMP_FN)cmp_fn, NULL);
for (int s = 2 * min_size; s < n + 1; ++s){
m_clear(m1); m_clear(m2);
for (int i = 0; i < min_size - 1; ++i){
m_add(m1, ts + i);
}
for (int i = min_size - 1; i < s; ++i){
m_add(m2, ts + i);
}
for (int t = min_size; t < s - min_size + 1; ++t){
m_add(m1, ts + t - 1);
m_remove(m2, ts + t - 1);
if (prev[t] > prev[t - 1]){
for (int i = prev[t - 1]; i < prev[t]; ++i){
m_remove(m1, ts + i);
}
}
if (prev[t] < prev[t - 1]){
for (int i = prev[t]; i < prev[t - 1]; ++i){
m_add(m1, ts + i);
}
}
double lm = *(double*)get_median(m1);
double rm = *(double*)get_median(m2);
double normalize = ((t - prev[t]) * (s - t)) \
/ ((double)(s - prev[t]) * (s - prev[t]));
double tmp_s = F[t] + normalize * (lm - rm) * (lm - rm) - beta * G(num[t]);
if (tmp_s > F[s]){
num[s] = num[t] + 1;
F[s] = tmp_s;
prev[s] = t;
}
}
}
int k = num[n];
*ol = k;
int * re = (int*)malloc(sizeof(int) * k);
memset(re, 0, sizeof(int) * k);
int i = n;
while(i > 0){
if (prev[i])
re[--k] = prev[i];
i = prev[i];
}
free(prev); prev = NULL;
free(num); num = NULL;
free(F); F = NULL;
m_free(m1); m1 = NULL;
m_free(m2); m2 = NULL;
return re;
}
/**
* ahrs_step() takes the values from the IMU and produces the
* new estimated attitude.
*/
void
ahrs_step(
v_t angles_out,
f_t dt,
f_t p,
f_t q,
f_t r,
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
m_t C;
m_t A;
m_t AT;
v_t X_dot;
m_t temp;
v_t Xm;
v_t Xe;
/* Construct the quaternion omega matrix */
rotate2omega( A, p, q, r );
/* X_dot = AX */
mv_mult( X_dot, A, X, 4, 4 );
/* X += X_dot dt */
v_scale( X_dot, X_dot, dt, 4 );
v_add( X, X_dot, X, 4 );
v_normq( X, X );
/*
printf( "X =" );
Vprint( X, 4 );
printf( "\n" );
*/
/* AT = transpose(A) */
m_transpose( AT, A, 4, 4 );
/* P = A * P * AT + Q */
m_mult( temp, A, P, 4, 4, 4 );
m_mult( P, temp, AT, 4, 4, 4 );
m_add( P, Q, P, 4, 4 );
compute_c( C, X );
/* Compute the euler angles of the measured attitude */
accel2euler(
Xm,
ax,
ay,
az,
heading
);
/*
* Compute the euler angles of the estimated attitude
* Xe = quat2euler( X )
*/
quat2euler( Xe, X );
/* Kalman filter update */
kalman_update(
P,
R,
C,
X,
Xe,
Xm
);
/* Estimated attitude extraction */
quat2euler( angles_out, X );
}
void
m_add_msgid(struct mproc *m, uint32_t v)
{
m_add(m, &v, sizeof(v));
}
void
m_add_sockaddr(struct mproc *m, const struct sockaddr *sa)
{
m_add_size(m, sa->sa_len);
m_add(m, sa, sa->sa_len);
}
void
m_add_int(struct mproc *m, int v)
{
m_add(m, &v, sizeof(v));
};
// Veh_Integral
void Vehicle::Update()
{
double wind_dir, hdng, rand_wind=0.0;
double headrot[4];
double senspostang[2], vehsenspos[2];
double chemDensity;
Point3D vpos;
double u_wind[3];
int i_v,iii,jjj, j;
static int detect_yet;
double Sim_Time;
if(ed == NULL) return; // if no enviromental data then return
Sim_Time = ed->GetSimTime();
// save last pos
GetPos(last_p);
// Simulate the batch of vehicles
for(i_v = 0; i_v < NUM_VEH; i_v++)
{
if(!(veh[i_v].src_fnd_flg)) // if not done
{
// following are the lines where the vehicle simulation interfaces
// with the wind and concentration sensor models. The wind speed
// and concentration can be evaluated at any position of interest
hdng = veh[i_v].omega;
headrot[0] = cos(hdng);
headrot[1] = -sin(hdng);
headrot[2] = sin(hdng);
headrot[3] = cos(hdng);
double sensposbody[2];
for(j = 0; j < NUM_SENS; j++)
{
sensposbody[0] = 0;
sensposbody[1] = 0; // (-ns+j)*lngth/ns;
m_mul(senspostang, headrot, sensposbody, 2, 2, 1);
m_add(vehsenspos, &(veh[i_v].x), senspostang, 2, 1);
//veh[i_v].snsd[j] = chem->ReadConc(vehsenspos, u_wind);
{
vpos.x = vehsenspos[0];
vpos.y = vehsenspos[1];
vpos.z = 0.0; // vehsenspos[2]
ed->ApplyWind(vpos, u_wind);
chemDensity = ed->GetChemDensity(vpos);
veh[i_v].snsd[j] = chemDensity;
vehsenspos[0] = vpos.x;
vehsenspos[1] = vpos.y;
//vehsenspos[2] = vpos.z;
}
}
if (i_v==0) rand_wind = 5.0*nrand();
wind_dir = atan2(u_wind[1],u_wind[0])*180/PI + rand_wind;
// following is the call to the strategy
strat->in[0] = veh[i_v].x;
strat->in[1] = (-1)*veh[i_v].y; // invert because of axis flip
strat->in[2] = sqrt(u_wind[0]*u_wind[0] + u_wind[1]*u_wind[1]);
strat->in[3] = wind_dir;
strat->in[4] = Sim_Time;
strat->in[5] = veh[i_v].u;
strat->in[6] = veh[i_v].omega;
// added by JES. copy global to fit older code
double src_loc[2];
src_loc[0] = srcloc.x;
src_loc[1] = srcloc.y;
//
for(j = 7; j < 7 + NUM_SENS; j++)
{
strat->in[j] = veh[i_v].snsd[j-7];
}
strat->out[0]= veh[i_v].vel_c; // Initialize output values
strat->out[1]= veh[i_v].omega_c; // Initialize output values
strat->out[2]= veh[i_v].src_fnd_flg; // Initialize output values
//-------------------------Call Strategy----------------------------------------------
MyStratFunc(strat->in,
strat->out,
&(strat->mem[N_STRAT_MEM*i_v]),
dt,
radius,
threshold,
Sim_Time,
currentBehaviorName
);
//-------------------------Return from strategy----------------------------------------------
veh[i_v].vel_c = strat->out[0];
veh[i_v].omega_c = strat->out[1];
// The source found flag once set is set for the run
veh[i_v].src_fnd_flg = ((strat->out[2])||(veh[i_v].src_fnd_flg));
x_left=0.0;
y_top=-50;
d_x=1;
d_y=1;
if( veh[i_v].snsd[0] > threshold)
{
veh[i_v].det_tm = Sim_Time; // store most recent detection time
if(veh[i_v].plm_find_tm > Sim_Time)
veh[i_v].plm_find_tm = Sim_Time; // record time of first detection
}
if( ( hdist(&(veh[i_v].x), src_loc) < radius) && // near the source
( veh[i_v].src_find_tm > Sim_Time) ) // for the first time
{
veh[i_v].src_find_tm = Sim_Time; // store the frst tm near src
fnd_cnt++;
} // if near source
if( (veh[i_v].src_fnd_flg) &&
(veh[i_v].src_dclr_tm > Sim_Time) )
{
// Graphics --------------------------------------------
if( (batch_not == 1) &&
(i_v ==0 ) )
{
for(iii = 0; iii < GRID_X; iii++)
{
for(jjj = 0; jjj < GRID_Y; jjj++)
{
bck_grnd[iii][jjj] = (float)(strat->mem[1000+iii*100+jjj]);
}
}
// RESET!
veh[i_v].x = vehicle_pos_x; // ((double)random(100))*(x_max-x_min)/100.0 + x_min;
veh[i_v].y = vehicle_pos_y; // ((double)random(100))*(y_max-y_min)/100.0 + y_min;
veh[i_v].z = vehicle_pos_z; // ((double)random(100))*(z_max-z_min)/100.0 + z_min;
veh[i_v].omega_c = nrand()*180.0;
veh[i_v].dsd = 0.0;
veh[i_v].dsf = 0.0;
veh[i_v].dpf = 0.0;
veh[i_v].det_tm = -100;
veh[i_v].plm_find_tm = 1e8;
veh[i_v].src_find_tm = 1e8;
veh[i_v].src_dclr_tm = 1e8;
veh[i_v].src_dclr_d = 0.0;
veh[i_v].snsd[NS + 1] = 0;
veh[i_v].src_fnd_flg = 0;
}
// --------------------------------------------
else
{
veh[i_v].src_dclr_tm = Sim_Time; // store tm of first src found flag
veh[i_v].src_dclr_d = hdist(&(veh[i_v].x), src_loc);
veh[i_v].x_real=veh[i_v].x;
veh[i_v].y_real=veh[i_v].y;
veh[i_v].vel_c = 0; // stop this vehicle
done_cnt++; // veh is done
}
} // if src fnd flag
vehicle_model(&(veh[i_v]), Sim_Time); // advance vehicle to next time
//Graphics --------------------------------------------
if((batch_not==1)&&(i_v==0))
{ // Have we detected anything this interval
detect_yet = ( (veh[i_v].snsd[0] > threshold) || (detect_yet) );
veh_plot_cnt++; // track interval
if(veh_plot_cnt >= VEH_MAX_PLT_CNT)
{
veh_plot_cnt = 0;
if(vh_count < VH_M)
{
det_flag[vh_count] = detect_yet;
detect_yet = 0;
veh_omega_flag[vh_count] = veh[0].omega*PI/180.0;
vh_x[vh_count] = veh[0].x;
vh_y[vh_count] = veh[0].y;
vh_count++;
}
else
{
vehicle_update();
det_flag[VH_M-1] = detect_yet;
detect_yet = 0;
veh_omega_flag[VH_M-1] = veh[0].omega*PI/180.0;
vh_x[VH_M-1] = veh[0].x;
vh_y[VH_M-1] = veh[0].y;
}
} // veh_plot_cnt
} // not batch
// --------------------------------------------
} // if not vehicle done
} // for each vehicle
init_flag = 0;
}
void
m_add_u32(struct mproc *m, uint32_t u32)
{
m_add(m, &u32, sizeof(u32));
};
/* This function takes a pointer to a block of memory and inserts it into
* the chain of memory chunks
*/
Static void __insert_chunk(mem * mem_chunk)
{
register mem *p1, *p2;
if (chunk_list == 0) { /* Simple case first */
m_next(mem_chunk) = chunk_list = mem_chunk;
noise("FIRST CHUNK", mem_chunk);
return;
}
p1 = mem_chunk;
p2 = chunk_list;
do {
if (p1 > p2) {
/* We're at the top of the chain, p1 is higher */
if (m_next(p2) <= p2) {
if (m_add(p2, m_size(p2)) == p1) {
/* Good, stick 'em together */
noise("INSERT CHUNK", mem_chunk);
m_size(p2) += m_size(p1);
noise("JOIN 1", p2);
}
else {
m_next(p1) = m_next(p2);
m_next(p2) = p1;
noise("INSERT CHUNK", mem_chunk);
noise("FROM", p2);
}
return;
}
if (m_next(p2) > p1) {
/* In chain, p1 between p2 and next */
m_next(p1) = m_next(p2);
m_next(p2) = p1;
noise("INSERT CHUNK", mem_chunk);
noise("FROM", p2);
/* Try to join above */
if (m_add(p1, m_size(p1)) == m_next(p1)) {
m_size(p1) += m_size(m_next(p1));
m_next(p1) = m_next(m_next(p1));
noise("JOIN 2", p1);
}
/* Try to join below */
if (m_add(p2, m_size(p2)) == p1) {
m_size(p2) += m_size(p1);
m_next(p2) = m_next(p1);
noise("JOIN 3", p2);
}
chunk_list = p2; /* Make sure it's valid */
return;
}
}
else if (p1 < p2) {
if (m_next(p2) <= p2 && p1 < m_next(p2)) {
/* At top of chain, next is bottom of chain,
* p1 is below next
*/
m_next(p1) = m_next(p2);
m_next(p2) = p1;
noise("INSERT CHUNK", mem_chunk);
noise("FROM", p2);
chunk_list = p2;
if (m_add(p1, m_size(p1)) == m_next(p1)) {
if (p2 == m_next(p1))
chunk_list = p1;
m_size(p1) += m_size(m_next(p1));
m_next(p1) = m_next(m_next(p1));
noise("JOIN 4", p1);
}
return;
}
}
chunk_list = p2; /* Save for search */
p2 = m_next(p2);
} while (p2 != chunk_list);
/* If we get here we have a problem, ignore it, maybe it'll go away */
noise("DROPPED CHUNK", mem_chunk);
}
void
m_add_size(struct mproc *m, size_t sz)
{
m_add(m, &sz, sizeof(sz));
};
void
m_add_time(struct mproc *m, time_t v)
{
m_add(m, &v, sizeof(v));
};
void
m_add_timeval(struct mproc *m, struct timeval *tv)
{
m_add(m, tv, sizeof(*tv));
}
static void do_my_pme(FILE *fp,real tm,gmx_bool bVerbose,t_inputrec *ir,
rvec x[],rvec xbuf[],rvec f[],
real charge[],real qbuf[],real qqbuf[],
matrix box,gmx_bool bSort,
t_commrec *cr,t_nsborder *nsb,t_nrnb *nrnb,
t_block *excl,real qtot,
t_forcerec *fr,int index[],FILE *fp_xvg,
int ngroups,unsigned short cENER[])
{
real ener,vcorr,q,xx,dvdl=0,vdip,vcharge;
tensor vir,vir_corr,vir_tot;
rvec mu_tot[2];
int i,m,ii,ig,jg;
real **epme,*qptr;
/* Initiate local variables */
fr->f_el_recip = f;
clear_mat(vir);
clear_mat(vir_corr);
if (ngroups > 1) {
fprintf(fp,"There are %d energy groups\n",ngroups);
snew(epme,ngroups);
for(i=0; (i<ngroups); i++)
snew(epme[i],ngroups);
}
/* Put x is in the box, this part needs to be parallellized properly */
/*put_atoms_in_box(box,nsb->natoms,x);*/
/* Here sorting of X (and q) is done.
* Alternatively, one could just put the atoms in one of the
* cr->nnodes slabs. That is much cheaper than sorting.
*/
for(i=0; (i<nsb->natoms); i++)
index[i] = i;
if (bSort) {
xptr = x;
qsort(index,nsb->natoms,sizeof(index[0]),comp_xptr);
xptr = NULL; /* To trap unintentional use of the ptr */
}
/* After sorting we only need the part that is to be computed on
* this processor. We also compute the mu_tot here (system dipole)
*/
clear_rvec(mu_tot[0]);
for(i=START(nsb); (i<START(nsb)+HOMENR(nsb)); i++) {
ii = index[i];
q = charge[ii];
qbuf[i] = q;
for(m=0; (m<DIM); m++) {
xx = x[ii][m];
xbuf[i][m] = xx;
mu_tot[0][m] += q*xx;
}
clear_rvec(f[ii]);
}
copy_rvec(mu_tot[0],mu_tot[1]);
if (debug) {
pr_rvec(debug,0,"qbuf",qbuf,nsb->natoms,TRUE);
pr_rvecs(debug,0,"xbuf",xbuf,nsb->natoms);
pr_rvecs(debug,0,"box",box,DIM);
}
for(ig=0; (ig<ngroups); ig++) {
for(jg=ig; (jg<ngroups); jg++) {
if (ngroups > 1) {
for(i=START(nsb); (i<START(nsb)+HOMENR(nsb)); i++) {
if ((cENER[i] == ig) || (cENER[i] == jg))
qqbuf[i] = qbuf[i];
else
qqbuf[i] = 0;
}
qptr = qqbuf;
}
else
qptr = qbuf;
ener = do_pme(fp,bVerbose,ir,xbuf,f,qptr,qptr,box,cr,
nsb,nrnb,vir,fr->ewaldcoeff,FALSE,0,&dvdl,FALSE);
vcorr = ewald_LRcorrection(fp,nsb,cr,fr,qptr,qptr,excl,xbuf,box,mu_tot,
ir->ewald_geometry,ir->epsilon_surface,
0,&dvdl,&vdip,&vcharge);
gmx_sum(1,&ener,cr);
gmx_sum(1,&vcorr,cr);
if (ngroups > 1)
epme[ig][jg] = ener+vcorr;
}
}
if (ngroups > 1) {
if (fp_xvg)
fprintf(fp_xvg,"%10.3f",tm);
for(ig=0; (ig<ngroups); ig++) {
for(jg=ig; (jg<ngroups); jg++) {
if (ig != jg)
epme[ig][jg] -= epme[ig][ig]+epme[jg][jg];
if (fp_xvg)
fprintf(fp_xvg," %12.5e",epme[ig][jg]);
}
}
if (fp_xvg)
fprintf(fp_xvg,"\n");
}
else {
fprintf(fp,"Time: %10.3f Energy: %12.5e Correction: %12.5e Total: %12.5e\n",
tm,ener,vcorr,ener+vcorr);
if (fp_xvg)
fprintf(fp_xvg,"%10.3f %12.5e %12.5e %12.5e\n",tm,ener+vcorr,vdip,vcharge);
if (bVerbose) {
m_add(vir,vir_corr,vir_tot);
gmx_sum(9,vir_tot[0],cr);
pr_rvecs(fp,0,"virial",vir_tot,DIM);
}
fflush(fp);
}
}
void
m_add_data(struct mproc *m, const void *v, size_t len)
{
m_add_size(m, len);
m_add(m, v, len);
};
real sum_ekin(t_grpopts *opts, gmx_ekindata_t *ekind, real *dekindlambda,
gmx_bool bEkinAveVel, gmx_bool bSaveEkinOld, gmx_bool bScaleEkin)
{
int i, j, m, ngtc;
real T, ek;
t_grp_tcstat *tcstat;
real nrdf, nd, *ndf;
ngtc = opts->ngtc;
ndf = opts->nrdf;
T = 0;
nrdf = 0;
clear_mat(ekind->ekin);
for (i = 0; (i < ngtc); i++)
{
nd = ndf[i];
tcstat = &ekind->tcstat[i];
/* Sometimes a group does not have degrees of freedom, e.g.
* when it consists of shells and virtual sites, then we just
* set the temperatue to 0 and also neglect the kinetic
* energy, which should be zero anyway.
*/
if (nd > 0)
{
if (bEkinAveVel)
{
if (!bScaleEkin)
{
/* in this case, kinetic energy is from the current velocities already */
msmul(tcstat->ekinf, tcstat->ekinscalef_nhc, tcstat->ekinf);
}
}
else
{
/* Calculate the full step Ekin as the average of the half steps */
for (j = 0; (j < DIM); j++)
{
for (m = 0; (m < DIM); m++)
{
tcstat->ekinf[j][m] =
0.5*(tcstat->ekinh[j][m]*tcstat->ekinscaleh_nhc + tcstat->ekinh_old[j][m]);
}
}
}
m_add(tcstat->ekinf, ekind->ekin, ekind->ekin);
tcstat->Th = calc_temp(trace(tcstat->ekinh), nd);
tcstat->T = calc_temp(trace(tcstat->ekinf), nd);
/* after the scaling factors have been multiplied in, we can remove them */
if (bEkinAveVel)
{
tcstat->ekinscalef_nhc = 1.0;
}
else
{
tcstat->ekinscaleh_nhc = 1.0;
}
}
else
{
tcstat->T = 0;
tcstat->Th = 0;
}
T += nd*tcstat->T;
nrdf += nd;
}
if (nrdf > 0)
{
T /= nrdf;
}
if (dekindlambda)
{
*dekindlambda = 0.5*(ekind->dekindl + ekind->dekindl_old);
}
return T;
}
void
m_add_evpid(struct mproc *m, uint64_t v)
{
m_add(m, &v, sizeof(v));
}
iterator<Pruner> insert(Feature const& interval, int32_t coverage=0, Pruner const& prune=Pruner())
{
m_type = LT_UNADD;
iterator<Pruner> newFeature = iterator<Pruner>(prune, m_color, m_add(interval, m_bottom, prune));
switch(m_type)
{
case LT_UNADD:
{
break;
}
case LT_ERROR:
{
// throw exception
break;
}
case LT_GENERATOR:
{
// add coverage
break;
}
case LT_CANDIDATE:
{
iterator<Pruner> tmp = newFeature;
newFeature = insert((*newFeature).label() + interval, 0, prune);
Node<Feature>::iterator edge = (*tmp).begin(), last = (*tmp).end();
while(edge != last)
{
(*newFeature).orphan(*tmp);
if(&(*edge).target() != &(*newFeature))
{
bool found = false;
Node<Feature>::iterator child = (*newFeature).begin(), final = (*newFeature).end();
while(child != final)
{
if(&(*edge).target() == &(*child).target())
{
found = true;
break;
}
++child;
}
if(!found)
{
(*newFeature).adopt((*edge).target());
}
}
(*tmp).orphan(edge);
++edge;
}
//delete &(*tmp);
break;
}
case LT_NEW:
{
// return newFeature
break;
}
default:
{
std::cerr << "unknown m_type" << std::endl;
}
}
real sum_ekin(bool bFirstStep,
t_grpopts *opts,gmx_ekindata_t *ekind,
tensor ekin,real *dekindlambda)
{
int i,j,m,ngtc;
real T,ek;
t_grp_tcstat *tcstat;
real nrdf,nd,*ndf;
ngtc = opts->ngtc;
ndf = opts->nrdf;
clear_mat(ekin);
T = 0;
nrdf = 0;
for(i=0; (i<ngtc); i++) {
tcstat = &ekind->tcstat[i];
nd = ndf[i];
/* Sometimes a group does not have degrees of freedom, e.g.
* when it consists of shells and virtual sites, then we just
* set the temperatue to 0 and also neglect the kinetic
* energy, which should be zero anyway.
*/
if (nd > 0) {
if (bFirstStep) {
/* This Ekin is only used for reporting the initial temperature
* or when doing mdrun -rerun.
*/
copy_mat(tcstat->ekinh,tcstat->ekin);
} else {
/* Calculate the full step Ekin as the average of the half steps */
for(j=0; (j<DIM); j++)
for(m=0; (m<DIM); m++)
tcstat->ekin[j][m] =
0.5*(tcstat->ekinh[j][m] + tcstat->ekinh_old[j][m]);
}
m_add(tcstat->ekin,ekin,ekin);
ek = 0;
for(m=0; (m<DIM); m++)
ek += tcstat->ekinh[m][m];
tcstat->Th = calc_temp(ek,nd);
ek = 0;
for(m=0; (m<DIM); m++)
ek += tcstat->ekin[m][m];
tcstat->T = calc_temp(ek,nd);
}
else {
tcstat->T = 0;
tcstat->Th = 0;
}
T += nd*tcstat->T;
nrdf += nd;
}
if (nrdf > 0)
T/=nrdf;
if (dekindlambda)
*dekindlambda = 0.5*(ekind->dekindl + ekind->dekindl_old);
return T;
}
