DenseVector ConstraintQuadratic::eval(const DenseVector &x) const
{
// Convert x to matrix so that it can be transposed
DenseMatrix z(x);
DenseMatrix zt = z.transpose();
DenseMatrix bt = b.transpose();
DenseVector cvec(1); cvec(0) = c;
DenseVector y = zt*A*z + bt*z + cvec;
return y;
}
Q3Vector operator*(qreal coef, const Q3Vector &vec)
{
Q3Vector cvec(vec.size());
for (int i = 0; i < vec.size(); ++i)
cvec[i] = coef * vec.at(i);
return cvec;
}
void EEMS::initialize_diffs( ) {
cerr << "[Diffs::initialize]" << endl;
n_2 = (double)n/2.0; nmin1 = n-1; logn = log(n);
J = MatrixXd::Zero(n,o);
cvec = VectorXd::Zero(o);
cinv = VectorXd::Zero(o);
for ( int i = 0 ; i < n ; i ++ ) {
J(i,graph.get_deme_of_indiv(i)) = 1;
cvec(graph.get_deme_of_indiv(i)) += 1;
}
cinv = pow(cvec.array(),-1.0).matrix(); // cinv is the vector of inverse counts
cmin1 = cvec; cmin1.array() -= 1; // cmin1 is the vector of counts - 1
Diffs = readMatrixXd(params.datapath + ".diffs");
if ((Diffs.rows()!=n)||(Diffs.cols()!=n)) {
cerr << " Error reading dissimilarities matrix " << params.datapath + ".diffs" << endl
<< " Expect a " << n << "x" << n << " matrix of pairwise differences" << endl; exit(1);
}
cerr << " Loaded dissimilarities matrix from " << params.datapath + ".diffs" << endl;
if (!isdistmat(Diffs)) {
cerr << " The dissimilarity matrix is not a full-rank distance matrix" << endl; exit(1);
}
L = MatrixXd::Constant(nmin1,n,-1.0);
L.topRightCorner(nmin1,nmin1).setIdentity();
JtDhatJ = MatrixXd::Zero(o,o);
JtDobsJ = J.transpose()*Diffs*J;
ldLLt = logdet(L*L.transpose());
ldLDLt = logdet(-L*Diffs*L.transpose());
ldDiQ = ldLLt - ldLDLt;
if (cvec.maxCoeff()>1) {
cerr << "[Diffs::initialize] Use this version only if there is at most one sample in every deme" << endl;
exit(1);
}
cerr << "[Diffs::initialize] Done." << endl << endl;
}
// use REML equation to estimate variance component
// input P, calculate PA, H, R and varcmp
void gcta::ai_reml(eigenMatrix &P, eigenMatrix &Hi, eigenVector &Py, eigenVector &prev_varcmp, eigenVector &varcmp, double dlogL)
{
int i=0, j=0;
Py=P*_y;
eigenVector cvec(_n);
eigenMatrix APy(_n, _r_indx.size());
for(i=0; i<_r_indx.size(); i++) (APy.col(i))=(_A.block(0,_r_indx[i]*_n,_n,_n))*Py;
// Calculate Hi
eigenVector R(_r_indx.size()+1);
for(i=0; i<_r_indx.size(); i++){
R(i)=(Py.transpose()*(APy.col(i)))(0,0);
cvec=P*(APy.col(i));
Hi(i,i)=((APy.col(i)).transpose()*cvec)(0,0);
for(j=0; j<i; j++) Hi(j,i)=Hi(i,j)=((APy.col(j)).transpose()*cvec)(0,0);
}
cvec=P*Py;
for(j=0; j<_r_indx.size(); j++) Hi(j,_r_indx.size())=Hi(_r_indx.size(),j)=((APy.col(j)).transpose()*cvec)(0,0);
R(_r_indx.size())=(Py.transpose()*Py)(0,0);
Hi(_r_indx.size(),_r_indx.size())=(Py.transpose()*cvec)(0,0);
Hi=0.5*Hi;
// Calcualte tr(PA) and dL
eigenVector tr_PA;
calcu_tr_PA(P, tr_PA);
R=-0.5*(tr_PA-R);
// Calculate variance component
comput_inverse_logdet_LU(Hi, "Error: AI matrix is not invertible.");
eigenVector delta(_r_indx.size()+1);
delta=Hi*R;
if(dlogL>1.0) varcmp=prev_varcmp+0.316*delta;
else varcmp=prev_varcmp+delta;
}
int main(int argc, char const *argv[])
{
std::vector<int> ivec(9, 8);
std::vector<double> dvec(8, 9.9);
std::vector<char> cvec(7, 'h');
std::cout << count(ivec, 8) << std::endl;
std::cout << count(dvec, 9.9) << std::endl;
std::cout << count(cvec, 'h') << std::endl;
std::vector<std::string> svec(6, "hey");
std::cout << count(svec, std::string("hey")) << std::endl;
return 0;
}
/////////////////////////////////////////////////////////////////////////////////
// calculate the intersection of a sphere the given ray
// the ray has an origin and a direction, ray = origin + t*direction
// find the t parameter, return true if it is between 0.0 and 1.0, false
// otherwise, write the results in following variables:
// depth - t \in [0.0 1.0]
// posX - x position of intersection point, nothing if no intersection
// posY - y position of intersection point, nothing if no intersection
// posZ - z position of intersection point, nothing if no intersection
// normalX - x component of normal at intersection point, nothing if no intersection
// normalX - y component of normal at intersection point, nothing if no intersection
// normalX - z component of normal at intersection point, nothing if no intersection
//
// attention: a sphere has usually two intersection points make sure to return
// the one that is closest to the ray's origin and still in the viewing frustum
//
/////////////////////////////////////////////////////////////////////////////////
bool
Sphere::intersect(Ray ray, double *depth,
double *posX, double *posY, double *posZ,
double *normalX, double *normalY, double *normalZ)
{
//////////*********** START OF CODE TO CHANGE *******////////////
// from slides:
// (cx + t * vx)^2 + (cy + t * vy)^2 + (cz + t * vy)^2 = r^2
// text:
// (e+td−c)·(e+td−c)−R2 = 0
// (d·d)t^2 +2d·(e−c)t+(e−c)·(e−c)−R^2 = 0
// d: the direction vector of the ray
// e: point at which the ray starts
// c: center point of the sphere
Vec3 dvec( ray.direction[0],
ray.direction[1],
ray.direction[2]);
Vec3 evec( ray.origin[0],
ray.origin[1],
ray.origin[2]);
Vec3 cvec( this->center[0],
this->center[1],
this->center[2]);
// use the quadratic equation, since we have the form At^2 + Bt + C = 0.
double a = dvec.dot(dvec);
double b = dvec.scale(2).dot(evec.subtract(cvec));
Vec3 eMinusCvec = evec.subtract(cvec);
double c = eMinusCvec.dot(eMinusCvec) - (this->radius * this->radius);
// discriminant: b^2 - 4ac
double discriminant = (b * b) - (4 * a * c);
// From text: If the discriminant is negative, its square root
// is imaginary and the line and sphere do not intersect.
if (discriminant < 0) {
//printf("No intersection with sphere - 1\n");
return false;
} else {
// there is at least one intersection point
double t1 = (-b + sqrt(discriminant)) / (2 * a);
double t2 = (-b - sqrt(discriminant)) / (2 * a);
double tmin = fminf(t1, t2);
double tmax = fmaxf(t1, t2);
double t = 0; // t is set to either tmin or tmax (or the function returns false)
if (tmin >= 0) { //} && tmin <= 1) {
t = tmin;
} else if (tmax >= 0) { //} && tmax <= 1) {
t = tmax;
} else {
// return false if neither interestion point is within [0, 1]
//printf("No intersection with sphere. t values (%f, %f)\n", t1, t2);
return false;
}
*depth = t;
// position: (e + td)
Vec3 posvec = dvec.scale(t).add(evec);
*posX = posvec[0];
*posY = posvec[1];
*posZ = posvec[2];
// normal: 2(p - c)
Vec3 normalvec = posvec.subtract(cvec).scale(2);
normalvec.normalize();
*normalX = normalvec[0];
*normalY = normalvec[1];
*normalZ = normalvec[2];
}
//////////*********** END OF CODE TO CHANGE *******////////////
//printf("Sphere intersection found (%f, %f, %f) \n", *posX, *posY, *posZ);
return true;
}
int main(int argc, char * argv [])
{
int n = 1000;
srand48(120);
// srand48(123);
if (argc > 1)
srand48(atoi(argv[1]));
if (argc > 2)
n = atoi(argv[2]);
fprintf(stderr, "n= %d\n", n);
MSW lp(1.0);
#if 1
for (int i = 0; i < n; i++)
{
const real lx = 0.75;
const real ly = 0.75;
const real lz = 0.75;
const real nx = (1.0-2.0*drand48())*lx;
const real ny = (1.0-2.0*drand48())*ly;
const real nz = (1.0-2.0*drand48())*lz;
const real x = -nx;
const real y = -ny;
const real z = -nz;
lp.push(HalfSpace(vec3(nx,ny,nz), vec3(x, y, z)));
}
#else
lp.push(HalfSpace(vec3(-1.0, -1.0, 0.0), vec3(0.3, 0.3, 0.00)));
lp.push(HalfSpace(vec3(+1.0, 0.0, 0.0), vec3(0.25, 0.5, 0.5)));
lp.push(HalfSpace(vec3(-1.0, 0.0, 0.0), vec3(0.75, 0.5, 0.5)));
lp.push(HalfSpace(vec3(0.0, +1.0, 0.0), vec3(0.5, 0.25, 0.5)));
lp.push(HalfSpace(vec3(0.0, -1.0, 0.0), vec3(0.5, 0.75, 0.5)));
lp.push(HalfSpace(vec3(0.0, 0.0, +1.0), vec3(0.5, 0.5, 0.25)));
lp.push(HalfSpace(vec3(0.0, 0.0, -1.0), vec3(0.5, 0.5, 0.75)));
// lp.push(HalfSpace(vec3(0.0, 0.0, +1.0), vec3(0.5, 0.5, 0.40)));
// lp.push(HalfSpace(vec3(0.0, 0.0, -1.0), vec3(0.5, 0.5, 0.30)));
// lp.push(HalfSpace(vec3(0.0, +1.0, 0.0), vec3(0.5, 0.3, 0.30)));
#endif
fprintf(stderr, " nspace= %d\n", lp.nspace());
const int nrep = 1000;
vec3 cvec(-1.0, +1.0, 0.0);
#if 0
vec3 pos = lp.solve(cvec, false);
fprintf(stderr, " pos= %g %g %g \n", pos.x, pos.y, pos.z);
#else
cvec = vec3(1.0 - 2.0*drand48(), 1.0 - 2.0*drand48(), 1.0 - 2.0*drand48());
vec3 pos = lp.solve(cvec, false);
{
const double t0 = get_wtime();
fprintf(stderr, " pos= %g %g %g \n", pos.x, pos.y, pos.z);
for (int i = 0; i < nrep; i++)
{
vec3 pos1 = lp.solve(cvec, false);
if ((pos1 - pos).norm2() > 1.0e-20*pos.norm2())
fprintf(stderr, " pos= %g %g %g \n", pos1.x, pos1.y, pos1.z);
}
const double dt = get_wtime() - t0;
fprintf(stderr, " norm done in %g sec\n", dt/nrep);
}
{
const double t0 = get_wtime();
for (int i = 0; i < nrep; i++)
{
vec3 pos1 = lp.solve(cvec, true);
if ((pos1 - pos).norm2() > 1.0e-20*pos.norm2())
fprintf(stderr, " rpos= %g %g %g \n", pos1.x, pos1.y, pos1.z);
}
const double dt = get_wtime() - t0;
fprintf(stderr, " rand done in %g sec\n", dt/nrep);
}
{
nflops = 0;
int nrep = 100000;
std::vector<vec3> vecList(nrep);
const double t0 = get_wtime();
for (int i = 0; i < nrep; i++)
{
const vec3 cvec(1.0 - 2.0*drand48(), 1.0 - 2.0*drand48(), 1.0 - 2.0*drand48());
vecList[i] = lp.solve(cvec, false);
}
const double dt = get_wtime() - t0;
fprintf(stderr, " test done in %g sec [%g sec per element]\n", dt, dt/nrep);
fprintf(stderr, " performance: %g GFLOP %g GFLOP/s \n", nflops*1.0/1e9, nflops*1.0/dt/1e9);
}
#endif
return 0;
}
//
// fill missing bins in histogram h fitting a plane to the
// surrounding bins
//
void fitQuadratic (TH2* h, TH2* refHisto, int nmin=9, int nmin2=12) {
//
// prepare histogram
//
TH2* hOrig = (TH2*)h->Clone();
int nbx = hOrig->GetNbinsX();
int nby = hOrig->GetNbinsY();
//
// matrix and vector for fit
//
TMatrixD mat(6,6);
TVectorD cvec(6);
//
// loop over histogram
//
for ( int ix=1; ix<=nbx; ++ix ) {
for ( int iy=1; iy<=nby; ++iy ) {
// clear matrix and vector used for fit
int nn(0);
for ( int i=0; i<6; ++i ) {
cvec(i) = 0.;
for ( int j=0; j<6; ++j ) mat(i,j) = 0.;
}
// loop over neighbours
for ( int jx=-1; jx<2; ++jx ) {
for ( int jy=-1; jy<2; ++jy ) {
if ( (ix+jx)<1 || (ix+jx)>nbx ) continue;
if ( (iy+jy)<1 || (iy+jy)>nby ) continue;
// fill vector and matrix
fillForQuadFit(hOrig,refHisto,ix,iy,jx,jy,nn,mat,cvec);
}
}
for ( int jx=1; jx<6; ++jx ) {
for ( int jy=0; jy<jx; ++jy ) mat(jx,jy) = mat(jy,jx);
}
// if < nmin neighbours in delta_i==1: try to add delta_i==2
if ( nn<nmin ) {
// loop over neighbours
for ( int jx=-2; jx<3; ++jx ) {
for ( int jy=-2; jy<3; ++jy ) {
// skip the central bin (the one to be filled)
if ( abs(jx)<2 && abs(jy)<2 ) continue;
if ( (ix+jx)<1 || (ix+jx)>nbx ) continue;
if ( (iy+jy)<1 || (iy+jy)>nby ) continue;
// fill vector and matrix
fillForQuadFit(hOrig,refHisto,ix,iy,jx,jy,nn,mat,cvec);
}
}
for ( int jx=1; jx<6; ++jx ) {
for ( int jy=0; jy<jx; ++jy ) mat(jx,jy) = mat(jy,jx);
}
// drop bin if <nmin2 in 5x5 area
if ( nn<nmin2 ) continue;
}
// cout << "x / y = " << h->GetXaxis()->GetBinCenter(ix) << " "
// << h->GetYaxis()->GetBinCenter(iy) << endl;
//
// linear 2D fit to neighbours (in units of bin number):
// par(0)*(x-ix)+par(1)*(y-iy)+par(2)
//
double det = mat.Determinant();
if ( det < 1.e-6 ) std::cout << "************* " << det << std::endl;
if ( det < 1.e-6 ) continue;
// TMatrixD mat1(mat);
mat.Invert(&det);
TVectorD par = mat*cvec;
// TVectorD tmp = mat1*par;
h->SetBinContent(ix,iy,par(5));
}
}
delete hOrig;
}
