int
main (int argc, char **argv)
{
FILE *ofile;
int nlambda = 0;
int i, m;
double zn, zvar, tw, tail;
double *xx[0], *lambda;
readcommands (argc, argv);
settwxtable (twxtab);
if (oname == NULL)
ofile = stdout;
else
openit (oname, &ofile, "w");
if (iname == NULL)
fatalx ("i paraameter compulsory\n");
nlambda = numlines (iname);
ZALLOC(lambda, nlambda, double);
xx[0] = lambda;
nlambda = getxx (xx, nlambda, 1, iname);
vst (lambda, lambda, -1.0, nlambda);
sortit (lambda, NULL, nlambda);
vst (lambda, lambda, -1.0, nlambda);
m = numgtz (lambda, nlambda);
fprintf (ofile, "%4s %12s", "#N", "eigenvalue");
fprintf (ofile, "%12s", "difference");
fprintf (ofile, " %9s %12s", "twstat", "p-value");
fprintf (ofile, " %9s", "effect. n");
fprintf (ofile, "\n");
for (i = 0; i < m; ++i)
{
zn = nval;
tail = dotwcalc (lambda + i, m - i, &tw, &zn, &zvar, minleneig);
fprintf (ofile, "%4d %12.6f", i + 1, lambda[i]);
if (i == 0)
fprintf (ofile, "%12s", "NA");
else
fprintf (ofile, "%12.6f", lambda[i] - lambda[i - 1]);
if (tail >= 0.0)
fprintf (ofile, " %9.3f %12.6g", tw, tail);
else
fprintf (ofile, " %9s %12s", "NA", "NA");
if (zn > 0.0)
{
fprintf (ofile, " %9.3f", zn);
}
else
{
fprintf (ofile, " %9s", "NA");
}
fprintf (ofile, "\n");
}
return 0;
}
Real NSMomentumViscousFlux::computeQpResidual()
{
// Yay for less typing!
const RealTensorValue & vst = _viscous_stress_tensor[_qp];
// _component'th column of vst...
RealVectorValue vec(vst(0,_component),
vst(1,_component),
vst(2,_component));
// ... dotted with grad(phi), note: sign is positive as this term was -div(tau) on the lhs
return vec*_grad_test[_i][_qp];
}
void eigvals(double *mat, double *evals, int n)
{
double *pmat ;
int len ;
len = n*(n+1) ; len /= 2 ;
ZALLOC(pmat, len, double) ;
vst(mat, mat, -1.0, n*n) ;
packsym(pmat, mat, n) ;
eigx_(pmat, evals, &n) ;
free(pmat) ;
vst(mat, mat, -1.0, n*n) ;
vst(evals, evals, -1.0, n) ;
}
int Ng_SetPrimitiveData (ClientData clientData,
Tcl_Interp * interp,
int argc, tcl_const char *argv[])
{
CSGeometry * geometry = dynamic_cast<CSGeometry*> (ng_geometry.get());
if (!geometry)
{
Tcl_SetResult (interp, err_needscsgeometry, TCL_STATIC);
return TCL_ERROR;
}
tcl_const char * name = argv[1];
tcl_const char * value = argv[2];
Array<double> coeffs;
cout << "Set primitive data, name = " << name
<< ", value = " << value << endl;
istringstream vst (value);
double val;
while (!vst.eof())
{
vst >> val;
coeffs.Append (val);
}
((Primitive*)
geometry->GetSolid (name)->GetPrimitive())->SetPrimitiveData (coeffs);
return TCL_OK;
}
int Ng_SetSolidData (ClientData clientData,
Tcl_Interp * interp,
int argc, tcl_const char *argv[])
{
CSGeometry * geometry = dynamic_cast<CSGeometry*> (ng_geometry.Ptr());
if (!geometry)
{
Tcl_SetResult (interp, err_needscsgeometry, TCL_STATIC);
return TCL_ERROR;
}
tcl_const char * name = argv[1];
tcl_const char * val = argv[2];
cout << "Set Solid Data, name = " << name
<< ", value = " << val << endl;
istringstream vst (val);
Solid * nsol = Solid::CreateSolid (vst, geometry->GetSolids());
geometry->SetSolid (name, nsol);
return TCL_OK;
}
void
getcolxz(double *xcol, SNP *cupt, int *xindex, int nrows, int col,
double *xmean, double *xfancy, int *n0, int *n1)
// side effect set xmean xfancy and count variant and reference alleles
{
int j, n, g ;
double pmean, yfancy ;
int *rawcol ;
int c0, c1 ;
c0 = c1 = 0 ;
ZALLOC(rawcol, nrows, int) ;
n = cupt -> ngtypes ;
if (n<nrows) fatalx("bad snp: %s %d\n", cupt -> ID, n) ;
getrawcol(rawcol, cupt, xindex, nrows) ;
for (j=0; j<nrows; ++j) {
g = rawcol[j] ;
if (g<0) continue ;
c0 += g ;
c1 += 2-g ;
}
floatit(xcol, rawcol, nrows) ;
fvadjust(xcol, nrows, &pmean, &yfancy) ;
vst(xcol, xcol, yfancy, nrows) ;
if (xmean != NULL) {
xmean[col] = pmean*yfancy ;
xfancy[col] = yfancy ;
}
free(rawcol) ;
*n0 = c0 ;
*n1 = c1 ;
}
void
getcolxf(double *xcol, SNP *cupt, int *xindex, int nrows, int col,
double *xmean, double *xfancy)
// side effect set xmean xfancy
{
int n ;
double pmean, yfancy ;
int *rawcol ;
if (xmean != NULL) {
xmean[col] = xfancy[col] = 0.0 ;
}
if (cupt -> ignore) {
vzero(xcol, nrows) ;
return ;
}
ZALLOC(rawcol, nrows, int) ;
n = cupt -> ngtypes ;
if (n<nrows) fatalx("bad snp: %s %d\n", cupt -> ID, n) ;
getrawcol(rawcol, cupt, xindex, nrows) ;
floatit(xcol, rawcol, nrows) ;
fvadjust(xcol, nrows, &pmean, &yfancy) ;
vst(xcol, xcol, yfancy, nrows) ;
if (xmean != NULL) {
xmean[col] = pmean*yfancy ;
xfancy[col] = yfancy ;
}
free(rawcol) ;
}
int main(int argc, char **argv)
{
if (argc != 2) {
fprintf(stderr, "%s [mct|vst]\n", argv[0]);
exit(1);
}
if (!FIPS_mode_set(1)) {
do_print_errors();
exit(1);
}
FIPS_rand_reset();
if (!FIPS_rand_test_mode()) {
fprintf(stderr, "Error setting PRNG test mode\n");
do_print_errors();
exit(1);
}
if (!strcmp(argv[1], "mct"))
mct();
else if (!strcmp(argv[1], "vst"))
vst();
else {
fprintf(stderr, "Don't know how to %s.\n", argv[1]);
exit(1);
}
return 0;
}
void ransamp(int *samp, int nsamp, double *p, int plen)
/**
pick nsamp elements from random distribution
uses randis but array is at least sorted optimally
*/
{
double *px ;
int *indx ;
double y ;
int i, j, k ;
if (plen<=1) {
ivzero(samp, nsamp) ;
return ;
}
ZALLOC(px, plen, double) ;
ZALLOC(indx, plen, int) ;
y = asum(p, plen) ;
vst(px, p, -1.0/y, plen) ;
sortit(px, indx, plen) ;
vst(px, px, -1.0, plen) ;
for (i=0; i<nsamp; i++) {
/**
really need binary chop picker
*/
j = randis(px, plen) ;
if (j<0) {
for (k=0; k<plen; k++) {
printf("zz %d %d %12.6f %12.6f\n",k, indx[k], p[k], px[k]) ;
}
fatalx("bad ransamp\n") ;
}
k = indx[j] ;
samp[i] = k ;
}
free (px) ;
free (indx) ;
}
void
writesnpeigs(char *snpeigname, SNP **xsnplist, double *ffvecs, int numeigs, int ncols)
{
// this is called at end and ffvecs overwritten
double *xpt, y, yscal, *snpsc ;
int i, j, k, kmax, kmin ;
SNP * cupt ;
FILE *fff ;
for (j=0; j<numeigs; ++j) {
xpt = ffvecs+j*ncols ;
y = asum2(xpt, ncols) ;
yscal = (double) ncols / y ;
yscal = sqrt(yscal) ;
vst(xpt, xpt, yscal, ncols) ;
}
ZALLOC(snpsc, ncols, double) ;
vclear(snpsc, -99999, ncols) ;
for (j=0; j<numeigs; ++j) {
for (i=0; i<ncols; ++i) {
cupt = xsnplist[i] ;
if (cupt -> ignore) continue ;
y = ffvecs[j*ncols+i] ;
snpsc[i] = fabs(y) ;
}
for (k=0; k<=10; ++k) {
vlmaxmin(snpsc, ncols, &kmax, &kmin) ;
cupt = xsnplist[kmax] ;
printf("eigbestsnp %4d %20s %2d %12d %9.3f\n", j+1, cupt -> ID, cupt -> chrom, nnint(cupt -> physpos), snpsc[kmax]) ;
snpsc[kmax] = -1.0 ;
}
}
free(snpsc) ;
if (snpeigname == NULL) return ;
openit (snpeigname, &fff, "w") ;
for (i=0; i<ncols; ++i) {
cupt = xsnplist[i] ;
if (cupt -> ignore) continue ;
fprintf(fff, "%20s", cupt -> ID) ;
fprintf(fff, " %2d", cupt -> chrom) ;
fprintf(fff, " %12d", nnint(cupt -> physpos)) ;
for (j=0; j<numeigs; ++j) {
fprintf(fff, " %9.3f", ffvecs[j*ncols+i]) ;
}
fprintf(fff, "\n") ;
}
fclose(fff) ;
}
double bal1 (double *a, int n)
// WARNING a is input and output
{
double y ;
y = asum(a, n) ;
if (y<=0.0) fatalx("bad bal1\n") ;
vst(a, a, 1.0/y, n) ;
return y ;
}
void bal(double *a, double *b, int n)
/**
normalize mean 0 s.d 1
*/
{
double t ;
t = asum(b,n)/ (double) n ;
vsp (a, b, -t, n) ;
t = asum2(a,n)/ (double) n ;
vst (a, a, 1.0/sqrt(t), n) ;
}
int main(int argc, char** argv)
{
// Check if video source has been passed as a parameter
ros::init(argc, argv, "vision_st_driver_node");
ros::NodeHandle nh;
VisionSTCam vst(nh);
// boost::thread ctrl_thread(boost::bind(&VisionSTCam::ControlIntensity, &vst));
// boost::thread grab_thread = boost::thread(boost::bind(&VisionSTCam::process, &vst));
vst.process();
// while (nh.ok()) {
// }
ros::spinOnce();
}
void
ldreg (double *ldmat, double *ldmat2, double *vv, double *vv2, double *ldvv,
double *ldvv2, int rsize, int n)
/** ldmat2 is inner product matrix for last rsize columns on exit */
{
int i, j, k1, k2;
double *rr, *ans, *tt;
double y;
ZALLOC(rr, rsize, double);
ZALLOC(ans, rsize, double);
ZALLOC(tt, n, double);
if (rsize > 1)
copyarr (ldvv, ldvv2 + n, n * (rsize - 1));
for (i = 0; i < rsize - 1; i++)
{
for (j = 0; j < rsize - 1; j++)
{
k1 = i * rsize + j;
k2 = (i + 1) * rsize + j + 1;
ldmat2[k2] = ldmat[k1];
}
}
copyarr (vv, ldvv2, n);
i = 0;
for (j = 0; j < rsize; j++)
{
y = rr[j] = vdot (vv, ldvv + j * n, n);
y = vdot (vv, ldvv2 + j * n, n);
if (j == 0)
y += 1.0e-6;
ldmat2[i * rsize + j] = ldmat2[j * rsize + i] = y;
}
solvit (ldmat, rr, rsize, ans); /* solve normal equations */
copyarr (vv, vv2, n);
for (i = 0; i < rsize; i++)
{
vst (tt, ldvv + i * n, -ans[i], n);
vvp (vv2, vv2, tt, n);
}
free (rr);
free (ans);
free (tt);
}
double
dotwcalc(double *lambda, int m, double *ptw, double *pzn, double *pzvar, int minm)
{
double nv, mv, tzn, tm ;
double *evals ;
double y, top, bot, zn, tw, ystat ;
double tail, lsum ;
if (m<minm) {
*pzn = *pzvar = *ptw = -1 ;
return -1.0 ;
}
lsum = asum(lambda, m) ;
if (lsum<=0.0) {
*pzn = *pzvar = *ptw = -1 ;
return -1.0 ;
}
tzn = *pzn ;
tm = (double) m ;
y = (double) m / lsum ;
ystat = lambda[0] * y * tzn ;
if (tzn>0.0) {
tw = twnorm(ystat, tm, tzn) ;
*pzn = tzn ;
*ptw = tw ;
tail = twtail(tw) ;
return tail ;
}
ZALLOC(evals, m, double) ;
vst(evals, lambda, y, m) ;
top = (double) (m*(m+2)) ;
bot = asum2(evals, m) - (double) m ;
zn = top/bot ; // see appendix to eigenpaper NJP
y = evals[0]*zn ;
tw = twnorm(y, tm, zn) ;
*pzn = zn ;
*ptw = tw ;
tail = twtail(tw) ;
free(evals) ;
return tail ;
}
std::pair<std::size_t, OutputIterator>
biconnected_components(const Graph& g, ComponentMap comp, OutputIterator out,
VertexIndexMap index_map)
{
typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
typedef typename graph_traits<Graph>::vertices_size_type
vertices_size_type;
std::vector<vertices_size_type> discover_time(num_vertices(g));
std::vector<vertices_size_type> lowpt(num_vertices(g));
vertices_size_type vst(0);
return biconnected_components
(g, comp, out,
make_iterator_property_map(discover_time.begin(), index_map, vst),
make_iterator_property_map(lowpt.begin(), index_map, vst),
index_map);
}
double chitest(double *a, double *p, int n)
/* a is n boxes. Goodness of fit test to p */
{
double *x, *b, *pp ;
double y1=0.0, y2=0.0 ;
int i ;
ZALLOC(pp, n, double) ;
if (p != NULL)
copyarr(p,pp,n) ;
else
vclear(pp, 1.0, n) ;
y1 = asum(pp,n) ;
y2 = asum(a,n) ;
if ( (y1==0.0) || (y2==0.0) ) {
free(pp) ;
return 0.0 ;
}
ZALLOC(x,n,double) ;
ZALLOC(b,n,double) ;
vst (x, pp, y2/y1, n) ; /* expected */
vsp (x, x, .0001, n) ;
vvm (b, a, x, n) ;
vvt (b, b, b, n) ;
vvd (b, b, x, n) ;
y1 = asum(b,n) ;
free(x) ;
free(b) ;
return y1 ;
}
void KernelHandle::createVirtualStigmergy(unsigned int id)
{
std::string robot_id_string=boost::lexical_cast<std::string>(KernelInitializer::unique_robot_id_);
std::string shm_object_name="KernelData"+robot_id_string;
std::string mutex_name="named_kernel_mtx"+robot_id_string;
boost::interprocess::named_mutex named_kernel_mtx(boost::interprocess::open_or_create, mutex_name.data());
boost::interprocess::managed_shared_memory segment(boost::interprocess::open_or_create ,shm_object_name.data(), 102400);
std::string id_string=boost::lexical_cast<std::string>(id);
std::string virtual_stigmergy_name="shm_virtual_stigmergy_tuple_"+id_string;
const char *p=virtual_stigmergy_name.data();
VoidAllocator alloc_inst (segment.get_segment_manager());
std::pair<shm_virtual_stigmergy_type*, std::size_t> virtual_stigmergy = segment.find<shm_virtual_stigmergy_type>("shm_virtual_stigmergy_");
if(virtual_stigmergy.first==0)
{
return;
}
shm_virtual_stigmergy_type *vst_pointer=virtual_stigmergy.first;
shm_virtual_stigmergy_type::iterator vst_it;
vst_it=vst_pointer->find(id);
if(vst_it!=vst_pointer->end())
{
return;
}
else
{
shm_virtual_stigmergy_tuple_type *tmp=segment.construct<shm_virtual_stigmergy_tuple_type>(p)(std::less<shm_string>(), alloc_inst);
VirtualStigmergyType vst(id, *tmp);
named_kernel_mtx.lock();
vst_pointer->insert(vst);
named_kernel_mtx.unlock();
}
}
double hwstat(double *x)
/** Hardy-Weinberg equilibrium test
returns standard normal in null case.
+sign is excess heterozygosity
x[0] [1] [2] are counts for homozm hetero h**o (alt allele)
*/
{
double p, q, ysum, s1, y1, y2, ychi, sig ;
double a1[3], a2[3] ;
ysum = asum(x,3) ;
if (ysum < 0.001) return 0.0 ;
s1 = 2*x[2]+x[1] ;
p = 0.5*s1/ysum;
q = 1.0-p ;
a1 [0] = q*q ;
a1 [1] = 2*p*q ;
a1 [2] = p*p ;
vsp(a1, a1, 1.0e-8, 3) ;
vst(a2, x, 1.0/ysum, 3) ;
vsp(a2, a2, 1.0e-8, 3) ;
y2 = vldot(x, a2, 3) ;
y1 = vldot(x, a1, 3) ;
ychi = 2.0*(y2-y1) ;
sig = sqrt(ychi+1.0e-8) ;
if (a2[1]<a1[1]) sig = -sig ;
/* negative => hets lo */
return sig ;
}
double doeig2(double *vals, int m, double *pzn, double *ptw)
{
static int ncall = 0 ;
double y, tw, tail ;
double zn, top, bot ;
double *evals ;
++ncall ;
ZALLOC(evals, m, double) ;
copyarr(vals, evals, m) ;
y = (double) m / asum(evals, m) ;
vst(evals, evals, y, m) ;
top = (double) (m*(m+2)) ;
bot = asum2(evals, m) - (double) m ;
zn = top/bot ;
y = evals[0]*zn ;
tw = twnorm(y, (double) m, zn) ;
tail = twtail(tw) ;
free(evals) ;
*pzn = zn ;
*ptw = tw ;
return tail ;
}
int main(int argc, char **argv)
{
char **eglist ;
int numeg ;
int i, j, k, pos;
int *vv ;
SNP *cupt, *cupt2 ;
Indiv *indx ;
double y1, y2, y ;
int n0, n1, nkill ;
int nindiv = 0 ;
int nignore, numrisks = 1 ;
SNP **xsnplist ;
Indiv **xindlist ;
int *xindex ;
int nrows, ncols, m ;
double *XTX, *cc, *evecs, *ww ;
double *lambda ;
double *tvecs ;
int weightmode = NO ;
int t ;
double *xmean, *xfancy ;
double *ldmat = NULL, *ldmat2 = NULL;
double *ldvv = NULL, *ldvv2 = NULL, *vv2 = NULL ;
int chrom, numclear ;
double gdis ;
int outliter, numoutiter, *badlist, nbad ;
int a, b, n ;
FILE *outlfile ;
int xblock, blocksize=10000 ;
double *tblock ;
OUTLINFO *outpt ;
int *idperm, *vecind ; // for sort
readcommands(argc, argv) ;
printf("## smartrel version: %s\n", WVERSION) ;
packmode = YES ;
setomode(&outputmode, omode) ;
if (parname == NULL) return 0 ;
if (xchrom == (numchrom+1)) noxdata = NO ;
if (fstonly) {
printf("fstonly\n") ;
numeigs = 0 ;
numoutliter = 0 ;
numoutiter = 0 ;
outputname = NULL ;
snpeigname = NULL ;
}
if (fancynorm) printf("norm used\n\n") ;
else printf("no norm used\n\n") ;
nostatslim = MAX(nostatslim, 3) ;
outlfile = ofile = stdout;
if (outputname != NULL) openit(outputname, &ofile, "w") ;
if (outliername != NULL) openit(outliername, &outlfile, "w") ;
if (fstdetailsname != NULL) openit(fstdetailsname, &fstdetails, "w") ;
numsnps =
getsnps(snpname, &snpmarkers, 0.0, badsnpname, &nignore, numrisks) ;
numindivs = getindivs(indivname, &indivmarkers) ;
k = getgenos(genotypename, snpmarkers, indivmarkers,
numsnps, numindivs, nignore) ;
if (poplistname != NULL)
{
ZALLOC(eglist, numindivs, char *) ;
numeg = loadlist(eglist, poplistname) ;
seteglist(indivmarkers, numindivs, poplistname);
}
else
{
setstatus(indivmarkers, numindivs, NULL) ;
ZALLOC(eglist, MAXPOPS, char *) ;
numeg = makeeglist(eglist, MAXPOPS, indivmarkers, numindivs) ;
}
for (i=0; i<numeg; i++)
{
/* printf("%3d %s\n",i, eglist[i]) ; */
}
nindiv=0 ;
for (i=0; i<numindivs; i++)
{
indx = indivmarkers[i] ;
if(indx -> affstatus == YES) ++nindiv ;
}
for (i=0; i<numsnps; i++)
{
cupt = snpmarkers[i] ;
chrom = cupt -> chrom ;
if ((noxdata) && (chrom == (numchrom+1))) cupt-> ignore = YES ;
if (chrom == 0) cupt -> ignore = YES ;
if (chrom > (numchrom+1)) cupt -> ignore = YES ;
}
for (i=0; i<numsnps; i++)
{
cupt = snpmarkers[i] ;
pos = nnint(cupt -> physpos) ;
if ((xchrom>0) && (cupt -> chrom != xchrom)) cupt -> ignore = YES ;
if ((xchrom > 0) && (pos < lopos)) cupt -> ignore = YES ;
if ((xchrom > 0) && (pos > hipos)) cupt -> ignore = YES ;
if (cupt -> ignore) continue ;
if (numvalidgtx(indivmarkers, cupt, YES) <= 1)
{
printf("nodata: %20s\n", cupt -> ID) ;
cupt -> ignore = YES ;
}
}
if (killr2) {
nkill = killhir2(snpmarkers, numsnps, numindivs, r2physlim, r2genlim, r2thresh) ;
if (nkill>0) printf("killhir2. number of snps killed: %d\n", nkill) ;
}
ZALLOC(vv, numindivs, int) ;
numvalidgtallind(vv, snpmarkers, numsnps, numindivs) ;
for (i=0; i<numindivs; ++i) {
if (vv[i] == 0) {
indx = indivmarkers[i] ;
indx -> ignore = YES ;
}
}
free(vv) ;
numsnps = rmsnps(snpmarkers, numsnps, NULL) ; // rid ignorable snps
if (missingmode)
{
setmiss(snpmarkers, numsnps) ;
fancynorm = NO ;
}
if (weightname != NULL)
{
weightmode = YES ;
getweights(weightname, snpmarkers, numsnps) ;
}
if (ldregress>0)
{
ZALLOC(ldvv, ldregress*numindivs, double) ;
ZALLOC(ldvv2, ldregress*numindivs, double) ;
ZALLOC(vv2, numindivs, double) ;
ZALLOC(ldmat, ldregress*ldregress, double) ;
ZALLOC(ldmat2, ldregress*ldregress, double) ;
setidmat(ldmat, ldregress) ;
vst(ldmat, ldmat, 1.0e-6, ldregress*ldregress) ;
}
ZALLOC(xindex, numindivs, int) ;
ZALLOC(xindlist, numindivs, Indiv *) ;
ZALLOC(xsnplist, numsnps, SNP *) ;
if (popsizelimit > 0)
{
setplimit(indivmarkers, numindivs, eglist, numeg, popsizelimit) ;
}
nrows = loadindx(xindlist, xindex, indivmarkers, numindivs) ;
ncols = loadsnpx(xsnplist, snpmarkers, numsnps, indivmarkers) ;
printf("number of samples used: %d number of snps used: %d\n", nrows, ncols) ;
/**
cupt = xsnplist[0] ;
for (j=0; j<nrows; ++j) {
k = xindex[j] ;
g = getgtypes(cupt, k) ;
indx = indivmarkers[k] ;
t = indxindex(eglist, numeg, indx -> egroup) ;
printf("yy1 %20s %20s %20s %d %d %d\n", cupt ->ID, indx -> ID, indx -> egroup, j, k, g) ;
}
printf("yya: ") ; printimat(xindex, 1, nrows) ;
printf("zzindxa: %s\n", indivmarkers[230] -> egroup) ;
*/
/* printf("## nrows: %d ncols %d\n", nrows, ncols) ; */
ZALLOC(xmean, ncols, double) ;
ZALLOC(xfancy, ncols, double) ;
ZALLOC(XTX, nrows*nrows, double) ;
ZALLOC(evecs, nrows*nrows, double) ;
ZALLOC(tvecs, nrows*nrows, double) ;
ZALLOC(lambda, nrows, double) ;
ZALLOC(cc, nrows, double) ;
ZALLOC(ww, nrows, double) ;
ZALLOC(badlist, nrows, int) ;
blocksize = MIN(blocksize, ncols) ;
ZALLOC(tblock, nrows*blocksize, double) ;
// xfancy is multiplier for column xmean is mean to take off
// badlist is list of rows to delete (outlier removal)
numoutiter = 1 ;
if (numoutliter>=1)
{
numoutiter = numoutliter+1 ;
ZALLOC(outinfo, nrows, OUTLINFO *) ;
for (k=0; k<nrows; k++)
{
ZALLOC(outinfo[k], 1, OUTLINFO) ;
}
/* fprintf(outlfile, "##%18s %4s %6s %9s\n", "ID", "iter","eigvec", "score") ; */
}
for (outliter = 1; outliter <= numoutiter ; ++outliter) {
if (fstonly) {
setidmat(XTX, nrows) ;
vclear(lambda, 1.0, nrows) ;
break ;
}
if (outliter>1) {
ncols = loadsnpx(xsnplist, snpmarkers, numsnps, indivmarkers) ;
}
vzero(XTX, nrows*nrows) ;
vzero(tblock, nrows*blocksize) ;
xblock = 0 ;
vzero(xmean, ncols) ;
vclear(xfancy, 1.0, ncols) ;
for (i=0; i<ncols; i++)
{
cupt = xsnplist[i] ;
chrom = cupt -> chrom ;
getcolxz(cc, cupt, xindex, nrows, i, xmean, xfancy, &n0, &n1) ;
t = MIN(n0, n1) ;
if (t <= minallelecnt) {
cupt -> ignore = YES ;
vzero(cc, nrows) ;
}
if (weightmode)
{
vst(cc, cc, xsnplist[i] -> weight, nrows) ;
}
if (ldregress>0)
{
numclear = 0 ;
for (k=1; k<= ldregress; ++k)
{
j = i-k ;
if (j<0)
{
numclear = ldregress-k+1 ;
break ;
}
cupt2 = xsnplist[j] ;
if (cupt2 -> chrom != chrom) gdis = ldlimit + 1.0 ;
else gdis = cupt -> genpos - cupt2 -> genpos ;
if (gdis>=ldlimit)
{
numclear = ldregress-k+1 ;
break ;
}
}
if (numclear>0) clearld(ldmat, ldvv, ldregress, nrows, numclear) ;
ldreg(ldmat, ldmat2, cc, vv2, ldvv, ldvv2, ldregress, nrows) ;
copyarr(ldmat2, ldmat, ldregress*ldregress) ;
copyarr(vv2, cc, nrows) ;
copyarr(ldvv2, ldvv, ldregress*nrows) ;
}
copyarr(cc, tblock+xblock*nrows, nrows) ;
++xblock ;
/** this is the key code to parallelize */
if (xblock==blocksize)
{
domult(tvecs, tblock, xblock, nrows) ;
vvp(XTX, XTX, tvecs, nrows*nrows) ;
xblock = 0 ;
vzero(tblock, nrows*blocksize) ;
}
}
if (xblock>0)
{
domult(tvecs, tblock, xblock, nrows) ;
vvp(XTX, XTX, tvecs, nrows*nrows) ;
}
symit(XTX, nrows) ;
/**
a = 0; b=0 ;
printf("zz1 %12.6f ", XTX[a*nrows+b]) ;
a = nrows-1; b=nrows-1 ;
printf(" %12.6f %15.9g\n", XTX[a*nrows+b], asum(XTX, nrows*nrows)) ;
*/
if (verbose)
{
printdiag(XTX, nrows) ;
}
y = trace(XTX, nrows) / (double) (nrows-1) ;
if (isnan(y)) fatalx("bad XTX matrix\n") ;
/* printf("trace: %9.3f\n", y) ; */
if (y<=0.0) fatalx("XTX has zero trace (perhaps no data)\n") ;
vst(XTX, XTX, 1.0/y, nrows * nrows) ;
/// mean eigenvalue is 1
eigvecs(XTX, lambda, evecs, nrows) ;
// eigenvalues are in decreasing order
if (outliter > numoutliter) break ;
// last pass skips outliers
numoutleigs = MIN(numoutleigs, nrows-1) ;
nbad = ridoutlier(evecs, nrows, numoutleigs, outlthresh, badlist, outinfo) ;
if (nbad == 0) break ;
for (i=0; i<nbad; i++)
{
j = badlist[i] ;
indx = xindlist[j] ;
outpt = outinfo[j] ;
fprintf(outlfile, "REMOVED outlier %s iter %d evec %d sigmage %.3f\n", indx -> ID, outliter, outpt -> vecno, outpt -> score) ;
indx -> ignore = YES ;
}
nrows = loadindx(xindlist, xindex, indivmarkers, numindivs) ;
printf("number of samples after outlier removal: %d\n", nrows) ;
}
if (outliername != NULL) fclose(outlfile) ;
m = numgtz(lambda, nrows) ;
/* printf("matrix rank: %d\n", m) ; */
if (m==0) fatalx("no data\n") ;
/** smartrel code */
for (i=0; i<numeigs; i++) {
y = sqrt(lambda[i]) ;
vst(ww, evecs+i*nrows, y, nrows) ;
subouter(XTX, ww, nrows) ;
}
free(tvecs) ;
n = 0 ;
ZALLOC(vecind, nrows*nrows/2, int) ;
for (i=0; i<nrows; i++) {
for (j=i+1; j<nrows; j++) {
k = i*nrows + j ;
y1 = XTX[i*nrows+i] ;
y2 = XTX[j*nrows+j] ;
y = XTX[k]/sqrt(y1*y2) ;
y += 1/(double)(nrows-1);
if (y<relthresh) continue ;
vecind[n] = k ;
evecs[n] = -y ;
++n ;
}
}
free(XTX) ;
if (n==0) {
printf("## nothing above relthresh!\n") ;
printf("##end of smartrel run\n") ;
return 0 ;
}
ZALLOC(idperm, n, int) ;
sortit(evecs, idperm, n) ;
for (i=0; i<n; i++) {
j = idperm[i] ;
k = vecind[j] ;
a = k/nrows ;
b = k%nrows ;
printf("rel: %20s ", xindlist[a] ->ID) ;
printf("%20s ", xindlist[b] ->ID) ;
printf(" %9.3f", -evecs[i]) ;
printnl() ;
}
printf("##end of smartrel run\n") ;
return 0 ;
}
static void vst(FILE *in, FILE *out)
{
unsigned char *key = NULL;
unsigned char *v = NULL;
unsigned char *dt = NULL;
unsigned char ret[16];
char buf[1024];
char lbuf[1024];
uint8_t seed[1024];
char *keyword, *value;
long i, keylen;
struct session_op session = {
.rng = CRYPTO_ANSI_CPRNG,
.key = seed
};
struct crypt_op op = {
.dst = ret,
.len = 16
};
keylen = 0;
while(fgets(buf,sizeof buf,in) != NULL)
{
fputs(buf,out);
if(!strncmp(buf,"[AES 128-Key]", 13))
keylen = 16;
else if(!strncmp(buf,"[AES 192-Key]", 13))
keylen = 24;
else if(!strncmp(buf,"[AES 256-Key]", 13))
keylen = 32;
if (!parse_line(&keyword, &value, lbuf, buf))
continue;
if(!strcmp(keyword,"Key"))
{
key=hex2bin_m(value,&i);
if (i != keylen)
{
fprintf(stderr, "Invalid key length, expecting %ld\n", keylen);
return;
}
}
else if(!strcmp(keyword,"DT"))
{
dt=hex2bin_m(value,&i);
if (i != 16)
{
fprintf(stderr, "Invalid DT length\n");
return;
}
}
else if(!strcmp(keyword,"V"))
{
v=hex2bin_m(value,&i);
if (i != 16)
{
fprintf(stderr, "Invalid V length\n");
return;
}
if (!key || !dt)
{
fprintf(stderr, "Missing key or DT\n");
return;
}
session.keylen = build_seed(seed, v, key, dt, keylen);
if (session.keylen > sizeof(seed)/sizeof(*seed)) {
fprintf(stderr, "Seed buffer overrun\n");
return;
}
if (!cryptodev_op(session,op))
{
fprintf(stderr, "Error getting PRNG value\n");
return;
}
OutputValue("R", ret, 16, out, 0);
OPENSSL_free(key);
key = NULL;
OPENSSL_free(dt);
dt = NULL;
OPENSSL_free(v);
v = NULL;
}
}
}
static void mct(FILE *in, FILE *out)
{
unsigned char *key = NULL;
unsigned char *v = NULL;
unsigned char *dt = NULL;
unsigned char ret[16];
char buf[1024];
char lbuf[1024];
uint8_t seed[1024];
char *keyword, *value;
long i, keylen;
int cryptofd;
struct session_op session = {
.rng = CRYPTO_ANSI_CPRNG,
.key = seed
};
struct crypt_op op = {
.dst = ret,
.len = 16
};
keylen = 0;
while(fgets(buf,sizeof buf,in) != NULL)
{
fputs(buf,out);
if(!strncmp(buf,"[AES 128-Key]", 13))
keylen = 16;
else if(!strncmp(buf,"[AES 192-Key]", 13))
keylen = 24;
else if(!strncmp(buf,"[AES 256-Key]", 13))
keylen = 32;
if (!parse_line(&keyword, &value, lbuf, buf))
continue;
if(!strcmp(keyword,"Key"))
{
key=hex2bin_m(value,&i);
if (i != keylen)
{
fprintf(stderr, "Invalid key length, expecting %ld\n", keylen);
return;
}
}
else if(!strcmp(keyword,"DT"))
{
dt=hex2bin_m(value,&i);
if (i != 16)
{
fprintf(stderr, "Invalid DT length\n");
return;
}
}
else if(!strcmp(keyword,"V"))
{
v=hex2bin_m(value,&i);
if (i != 16)
{
fprintf(stderr, "Invalid V length\n");
return;
}
if (!key || !dt)
{
fprintf(stderr, "Missing key or DT\n");
return;
}
session.keylen = build_seed(seed, v, key, dt, keylen);
if (session.keylen > sizeof(seed)/sizeof(*seed)) {
fprintf(stderr, "Seed buffer overrun\n");
return;
}
cryptodev_start_session(&session, &cryptofd);
for (i = 0; i < 10000; i++)
{
if (cryptodev_session_op(session, cryptofd, op) <= 0)
{
fprintf(stderr, "Error getting PRNG value\n");
return;
}
}
cryptodev_end_session(session, cryptofd);
OutputValue("R", ret, 16, out, 0);
OPENSSL_free(key);
key = NULL;
OPENSSL_free(dt);
dt = NULL;
OPENSSL_free(v);
v = NULL;
}
}
}
#ifdef FIPS_ALGVS
int fips_rngvs_main(int argc, char **argv)
#else
int main(int argc, char **argv)
#endif
{
FILE *in, *out;
if (argc == 4)
{
in = fopen(argv[2], "r");
if (!in)
{
fprintf(stderr, "Error opening input file\n");
exit(1);
}
out = fopen(argv[3], "w");
if (!out)
{
fprintf(stderr, "Error opening output file\n");
exit(1);
}
}
else if (argc == 2)
{
in = stdin;
out = stdout;
}
else
{
fprintf(stderr,"%s [mct|vst]\n",argv[0]);
exit(1);
}
if(!strcmp(argv[1],"mct"))
mct(in, out);
else if(!strcmp(argv[1],"vst"))
vst(in, out);
else
{
fprintf(stderr,"Don't know how to %s.\n",argv[1]);
exit(1);
}
if (argc == 4)
{
fclose(in);
fclose(out);
}
return 0;
}
int
ridoutlier (double *evecs, int n, int neigs, double thresh, int *badlist,
OUTLINFO **outinfo)
{
/* badlist contains list of outliers */
double *ww, *w2, y1, y2, yy, zz;
int *vbad;
int i, j;
int nbad = 0;
OUTLINFO *outpt;
if (outliermode > 1)
return 0;
if (n < 3)
return 0;
ZALLOC(ww, n, double);
ZALLOC(vbad, n, int);
for (j = 0; j < n; j++)
{
outpt = outinfo[j];
outpt->vecno = -1;
}
for (i = 0; i < neigs; ++i)
{
copyarr (evecs + i * n, ww, n);
if (outliermode == 0)
{
y1 = asum (ww, n) / (double) n;
vsp (ww, ww, -y1, n);
y2 = asum2 (ww, n) / (double) n;
y2 = sqrt (y2);
vst (ww, ww, 1.0 / y2, n);
for (j = 0; j < n; j++)
{
if (fabs (ww[j]) > thresh)
{
vbad[j] = 1;
outpt = outinfo[j];
if (outpt->vecno < 0)
{
outpt->vecno = i;
outpt->score = ww[j];
}
}
}
}
if (outliermode == 1)
{
ZALLOC(w2, n, double);
for (j = 0; j < n; j++)
{
yy = ww[j];
ww[j] = 0;
y1 = asum (ww, n) / (double) (n - 1);
vsp (w2, ww, -y1, n);
w2[j] = 0;
y2 = asum2 (w2, n) / (double) n;
y2 = sqrt (y2);
zz = yy - y1;
zz /= y2;
if (fabs (zz) > thresh)
{
vbad[j] = 1;
outpt = outinfo[j];
if (outpt->vecno < 0)
{
outpt->vecno = i;
outpt->score = zz;
}
}
ww[j] = yy;
}
free (w2);
}
}
for (j = 0; j < n; j++)
{
if (vbad[j] == 1)
{
badlist[nbad] = j;
++nbad;
}
}
free (ww);
free (vbad);
return nbad;
}
void
dotpops(double *X, char **eglist, int numeg, int *xtypes, int nrows)
{
double *pp, *npp, val, yy ;
int *popsize ;
int i, j, k1, k2 ;
if (fstonly) return ;
ZALLOC(pp, numeg * numeg, double) ;
ZALLOC(npp, numeg * numeg, double) ;
popsize = xpopsize;
ivzero(popsize, numeg) ;
for (i=0; i<nrows; i++) {
k1 = xtypes[i] ;
++popsize[k1] ;
for (j=i+1; j<nrows; j++) {
k2 = xtypes[j] ;
if (k1 < 0) fatalx("bug\n") ;
if (k2 < 0) fatalx("bug\n") ;
if (k1>=numeg) fatalx("bug\n") ;
if (k2>=numeg) fatalx("bug\n") ;
val = X[i*nrows+i] + X[j*nrows+j] - 2.0*X[i*nrows+j] ;
pp[k1*numeg+k2] += val ;
pp[k2*numeg+k1] += val ;
++npp[k1*numeg+k2] ;
++npp[k2*numeg+k1] ;
}
}
vsp(npp, npp, 1.0e-8, numeg*numeg) ;
vvd(pp, pp, npp, numeg*numeg) ;
// and normalize so that mean on diagonal is 1
yy = trace(pp, numeg) / (double) numeg ;
vst(pp, pp, 1.0/yy, numeg*numeg) ;
printf("\n## Average divergence between populations:");
if (numeg<=10) {
printf("\n") ;
printf("%10s", "") ;
for (k1=0; k1<numeg; ++k1) {
printf(" %10s", eglist[k1]) ;
}
printf(" %10s", "popsize") ;
printf("\n") ;
for (k2=0; k2<numeg; ++k2) {
printf("%10s", eglist[k2]) ;
for (k1=0; k1<numeg; ++k1) {
val = pp[k1*numeg+k2] ;
printf(" %10.3f", val) ;
}
printf(" %10d", popsize[k2]) ;
printf("\n") ;
}
}
else { // numeg >= 10
printf("\n") ;
for (k2=0; k2<numeg; ++k2) {
for (k1=k2; k1<numeg; ++k1) {
printf("dotp: %10s", eglist[k2]) ;
printf(" %10s", eglist[k1]) ;
val = pp[k1*numeg+k2] ;
printf(" %10.3f", val) ;
printf(" %10d", popsize[k2]) ;
printf(" %10d", popsize[k1]) ;
printf("\n") ;
}
}
}
printf("\n") ;
printf("\n") ;
fflush(stdout) ;
free(pp) ;
free(npp) ;
}
void dopop3out(char **fglist, SNP **xsnplist, int ncols, char *line, char *outpop)
{
Indiv **xindlist ;
Indiv *indx ;
int *xindex, *xtypes ;
int nrows ;
int t, k, i, trun ;
double f3score, f3scoresig ;
double f2score, f2scoresig, y, y1, y2, p, q ;
char *eglist[4] ;
int numeg = 4 ;
double ytop, ybot, yxbot ;
double ztop, zbot ;
int col ;
SNP *cupt ;
double zztop[6], yytop[6] ;
double u, s1, s2, atop, btop, alphabot, betabot, alphatop ;
double ya, yb, za, zb, yt ;
char obuff[1024], *sx ;
int nsnp = 0 ;
copystrings(fglist, eglist, 3) ;
eglist[3] = strdup(outpop) ;
ZALLOC(xindex, numindivs, int) ;
ZALLOC(xindlist, numindivs, Indiv *) ;
setstatusv(indivmarkers, numindivs, NULL, NO) ;
setstatuslist(indivmarkers, numindivs, eglist, numeg) ;
nrows = loadindx(xindlist, xindex, indivmarkers, numindivs) ;
if (nrows == 0) {
for (i=0; i<numeg; ++i) {
printf("zz %s\n", eglist[i]) ;
}
fatalx("fatal error (probably missing pop)\n") ;
}
ZALLOC(xtypes, nrows, int) ;
for (i=0; i<nrows; i++) {
indx = xindlist[i] ;
k = indxindex(eglist, numeg, indx -> egroup) ;
xtypes[i] = k ;
}
ztop = zbot = 0.0 ;
vzero(zztop, 6) ;
for (col=0; col<ncols; ++col) {
cupt = xsnplist[col] ;
if (cupt -> ignore) continue ;
loadaa(cupt, xindex, xtypes, nrows, numeg) ;
f3scz(&ytop, &ybot, cupt, indivmarkers, xindex, xtypes, nrows, 2, 0, 1) ;
if (isnan(ytop)) fatalx("zznan\n") ;
if (ybot < -0.5) continue ;
f3scz(&yytop[0], &yxbot, cupt, indivmarkers, xindex, xtypes, nrows, 3, 0, 1) ; if (yxbot < -0.5) continue ;
f3scz(&yytop[1], &yxbot, cupt, indivmarkers, xindex, xtypes, nrows, 3, 0, 2) ; if (yxbot < -0.5) continue ;
f3scz(&yytop[2], &yxbot, cupt, indivmarkers, xindex, xtypes, nrows, 3, 1, 2) ; if (yxbot < -0.5) continue ;
f2scz(&yytop[3], &yxbot, cupt, indivmarkers, xindex, xtypes, nrows, 3, 0, 3) ; if (yxbot < -0.5) continue ;
f2scz(&yytop[4], &yxbot, cupt, indivmarkers, xindex, xtypes, nrows, 3, 1, 3) ; if (yxbot < -0.5) continue ;
f2scz(&yytop[5], &yxbot, cupt, indivmarkers, xindex, xtypes, nrows, 3, 2, 3) ; if (yxbot < -0.5) continue ;
ztop += ytop ;
zbot += ybot ;
if ((ytop>0) || (ybot > 0)) ++nsnp ; // monomorphic snps not counted
vvp(zztop, zztop, yytop, 6) ;
}
//verbose = YES ;
ztop /= zbot ;
vst(zztop, zztop, 1.0/zbot, 6) ;
u = zztop[0] ;
vsp(yytop, zztop, -u, 6) ;
s1 = yytop[1] ; /* alpha a */
s2 = yytop[2] ;
atop = yytop[3] ;
btop = yytop[4] ;
alphabot = s1/atop ;
betabot = s2/btop ;
alphatop = 1.0-betabot ;
y1 = -ztop -s1 ;
if (s2>s1) {
alphabot = MAX(alphabot, y1/(s2-s1)) ;
}
if (s2<s1) {
alphatop = MIN(alphatop, y1/(s2-s1)) ;
}
sx = obuff ;
sx += sprintf(sx, "%s", line) ;
//printf(" %12.6f", ztop) ;
sx += sprintf(sx, " %9.3f", alphabot) ;
sx += sprintf(sx, " %9.3f", alphatop) ;
/**
// next code is computing bounds on h (drift -> C)
za = alphatop; zb = 1.0-za ;
ya = s1/za; yb = s2/zb; yt = -za*zb*(ya+yb) ; y1 = ztop - yt ;
za = alphabot; zb = 1.0-za ;
ya = s1/za; yb = s2/zb; yt = -za*zb*(ya+yb) ; y2 = ztop - yt ;
sx += sprintf(sx, " %9.3f %9.3f", y1, y2) ;
sx += sprintf(sx, " %7d", nsnp) ;
*/
printf("%s", obuff) ;
printnl() ;
if (verbose) printmatwl(yytop, 1, 6, 6) ;
if (outputname != NULL) {
fprintf(ofile, "%s\n", obuff) ;
fflush(ofile) ;
}
free(xtypes) ;
free(xindex) ;
free(xindlist) ;
freeup(eglist, 4) ;
destroyaa() ;
return ;
}
static void
cpArbiterApplyImpulse_NEON(cpArbiter *arb)
{
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
cpFloatx2_t surface_vr = vld((cpFloat_t *)&arb->surface_vr);
cpFloatx2_t n = vld((cpFloat_t *)&arb->n);
cpFloat_t friction = arb->u;
int numContacts = arb->count;
struct cpContact *contacts = arb->contacts;
for(int i=0; i<numContacts; i++) {
struct cpContact *con = contacts + i;
cpFloatx2_t r1 = vld((cpFloat_t *)&con->r1);
cpFloatx2_t r2 = vld((cpFloat_t *)&con->r2);
cpFloatx2_t perp = vmake(-1.0, 1.0);
cpFloatx2_t r1p = vmul(vrev(r1), perp);
cpFloatx2_t r2p = vmul(vrev(r2), perp);
cpFloatx2_t vBias_a = vld((cpFloat_t *)&a->v_bias);
cpFloatx2_t vBias_b = vld((cpFloat_t *)&b->v_bias);
cpFloatx2_t wBias = vmake(a->w_bias, b->w_bias);
cpFloatx2_t vb1 = vadd(vBias_a, vmul_n(r1p, vget_lane(wBias, 0)));
cpFloatx2_t vb2 = vadd(vBias_b, vmul_n(r2p, vget_lane(wBias, 1)));
cpFloatx2_t vbr = vsub(vb2, vb1);
cpFloatx2_t v_a = vld((cpFloat_t *)&a->v);
cpFloatx2_t v_b = vld((cpFloat_t *)&b->v);
cpFloatx2_t w = vmake(a->w, b->w);
cpFloatx2_t v1 = vadd(v_a, vmul_n(r1p, vget_lane(w, 0)));
cpFloatx2_t v2 = vadd(v_b, vmul_n(r2p, vget_lane(w, 1)));
cpFloatx2_t vr = vsub(v2, v1);
cpFloatx2_t vbn_vrn = vpadd(vmul(vbr, n), vmul(vr, n));
cpFloatx2_t v_offset = vmake(con->bias, -con->bounce);
cpFloatx2_t jOld = vmake(con->jBias, con->jnAcc);
cpFloatx2_t jbn_jn = vmul_n(vsub(v_offset, vbn_vrn), con->nMass);
jbn_jn = vmax(vadd(jOld, jbn_jn), vdup_n(0.0));
cpFloatx2_t jApply = vsub(jbn_jn, jOld);
cpFloatx2_t t = vmul(vrev(n), perp);
cpFloatx2_t vrt_tmp = vmul(vadd(vr, surface_vr), t);
cpFloatx2_t vrt = vpadd(vrt_tmp, vrt_tmp);
cpFloatx2_t jtOld = {};
jtOld = vset_lane(con->jtAcc, jtOld, 0);
cpFloatx2_t jtMax = vrev(vmul_n(jbn_jn, friction));
cpFloatx2_t jt = vmul_n(vrt, -con->tMass);
jt = vmax(vneg(jtMax), vmin(vadd(jtOld, jt), jtMax));
cpFloatx2_t jtApply = vsub(jt, jtOld);
cpFloatx2_t i_inv = vmake(-a->i_inv, b->i_inv);
cpFloatx2_t nperp = vmake(1.0, -1.0);
cpFloatx2_t jBias = vmul_n(n, vget_lane(jApply, 0));
cpFloatx2_t jBiasCross = vmul(vrev(jBias), nperp);
cpFloatx2_t biasCrosses = vpadd(vmul(r1, jBiasCross), vmul(r2, jBiasCross));
wBias = vadd(wBias, vmul(i_inv, biasCrosses));
vBias_a = vsub(vBias_a, vmul_n(jBias, a->m_inv));
vBias_b = vadd(vBias_b, vmul_n(jBias, b->m_inv));
cpFloatx2_t j = vadd(vmul_n(n, vget_lane(jApply, 1)), vmul_n(t, vget_lane(jtApply, 0)));
cpFloatx2_t jCross = vmul(vrev(j), nperp);
cpFloatx2_t crosses = vpadd(vmul(r1, jCross), vmul(r2, jCross));
w = vadd(w, vmul(i_inv, crosses));
v_a = vsub(v_a, vmul_n(j, a->m_inv));
v_b = vadd(v_b, vmul_n(j, b->m_inv));
// TODO would moving these earlier help pipeline them better?
vst((cpFloat_t *)&a->v_bias, vBias_a);
vst((cpFloat_t *)&b->v_bias, vBias_b);
vst_lane((cpFloat_t *)&a->w_bias, wBias, 0);
vst_lane((cpFloat_t *)&b->w_bias, wBias, 1);
vst((cpFloat_t *)&a->v, v_a);
vst((cpFloat_t *)&b->v, v_b);
vst_lane((cpFloat_t *)&a->w, w, 0);
vst_lane((cpFloat_t *)&b->w, w, 1);
vst_lane((cpFloat_t *)&con->jBias, jbn_jn, 0);
vst_lane((cpFloat_t *)&con->jnAcc, jbn_jn, 1);
vst_lane((cpFloat_t *)&con->jtAcc, jt, 0);
}
}
int main() {
int n,looplim = 10000;
while(cin >> n) {
vMat all_Mat;
Mat sec_Mat;
cout << "=======" << endl;
cout << n << endl;
Mat L;
double start_t = clock();
double ans = INF;
double t3 = 0,t4 = 0;
clock_t c1,c2;
//step 0
vd ans_v;
for(int i = 0 ; i < n ; i++) {
vd tmpl,x;
for(int j = 0 ; j < n ; j++) {
if(i == j) {x.push_back(1);}
else {x.push_back(0);}
}
if(get_f(x) <= ans) {
ans = get_f(x);
ans_v = x;
}
for(int j = 0 ; j < n ; j++) {
if(x[j] == 1) {
tmpl.push_back(get_f(x));
} else {
tmpl.push_back(INF);
}
}
L.push_back(tmpl);
}
int cnt = 0;
vMat used_Mat;//,all_Mat;
all_Mat.push_back(L);
sec_Mat.push_back(get_sec(L));
multimap<double,Mat> dMatmp;
set<pvdi> st;
set<pdi> stf;
vector<set<pdi> > vst(n);
set<int> used_idx;
st.insert(make_pair(get_diag(L),all_Mat.size()-1));
stf.insert(make_pair(L[0][0],all_Mat.size()-1));
priority_queue<pair<double,int> , vector<pair<double,int> > , greater<pair<double,int> > > pq;
//priority_queue<pair<double,int> > pq;
pq.push(make_pair(get_d(L),all_Mat.size()-1));
//O(min(loop,local min))
int max_set_size = 1000000;
int cut_cnt = 0;
while(cnt++ < looplim && !pq.empty()) {
if(pq.size() > 1000000) break;
if(cnt % 1000 == 0) {
cout << "cnt:" << cnt << endl;
cout << "lb:" << pq.top().first << endl;
cout << "f:" << ans << endl;
cout << "size:" << pq.size() << endl;
cout << "cut:" << cut_cnt << endl;
cout << "time:" << (clock() - start_t)/CLOCKS_PER_SEC << endl;
cout << "---" << endl;
}
//step 1
//select l in Lk with the smallest d
double d = pq.top().first;
int idx = pq.top().second;
pq.pop();
L = all_Mat[idx];
//print_Mat(L);
//print_Mat(L);
//cout << endl;
if(used_idx.find(idx) != used_idx.end()){
continue;
}
if(ans - d < eps) {
break;
}
vd xs;
for(int i = 0 ; i < L.size() ; i++) {
//caution : division by zero
xs.push_back(d/L[i][i]);
}
//step 2
double tmpf = get_f(xs);
//print_v(xs);
//cout << get_f(xs) << endl;
if(tmpf <= ans) {
ans = tmpf;
ans_v = xs;
}
vd lk;
for(int i = 0 ; i < xs.size() ; i++) {
if(xs[i] > zero_boundary) lk.push_back(tmpf/xs[i]);
else lk.push_back(INF);
}
vMat Lm;
vi idxs = get_lower_idx_s(st,lk);
//vi idxs = get_lower_idx(st,lk,vst);
//vi idxs = get_lower_idx_s(st,lk);
Mat Sec;
for(int i = 0 ; i < idxs.size() ; i++) {
if(used_idx.find(idxs[i]) == used_idx.end()){
used_idx.insert(idxs[i]);
Lm.push_back(all_Mat[idxs[i]]);
Sec.push_back(sec_Mat[idxs[i]]);
}
}
used_idx.insert(idx);
//step 4
//O(L- size * n * n^2)
vd dlk = lk;
for(int j = 0 ; j < Lm.size() ; j++) {
Mat tmpL = Lm[j];
vd tmplk = lk;
//print_Mat(tmpL);
//cout << endl;
vi idx = get_swap_idx(tmplk,Sec[j]);
/*
cout << "s:" << endl;
for(int i = 0 ; i < idx.size() ; i++) {
cout << idx[i] << " ";
}
cout << endl;
*/
for(int i = 0 ; i < idx.size() ; i++) {
L = tmpL;
lk = tmplk;
//print_Mat(L);
swap(L[idx[i]],lk);
//cout << "--" << endl;
//print_v(Sec[j]);
//cout << "v:" << is_valid_combination(L,Sec[j]) << " " << is_valid_combination(L) << endl;
//if(get_d(L) > d) {
all_Mat.push_back(L);
sec_Mat.push_back(get_sec(L));
st.insert(make_pair(get_diag(L),all_Mat.size()-1));
stf.insert(make_pair(L[0][0],all_Mat.size()-1));
pq.push(make_pair(get_d(L),all_Mat.size()-1));
//}
swap(L[idx[i]],lk);
}
}
priority_queue<pair<double,int> , vector<pair<double,int> > , greater<pair<double,int> > > tmp_pq = pq;
priority_queue<pair<double,int> , vector<pair<double,int> > , greater<pair<double,int> > > nxt_pq;
int set_size = 0;
while(!tmp_pq.empty()) {
set_size++;
int idx = tmp_pq.top().second;
if(set_size < max_set_size) nxt_pq.push(tmp_pq.top());
else {
st.erase(make_pair(get_diag(all_Mat[idx]),idx));
}
tmp_pq.pop();
}
pq = nxt_pq;
}
cout << "cnt:" << cnt << endl;
cout << "ans:" << ans << endl;
cout << "ans_v:";
print_v(ans_v);
}
return 0;
}
