/* mat(),imat(),zeros(),eye() */
void utest1(void)
{
int i,j,n=100,m=200,*b;
double *a;
a=mat(0,0); assert(a==NULL);
a=mat(0,1); assert(a==NULL);
a=mat(1,0); assert(a==NULL);
a=mat(1,1); assert(a!=NULL);
free(a);
/* a=mat(1000000,1000000); assert(a==NULL); */
a=zeros(0,m); assert(a==NULL);
a=zeros(n,0); assert(a==NULL);
a=zeros(n,m); assert(a!=NULL);
for (i=0;i<n;i++) for (j=0;j<m;j++) assert(a[i+j*n]==0.0);
free(a);
/* a=zeros(10000000,1000000); assert(a==NULL); */
a=eye(0); assert(a==NULL);
a=eye(n); assert(a!=NULL);
for (i=0;i<n;i++) for (j=0;j<n;j++) assert(a[i+j*n]==(i==j?1.0:0.0));
free(a);
/* a=eye(1000000); assert(a==NULL); */
b=imat(0,m); assert(b==NULL);
b=imat(n,0); assert(b==NULL);
b=imat(n,m); assert(b!=NULL);
free(b);
/*
a=imat(1000000,1000000); assert(a==NULL);
*/
printf("%s utest1 : OK\n",__FILE__);
}
/* resolve integer ambiguity for ppp -----------------------------------------*/
extern int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
const double *azel)
{
double elmask;
int i,j,m=0,stat=0,*NW,*sat1,*sat2;
if (n<=0||rtk->opt.ionoopt!=IONOOPT_IFLC||rtk->opt.nf<2) return 0;
trace(3,"pppamb: time=%s n=%d\n",time_str(obs[0].time,0),n);
elmask=rtk->opt.elmaskar>0.0?rtk->opt.elmaskar:rtk->opt.elmin;
sat1=imat(n*n,1);
sat2=imat(n*n,1);
NW=imat(n*n,1);
/* average LC */
average_LC(rtk,obs,n,nav,azel);
/* fix wide-lane ambiguity */
for (i=0; i<n-1; i++) for (j=i+1; j<n; j++) {
if (!rtk->ssat[obs[i].sat-1].vsat[0]||
!rtk->ssat[obs[j].sat-1].vsat[0]||
azel[1+i*2]<elmask||azel[1+j*2]<elmask) continue;
#if 0
/* test already fixed */
if (rtk->ambc[obs[i].sat-1].flags[obs[j].sat-1]&&
rtk->ambc[obs[j].sat-1].flags[obs[i].sat-1]) continue;
#endif
sat1[m]=obs[i].sat;
sat2[m]=obs[j].sat;
if (fix_amb_WL(rtk,nav,sat1[m],sat2[m],NW+m)) m++;
}
/* fix narrow-lane ambiguity */
if (rtk->opt.modear==ARMODE_PPPAR) {
stat=fix_amb_ROUND(rtk,sat1,sat2,NW,m);
}
else if (rtk->opt.modear==ARMODE_PPPAR_ILS) {
stat=fix_amb_ILS(rtk,sat1,sat2,NW,m);
}
free(sat1);
free(sat2);
free(NW);
return stat;
}
//------------------------------------------------------------------------------
OrbitalEquation::OrbitalEquation(Config *cfg,
vector<bitset<BITS> > slaterDeterminants,
Interaction *V,
SingleParticleOperator *h):
cfg(cfg)
{
this->slaterDeterminants = slaterDeterminants;
this->V = V;
this->h = h;
try {
nParticles = cfg->lookup("system.nParticles");
nGrid = cfg->lookup("spatialDiscretization.nGrid");
nOrbitals = cfg->lookup("spatialDiscretization.nSpatialOrbitals");
} catch (const SettingNotFoundException &nfex) {
cerr << "OrbitalEquation::Error reading from config object." << endl;
exit(EXIT_FAILURE);
}
nSlaterDeterminants = slaterDeterminants.size();
invRho = cx_mat(nOrbitals, nOrbitals);
U = zeros<cx_mat>(nGrid, nOrbitals);
rightHandSide = zeros<cx_mat>(nGrid, nOrbitals);
// Q = cx_mat(nGrid, nGrid);
rho1 = zeros<cx_mat>(2*nOrbitals, 2*nOrbitals); // TMP
myRank = 0;
nNodes = 1;
#ifdef USE_MPI
// MPI-------------------------------------------
MPI_Comm_rank(MPI_COMM_WORLD, &myRank);
MPI_Comm_size(MPI_COMM_WORLD, &nNodes);
#endif
sizeRij = ivec(nNodes);
int tot = nGrid*nOrbitals;
allRH = imat(nGrid, nOrbitals);
int node = 0;
int s = 0;
for (int j = 0; j < nOrbitals; j++) {
for (int i = 0; i < nGrid; i++) {
if (myRank == node){
myRij.push_back(pair<int, int>(i,j));
}
allRH(i,j) = node;
s++;
if(s >= BLOCK_SIZE(node, nNodes, tot)){
sizeRij(node) = BLOCK_SIZE(node, nNodes, tot);
s = 0;
node++;
}
}
}
}
void homographyToPoseCV(at::real fx, at::real fy,
at::real tagSize,
const at::Mat& horig,
cv::Mat& rvec,
cv::Mat& tvec) {
at::Point3 opoints[4] = {
at::Point3(-1, -1, 0),
at::Point3( 1, -1, 0),
at::Point3( 1, 1, 0),
at::Point3(-1, 1, 0)
};
at::Point ipoints[4];
at::real s = 0.5*tagSize;
for (int i=0; i<4; ++i) {
ipoints[i] = project(horig, opoints[i].x, opoints[i].y);
opoints[i] *= s;
}
at::real Kdata[9] = {
fx, 0, 0,
0, fy, 0,
0, 0, 1
};
at::Mat Kmat(3, 3, Kdata);
at::Mat dcoeffs = at::Mat::zeros(4, 1);
cv::Mat_<at::Point3> omat(4, 1, opoints);
cv::Mat_<at::Point> imat(4, 1, ipoints);
cv::Mat r, t;
cv::solvePnP(omat, imat, Kmat, dcoeffs, r, t);
if (rvec.type() == CV_32F) {
r.convertTo(rvec, rvec.type());
} else {
rvec = r;
}
if (tvec.type() == CV_32F) {
t.convertTo(tvec, tvec.type());
} else {
tvec = t;
}
}
void out_imatrix (header *hd)
/***** out_matrix
print a complex matrix.
*****/
{ int c,r,i,j,c0,cend;
double *m,*x;
getmatrix(hd,&r,&c,&m);
for (c0=0; c0<c; c0+=ilinew)
{ cend=c0+ilinew-1;
if (cend>=c) cend=c-1;
if (c>ilinew) output2("Column %d to %d:\n",c0+1,cend+1);
for (i=0; i<r; i++)
{ x=imat(m,c,i,c0);
for (j=c0; j<=cend; j++) { interval_out(*x,*(x+1));
x+=2; }
output("\n");
if (test_key()==escape) return;
}
}
}
bool FlyingVehicleData::preload(bool server, String &errorStr)
{
if (!Parent::preload(server, errorStr))
return false;
TSShapeInstance* si = new TSShapeInstance(mShape, false);
// Resolve objects transmitted from server
if (!server) {
for (S32 i = 0; i < MaxSounds; i++)
if (sound[i])
Sim::findObject(SimObjectId(sound[i]),sound[i]);
for (S32 j = 0; j < MaxJetEmitters; j++)
if (jetEmitter[j])
Sim::findObject(SimObjectId(jetEmitter[j]),jetEmitter[j]);
}
// Extract collision planes from shape collision detail level
if (collisionDetails[0] != -1)
{
MatrixF imat(1);
PlaneExtractorPolyList polyList;
polyList.mPlaneList = &rigidBody.mPlaneList;
polyList.setTransform(&imat, Point3F(1,1,1));
si->animate(collisionDetails[0]);
si->buildPolyList(&polyList,collisionDetails[0]);
}
// Resolve jet nodes
for (S32 j = 0; j < MaxJetNodes; j++)
jetNode[j] = mShape->findNode(sJetNode[j]);
//
maxSpeed = maneuveringForce / minDrag;
delete si;
return true;
}
void coxph_reg::estimate(const coxph_data &cdatain,
const int model,
const std::vector<std::string> &modelNames,
const int interaction, const int ngpreds,
const bool iscox, const int nullmodel,
const mlinfo &snpinfo, const int cursnp)
{
coxph_data cdata = cdatain.get_unmasked_data();
mematrix<double> X = t_apply_model(cdata.X, model, interaction, ngpreds,
iscox, nullmodel);
int length_beta = X.nrow;
beta.reinit(length_beta, 1);
sebeta.reinit(length_beta, 1);
mematrix<double> newoffset = cdata.offset -
(cdata.offset).column_mean(0);
mematrix<double> means(X.nrow, 1);
for (int i = 0; i < X.nrow; i++)
{
beta[i] = 0.;
}
mematrix<double> u(X.nrow, 1);
mematrix<double> imat(X.nrow, X.nrow);
double *work = new double[X.ncol * 2 +
2 * (X.nrow) * (X.nrow) +
3 * (X.nrow)];
double loglik_int[2];
int flag;
// Use Efron method of handling ties (for Breslow: 0.0), like in
// R's coxph()
double sctest = 1.0;
// Set the maximum number of iterations that coxfit2() will run to
// the default value from the class definition.
int maxiterinput = MAXITER;
// Make separate variables epsinput and tolcholinput that are not
// const to send to coxfit2(), this way we won't have to alter
// that function (which is a good thing: we want to keep it as
// pristine as possible because it is copied from the R survival
// package).
double epsinput = EPS;
double tolcholinput = CHOLTOL;
coxfit2(&maxiterinput, &cdata.nids, &X.nrow, cdata.stime.data.data(),
cdata.sstat.data.data(), X.data.data(), newoffset.data.data(),
cdata.weights.data.data(), cdata.strata.data.data(),
means.data.data(), beta.data.data(), u.data.data(),
imat.data.data(), loglik_int, &flag, work, &epsinput,
&tolcholinput, &sctest);
// After coxfit2() maxiterinput contains the actual number of
// iterations that were used. Store it in niter.
niter = maxiterinput;
// Check the results of the Cox fit; mirrored from the same checks
// in coxph.fit.S and coxph.R from the R survival package.
// A vector to indicate for which covariates the betas/se_betas
// should be set to NAN.
std::vector<bool> setToNAN = std::vector<bool>(X.nrow, false);
// Based on coxph.fit.S lines with 'which.sing' and coxph.R line
// with if(any(is.NA(coefficients))). These lines set coefficients
// to NA if flag < nvar (with nvar = ncol(x)) and MAXITER >
// 0. coxph.R then checks for any NAs in the coefficients and
// outputs the warning message if NAs were found.
if (flag < X.nrow)
{
int which_sing = 0;
MatrixXd imateigen = imat.data;
VectorXd imatdiag = imateigen.diagonal();
for (int i = 0; i < imatdiag.size(); i++)
{
if (imatdiag[i] == 0)
{
which_sing = i;
setToNAN[which_sing] = true;
if (i != 0) {
// Don't warn for i=0 to exclude the beta
// coefficient for the (constant) mean from the
// check. For Cox regression the constant terms
// are ignored. However, we leave it in the
// calculations because otherwise the null model
// calculation will fail in case there are no
// other covariates than the SNP.
std::cerr << "Warning for " << snpinfo.name[cursnp]
<< ", model " << modelNames[model]
<< ": X matrix deemed to be singular (variable "
<< which_sing + 1 << ")" << std::endl;
}
}
}
}
if (niter >= MAXITER)
{
cerr << "Warning for " << snpinfo.name[cursnp]
<< ", model " << modelNames[model]
<< ": nr of iterations > the maximum (" << MAXITER << "): "
<< niter << endl;
}
if (flag == 1000)
{
cerr << "Warning for " << snpinfo.name[cursnp]
<< ", model " << modelNames[model]
<< ": Cox regression ran out of iterations and did not converge,"
<< " setting beta and se to 'NaN'\n";
std::fill(setToNAN.begin(), setToNAN.end(), true);
} else {
VectorXd ueigen = u.data;
MatrixXd imateigen = imat.data;
VectorXd infs = ueigen.transpose() * imateigen;
infs = infs.cwiseAbs();
VectorXd betaeigen = beta.data;
assert(betaeigen.size() == infs.size());
// We check the beta's for all coefficients
// (incl. covariates), maybe stick to only checking the SNP
// coefficient?
for (int i = 0; i < infs.size(); i++) {
if (infs[i] > EPS &&
infs[i] > sqrt(EPS) * abs(betaeigen[i])) {
setToNAN[i] = true;
cerr << "Warning for " << snpinfo.name[cursnp]
<< ", model " << modelNames[model]
<< ": beta for covariate " << i + 1 << " may be infinite,"
<< " setting beta and se to 'NaN'\n";
}
}
}
for (int i = 0; i < X.nrow; i++)
{
if (setToNAN[i])
{
// Cox regression failed somewhere, set results to NAN for
// this X row (covariate or SNP)
sebeta[i] = NAN;
beta[i] = NAN;
loglik = NAN;
} else {
sebeta[i] = sqrt(imat.get(i, i));
loglik = loglik_int[1];
}
}
delete[] work;
}
/* raim fde (failure detection and exclution) -------------------------------*/
static int raim_fde(const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
double *azel, int *vsat, double *resp, char *msg)
{
obsd_t *obs_e;
sol_t sol_e={{0}};
char tstr[32],name[16],msg_e[128];
double *rs_e,*dts_e,*vare_e,*azel_e,*resp_e,rms_e,rms=100.0;
int i,j,k,nvsat,stat=0,*svh_e,*vsat_e,sat=0;
trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n);
if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0;
rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n);
svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n);
for (i=0;i<n;i++) {
/* satellite exclution */
for (j=k=0;j<n;j++) {
if (j==i) continue;
obs_e[k]=obs[j];
matcpy(rs_e +6*k,rs +6*j,6,1);
matcpy(dts_e+2*k,dts+2*j,2,1);
vare_e[k]=vare[j];
svh_e[k++]=svh[j];
}
/* estimate receiver position without a satellite */
if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,&sol_e,azel_e,
vsat_e,resp_e,msg_e)) {
trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);
continue;
}
for (j=nvsat=0,rms_e=0.0;j<n-1;j++) {
if (!vsat_e[j]) continue;
rms_e+=SQR(resp_e[j]);
nvsat++;
}
if (nvsat<5) {
trace(3,"raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",
obs[i].sat,nvsat);
continue;
}
rms_e=sqrt(rms_e/nvsat);
trace(3,"raim_fde: exsat=%2d rms=%8.3f\n",obs[i].sat,rms_e);
if (rms_e>rms) continue;
/* save result */
for (j=k=0;j<n;j++) {
if (j==i) continue;
matcpy(azel+2*j,azel_e+2*k,2,1);
vsat[j]=vsat_e[k];
resp[j]=resp_e[k++];
}
stat=1;
*sol=sol_e;
sat=obs[i].sat;
rms=rms_e;
vsat[i]=0;
strcpy(msg,msg_e);
}
if (stat) {
time2str(obs[0].time,tstr,2); satno2id(sat,name);
trace(2,"%s: %s excluded by raim\n",tstr+11,name);
}
free(obs_e);
free(rs_e ); free(dts_e ); free(vare_e); free(azel_e);
free(svh_e); free(vsat_e); free(resp_e);
return stat;
}
void inline instrumentert::instrument_minimum_interference_inserter(
const std::set<event_grapht::critical_cyclet> &set_of_cycles)
{
/* Idea:
We solve this by a linear programming approach,
using for instance glpk lib.
Input: the edges to instrument E, the cycles C_j
Pb: min sum_{e_i in E} d(e_i).x_i
s.t. for all j, sum_{e_i in C_j} >= 1,
where e_i is a pair to potentially instrument,
x_i is a Boolean stating whether we instrument
e_i, and d() is the cost of an instrumentation.
Output: the x_i, saying which pairs to instrument
For this instrumentation, we propose:
d(poW*)=1
d(poRW)=d(rfe)=2
d(poRR)=3
This function can be refined with the actual times
we get in experimenting the different pairs in a
single IRIW.
*/
#ifdef HAVE_GLPK
/* first, identify all the unsafe pairs */
std::set<event_grapht::critical_cyclet::delayt> edges;
for(std::set<event_grapht::critical_cyclet>::iterator
C_j=set_of_cycles.begin();
C_j!=set_of_cycles.end();
++C_j)
for(std::set<event_grapht::critical_cyclet::delayt>::const_iterator e_i=
C_j->unsafe_pairs.begin();
e_i!=C_j->unsafe_pairs.end();
++e_i)
edges.insert(*e_i);
glp_prob *lp;
glp_iocp parm;
glp_init_iocp(&parm);
parm.msg_lev=GLP_MSG_OFF;
parm.presolve=GLP_ON;
lp=glp_create_prob();
glp_set_prob_name(lp, "instrumentation optimisation");
glp_set_obj_dir(lp, GLP_MIN);
message.debug() << "edges: "<<edges.size()<<" cycles:"<<set_of_cycles.size()
<< messaget::eom;
/* sets the variables and coefficients */
glp_add_cols(lp, edges.size());
std::size_t i=0;
for(std::set<event_grapht::critical_cyclet::delayt>::iterator
e_i=edges.begin();
e_i!=edges.end();
++e_i)
{
++i;
std::string name="e_"+std::to_string(i);
glp_set_col_name(lp, i, name.c_str());
glp_set_col_bnds(lp, i, GLP_LO, 0.0, 0.0);
glp_set_obj_coef(lp, i, cost(*e_i));
glp_set_col_kind(lp, i, GLP_BV);
}
/* sets the constraints (soundness): one per cycle */
glp_add_rows(lp, set_of_cycles.size());
i=0;
for(std::set<event_grapht::critical_cyclet>::iterator
C_j=set_of_cycles.begin();
C_j!=set_of_cycles.end();
++C_j)
{
++i;
std::string name="C_"+std::to_string(i);
glp_set_row_name(lp, i, name.c_str());
glp_set_row_bnds(lp, i, GLP_LO, 1.0, 0.0); /* >= 1*/
}
const std::size_t mat_size=set_of_cycles.size()*edges.size();
message.debug() << "size of the system: " << mat_size
<< messaget::eom;
std::vector<int> imat(mat_size+1);
std::vector<int> jmat(mat_size+1);
std::vector<double> vmat(mat_size+1);
/* fills the constraints coeff */
/* tables read from 1 in glpk -- first row/column ignored */
std::size_t col=1;
std::size_t row=1;
i=1;
for(std::set<event_grapht::critical_cyclet::delayt>::iterator
e_i=edges.begin();
e_i!=edges.end();
++e_i)
{
row=1;
for(std::set<event_grapht::critical_cyclet>::iterator
C_j=set_of_cycles.begin();
C_j!=set_of_cycles.end();
++C_j)
{
imat[i]=row;
jmat[i]=col;
if(C_j->unsafe_pairs.find(*e_i)!=C_j->unsafe_pairs.end())
vmat[i]=1.0;
else
vmat[i]=0.0;
++i;
++row;
}
++col;
}
#ifdef DEBUG
for(i=1; i<=mat_size; ++i)
message.statistics() <<i<<"["<<imat[i]<<","<<jmat[i]<<"]="<<vmat[i]
<< messaget::eom;
#endif
/* solves MIP by branch-and-cut */
glp_load_matrix(lp, mat_size, imat, jmat, vmat);
glp_intopt(lp, &parm);
/* loads results (x_i) */
message.statistics() << "minimal cost: " << glp_mip_obj_val(lp)
<< messaget::eom;
i=0;
for(std::set<event_grapht::critical_cyclet::delayt>::iterator
e_i=edges.begin();
e_i!=edges.end();
++e_i)
{
++i;
if(glp_mip_col_val(lp, i)>=1)
{
const abstract_eventt &first_ev=egraph[e_i->first];
var_to_instr.insert(first_ev.variable);
id2loc.insert(
std::pair<irep_idt, source_locationt>(
first_ev.variable, first_ev.source_location));
if(!e_i->is_po)
{
const abstract_eventt &second_ev=egraph[e_i->second];
var_to_instr.insert(second_ev.variable);
id2loc.insert(
std::pair<irep_idt, source_locationt>(
second_ev.variable, second_ev.source_location));
}
}
}
glp_delete_prob(lp);
#else
throw "sorry, minimum interference option requires glpk; "
"please recompile goto-instrument with glpk";
#endif
}
/* execute local file test -----------------------------------------------------
* execute local file test
* args : gtime_t ts,te I time start and end
* double tint I time interval (s)
* url_t *urls I download urls
* int nurl I number of urls
* char **stas I stations
* int nsta I number of stations
* char *dir I local directory
* int ncol I number of column
* int datefmt I date format (0:year-dow,1:year-dd/mm,2:week)
* FILE *fp IO log test result file pointer
* return : status (1:ok,0:error,-1:aborted)
*-----------------------------------------------------------------------------*/
extern void dl_test(gtime_t ts, gtime_t te, double ti, const url_t *urls,
int nurl, char **stas, int nsta, const char *dir,
int ncol, int datefmt, FILE *fp)
{
gtime_t time;
double tow;
char year[32],date[32],date_p[32];
int i,j,n,m,*nc,*nt,week,flag,abort=0;
if (ncol<1) ncol=1; else if (ncol>200) ncol=200;
fprintf(fp,"** LOCAL DATA AVAILABILITY (%s, %s) **\n\n",
time_str(timeget(),0),*dir?dir:"*");
for (i=n=0;i<nurl;i++) {
n+=strstr(urls[i].path,"%s")||strstr(urls[i].path,"%S")?nsta:1;
}
nc=imat(n,1);
nt=imat(n,1);
for (i=0;i<n;i++) nc[i]=nt[i]=0;
for (;timediff(ts,te)<1E-3&&!abort;ts=timeadd(ts,ti*ncol)) {
genpath(datefmt==0?" %Y-":"%Y/%m/","",ts,0,year);
if (datefmt<=1) fprintf(fp,"%s %s",datefmt==0?"DOY ":"DATE",year);
else fprintf(fp,"WEEK ");
*date_p='\0'; flag=0;
m=datefmt==2?1:2;
for (i=0;i<(ncol+m-1)/m;i++) {
time=timeadd(ts,ti*i*m);
if (timediff(time,te)>=1E-3) break;
if (datefmt<=1) {
genpath(datefmt==0?"%n":"%d","",time,0,date);
fprintf(fp,"%-4s",strcmp(date,date_p)?date:"");
}
else {
if (fabs(time2gpst(time,&week))<1.0) {
fprintf(fp,"%04d",week); flag=1;
}
else {
fprintf(fp,"%s",flag?"":" "); flag=0;
}
}
strcpy(date_p,date);
}
fprintf(fp,"\n");
for (i=j=0;i<nurl&&!abort;i++) {
time=timeadd(ts,ti*ncol-1.0);
if (timediff(time,te)>=0.0) time=te;
/* test local files */
abort=test_locals(ts,time,ti,urls+i,stas,nsta,dir,nc+j,nt+j,fp);
j+=strstr(urls[i].path,"%s")||strstr(urls[i].path,"%S")?nsta:1;
}
fprintf(fp,"\n");
}
fprintf(fp,"# COUNT : FILES/TOTAL\n");
for (i=j=0;i<nurl;i++) {
j+=print_total(urls+i,stas,nsta,nc+j,nt+j,fp);
}
free(nc); free(nt);
}
