void Covariance::setCovIndPar( const valarray<double>& indParNew )
{
// If values have been cached, then check the elements of the
// parameter vector that affect this covariance matrix to see
// if the cached values are still valid.
if ( isCacheStatusValid() )
{
if ( !isDifferent( covIndPar, indParNew ) )
{
return; // Cached values are ok.
}
else
{
setCacheStatusInvalid();
}
}
covIndPar.resize( indParNew.size() );
covIndPar = indParNew;
}
vector<double> autocovariance(const valarray<double>& x,unsigned max)
{
const int N = x.size();
// specify sample size if unspecified
if (max==0)
max = std::max(1+N/4,N-15);
if (max >= N)
max = N-1;
// compute mean of X
double mean = 0;
for(int i=0;i<N;i++)
mean += x[i];
mean /= N;
// allocate covariances
vector<double> rho(max);
// compute each autocorrelation rho[k]
double limit = 0.01/N;
for(int k=0;k<max;k++)
{
double total = 0;
for(int i=0;i<N-k;i++)
total += (x[i]-mean)*(x[i+k]-mean);
rho[k] = total/(N-k);
if (rho[k] < limit and k>0) {
rho.resize(k);
break;
}
}
return rho;
}
inline void sortArray(valarray<double>& x, vector<int> &id)
{
int i,j,k,t1;
int n=x.size();
double t2;
for(i=0;i<n;i++)
id.push_back(i);
for(i=0;i<n-1;i++)
{
k=i;
for(j=i+1;j<n;j++)
if(x[j]>x[k])
k=j;
if(k!=i)
{
t2=x[i];
x[i]=x[k];
x[k]=t2;
t1=id[i];
id[i]=id[k];
id[k]=t1;
}
}
}
const valarray<double> elsq_x(
const valarray<double> &z, // n size vector
const valarray<double> &h, // n size vector
const valarray<double> &Q, // n by n symmetric, positive definite
const valarray<double> &Qinv, // n by n symmetric, positive definite
const valarray<double> &h_x, // n by nX matrix
const valarray<double> &Q_x // n^2 by nX matrix
)
{
const int n = z.size();
assert( h.size() == n );
assert( Q.size() == n * n );
const int nX = h_x.size() / n;
assert( n * nX == h_x.size() );
assert( Q_x.size() == n * n * nX );
valarray<double> residual = z - h;
valarray<double> residualTran = residual; // 1 by n
double val;
valarray<double> W = multiply(residualTran, n, Qinv, n); // 1 by n
valarray<double> rvecQinvTrans = rvec( Qinv, n ); // 1 by n^2: don't really need to transpose because it's just an array
valarray<double> term1 = multiply(rvecQinvTrans, n*n, Q_x, nX ); // 1 by nX
valarray<double> term2 = multiply(W, n, h_x, nX);
assert(term2.size()== nX);
//
// The loop below untangles the following statement
// for eliminating matrix object constructions and taking advantage of
// Q being symmetric.
//
// valarray<double> term3 = AkronBtimesC( W, n, rvecQinvTrans, n, Q_x, n);
//
// Let w be (z-h).
//
// The orignal formula
// 0.5 [w^T kron w^T] partial_x(Qinv) --- (eq. 1)
//
// is equivalent to:
// 0.5 partial_x(k) [ w^T Qinv w ] --- (eq. 2)
// =0.5 SUM [ w(i) w(j) partial_x(k)(Q(i,j)) ], over 1<=i<=m and 1<=j<=m.
//
// For Q being symmetric, the following is true:
// w(i) w(j) partial_x(k)(Q(i,j)) == w(j) w(i) partial_x(k)(Q(j,i))
//
//
valarray<double> term3( 0.0, nX ); // 1 by nX
for( int k=0; k<nX; k++ )
{
for( int j=0; j<n; j++ )
{
for( int i=0; i<n; i++ )
{
if( i<=j )
{
val = W[i] * W[j] * Q_x[ j*n+i+k*n*n ] / 2.0;
term3[k] += val;
if( i<j )
term3[k] += val;
}
}
}
}
// Returns a 1 by nX matrix.
return 0.5 * term1 - term2 - term3;
}
void print(const valarray<T>& v) {
for(size_t i = 0; i < v.size(); ++i)
cout << '\t' << v[i];
cout << endl;
}
void indStatistics( const valarray<double>& indPar,
const valarray<double>& dataMean_indPar,
const valarray<double>& dataVariance_indPar,
const valarray<double>& dataVarianceInv,
valarray<double>* indParCovOut,
valarray<double>* indParSEOut,
valarray<double>* indParCorOut,
valarray<double>* indParCVOut,
valarray<double>* indParCIOut )
{
using std::endl;
using std::ends;
//----------------------------------------------------------------
// Preliminaries.
//----------------------------------------------------------------
// Return if there are no output values to compute.
if( indParCovOut == 0 && indParSEOut == 0 && indParCorOut == 0 &&
indParCVOut && indParCIOut == 0 )
return;
// Number of individual parameters
const int nB = static_cast<int>( indPar.size() );
assert( nB > 0 );
// Number of data points
const int nY = static_cast<int>( dataMean_indPar.size() ) / nB;
assert( static_cast<int>( dataMean_indPar.size() ) == nY * nB );
assert( nY > 0 );
using namespace std;
// Degree of freedom
const int nFree = nY - nB;
if( indParCIOut )
{
if( nFree < 1 )
{
const int max = SpkError::maxMessageLen();
char message[max];
sprintf( message, "The degree of freedom (#of measurements<%d> - #of random effects<%d>) must be positive.", nY, nB );
throw SpkException(
SpkError::SPK_USER_INPUT_ERR,
message,
__LINE__, __FILE__
);
}
}
//----------------------------------------------------------------
// Calculate Covariance of individual parameter estimates
//----------------------------------------------------------------
valarray<double> indParCov( nB * nB );
try
{
indParCov = inverse( 0.5 * multiply( transpose( dataVariance_indPar, nB ), nY * nY,
AkronBtimesC( dataVarianceInv, nY, dataVarianceInv,
nY, dataVariance_indPar, nB ), nB )
+ multiply( transpose( dataMean_indPar, nB ), nY,
multiply( dataVarianceInv, nY, dataMean_indPar, nB ), nB ),
nB );
}
catch(SpkException& e)
{
throw e.push( SpkError::SPK_NOT_INVERTABLE_ERR,
"Failed to invert information matrix",
__LINE__, __FILE__ );
}
if( indParCovOut != 0 )
*indParCovOut = indParCov;
statistics( indPar, indParCov, nFree, indParSEOut, indParCorOut, indParCVOut, indParCIOut );
}
log_double_t dirichlet_safe_pdf(const valarray<double>& p,double N, const valarray<double>& q)
{
return dirichlet_safe_pdf(p,N*p.size()*q);
}
log_double_t dirichlet_pdf(const valarray<double>& p,double N) {
return dirichlet_pdf(p,valarray<double>(N,p.size()));
}
valarray<T> rbgauss_seidel(const matrix_crs<T>& A, const valarray<T>& f,
const valarray<T>& v0, const T resid_tol, int& num_itr) {
// Red-Black Gauss-Seidel method driver
// pass in the matrix, RHS vector
// pass in an initial guess (there is another interface which creates a
// random initial guess)
// num_itr is the number of iterations to do. defaults to -1, which will
// run until we reach the maximum number of iterations or reach tolerance
// On return, num_itr will have the number of iterations completed.
//T resid_tol = static_cast<T>(10e-8);
unsigned num_its_done;
// check sizes
if ( A.m != f.size() ) {
cerr << "error: classical_solvers:rbgauss_seidel: dimension mismatch"
<< endl;
exit(-1);
}
// it is assumed that A is odd x odd, otherwise RB GS won't work
if ( A.m % 2 != 1 ) {
cerr << "error: classical_solvers:rbgauss_seidel: A has even dimensions"
<< endl;
exit(-1);
}
// random initial guess
valarray<T> v = v0;
valarray<T> resid = f-A*v;
// number of iterations to do
if ( num_itr == -1 ) { // just do normal solve until we reach tol or
// _RBGS_MAX_ITR_
// do iterations
// norm(...,0) is infinity norm
num_its_done = 0;
while ( norm(resid,0) > resid_tol && num_its_done < _RBGS_MAX_ITR_) {
rbgauss_seidel_it(A,f,v);
resid = f-A*v;
++num_its_done;
}
}
else if ( num_itr >= 0 ) { // do exactly num_itr iterations
// do iterations
// norm(...,0) is infinity norm
unsigned num_its_todo = static_cast<unsigned>(num_itr);
num_its_done = 0;
while ( num_its_done < num_its_todo ) {
rbgauss_seidel_it(A,f,v);
++num_its_done;
}
}
else {
cerr << "error: classical_solvers:rbgauss_seidel: bad input number of "
<< "iterations" << endl;
exit(-1);
}
num_itr = static_cast<int>(num_its_done);
return v;
}
unsigned size() const {return f.size();}
void trace(double& tr,const valarray<double>& M){
int Dim = sqrt(double(M.size()));
tr = 0.0;
for(int i=0; i<Dim; ++i) tr+= M[Dim*i+i];
}
void setX(valarray<double> _x){
x.resize(_x.size());
x=_x;
};
void setY(valarray<double> _y){
y.resize(_y.size());
y=_y;
};
Apertures::Apertures(const Ntuple* nt,unsigned int verbose) // constructor
// 1st cleanup the aperture information directly from madx tfs and keep the result as vector<double> svec, aper_1_vec; --- more cleanup possible later
{
if(verbose) cout << __FILE__ << " " << __PRETTY_FUNCTION__ << " line " << __LINE__ << " constructor start verbose=" << verbose << " nt->Noent()=" << nt->Noent() << '\n';
// copy to local const valarray's
const valarray<double> s_array=nt->GetVar("S"); // http://www.cplusplus.com/reference/valarray/
const valarray<double> aper_1_array=nt->GetVar("APER_1");
const valarray<double> aper_2_array=nt->GetVar("APER_2");
const valarray<double> aper_3_array=nt->GetVar("APER_3");
const valarray<double> aper_4_array=nt->GetVar("APER_4");
bool has_betx=nt->VarExists("BETX");
bool has_bety=nt->VarExists("BETY");
valarray<double> betx_array(nt->Nvar()),bety_array(nt->Nvar());
if(has_betx) betx_array=nt->GetVar("BETX");
if(has_bety) bety_array=nt->GetVar("BETY");
const valarray<double> z_array=nt->GetVar("Z");
const valarray<double> y_array=nt->GetVar("Y"); // y directly
const valarray<double> x_2_array=nt->GetVar("X_2"); // x_2 is x from survey in global Euclidian coordinates
const valarray<double> aper_x_n = aper_1_array / sqrt(betx_array); // normalized aperture in x not working correctly in CINT
const valarray<double> aper_y_n = aper_1_array / sqrt(bety_array); // normalized aperture in y not working correctly in CINT
cout << std::defaultfloat;
bool HasX2=nt->VarExists("X_2");
if(verbose>2)
{
cout
<< " aper_x_n.size()=" << aper_x_n.size()
<< " aper_1_array[0]=" << aper_1_array[0]
<< " betx_array[0]=" << betx_array[0]
<< " aper_x_n[0]=" << aper_x_n[0]
<< '\n';
if(HasX2) cout << "X_2 exists" << '\n';
}
this->verbose=verbose;
if(verbose>1)
{
cout << "Apertures constructor"
<< " s_array.size()=" << s_array.size()
<< " aper_1_array.size()=" << aper_1_array.size() << '\n';
}
double aper_x_n_min=numeric_limits<double>::max();
double aper_y_n_min=numeric_limits<double>::max();
isigx_min=0;
isigy_min=0;
vector<string> nt_StrCol_Name=nt->GetStrCol("NAME");
vector<string> nt_StrCol_Keyw=nt->GetStrCol("KEYWORD");
vector<string> nt_StrCol_Type=nt->GetStrCol("APERTYPE");
if(verbose) cout
<< " nt_StrCol_Name.size()=" << nt_StrCol_Name.size()
<< " nt_StrCol_Keyw.size()=" << nt_StrCol_Keyw.size()
<< " nt_StrCol_Type.size()=" << nt_StrCol_Name.size()
<< '\n';
// verbose=3; // CSPE
for(unsigned int j=0;j<s_array.size();++j)
{
bool AperOK=aper_1_array[j]>0 && aper_1_array[j]<5;
if( AperOK ) // reasonable between 0 and 5 m, aperture information available
{
AperName.push_back(nt_StrCol_Name[j]); // The name should always be defined
if(j<nt_StrCol_Keyw.size()) AperKeyw.push_back(nt_StrCol_Keyw[j]);
if(j<nt_StrCol_Type.size()) AperType.push_back(nt_StrCol_Type[j]);
svec.push_back( s_array[j]);
yvec.push_back( y_array[j]);
aper_1_vec.push_back( aper_1_array[j]);
if(aper_2_array.size()>0) aper_2_vec.push_back( aper_2_array[j]);
if(aper_3_array.size()>0) aper_3_vec.push_back( aper_3_array[j]);
if(aper_4_array.size()>0) aper_4_vec.push_back( aper_4_array[j]);
if(HasX2)
{
zvec.push_back(z_array[j]);
x_2_vec.push_back(x_2_array[j]);
}
else
{
zvec.push_back(0);
x_2_vec.push_back(0);
}
if( aper_x_n[j]<aper_x_n_min )
{
aper_x_n_min=aper_x_n[j];
isigx_min=j;
if(verbose>2) cout << "new aper_x_n_min=" << aper_x_n_min << " at isigx_min=" << isigx_min << '\n';
}
if( aper_y_n[j]<aper_y_n_min )
{
aper_y_n_min=aper_y_n[j];
isigy_min=j;
if(verbose>2) cout << "new aper_y_n_min=" << aper_y_n_min << " at isigy_min=" << isigy_min << '\n';
}
}
}
if(verbose)
{
cout << "isigx_min=" << isigx_min << " isigy_min=" << isigy_min << '\n';
cout << "aper_x_n_min=" << aper_x_n_min << "/sqrt(m) at s=" << s_array[isigx_min] << " " << nt_StrCol_Name[isigx_min] << '\n';
cout << "aper_y_n_min=" << aper_y_n_min << "/sqrt(m) at s=" << s_array[isigy_min] << " " << nt_StrCol_Name[isigy_min] << '\n';
cout << "Apertures constructor end svec.size()=" << svec.size() << '\n';
}
}
void Plot_Bend_SR_Cones(const Ntuple& nt,const Beam &Mach,const double zmin,const double zmax,const double Scale_xy,const unsigned int verbose,const bool goto_CM_units,const double sign) // see also ~/root_git_build/tutorials/eve/jetcone.C
// -- draw tangential lines and SR cones towards z=0 w/o beam divergence, relevant for neutral tracking --- lines ok, cones shuld be improved
// done here in TEve. Alternative could be to create each cone as a geometry which could be saved as and reloaded, to allow for individual colors, names ..
{
if(verbose) cout << __FILE__ << " " << __PRETTY_FUNCTION__ << " line " << __LINE__ << " Scale_xy=" << Scale_xy << endl;
unsigned int n=nt.Noent();
vector<string> NameCol =nt.GetStrCol("NAME");
vector<string> KeywordCol=nt.GetStrCol("KEYWORD");
if(gEve==NULL) cout << " *** careful *** global gEve=" << gEve << " is not defined, some general features like setting colors may cause segmentation violation" << endl;
// use two line sets, one for start and one for end of bend
TEveStraightLineSet* eve_line_s = new TEveStraightLineSet("BendLines"); // can display lines together as set, but only save one by one seems http://root.cern.ch/root/html/TEveStraightLineSet.html
TEveStraightLineSet* eve_line_e = new TEveStraightLineSet("BendLines"); // can display lines together as set, but only save one by one seems http://root.cern.ch/root/html/TEveStraightLineSet.html
eve_line_s->SetLineWidth(2); eve_line_s->SetLineColor(kRed); // draw a red line from start of bend
eve_line_e->SetLineWidth(2); eve_line_e->SetLineColor(kGreen); // draw a green line from end of bend
double length=1;
Plot_axis_arrow("x",length,Scale_xy,verbose);
Plot_axis_arrow("y",length,Scale_xy,verbose);
if(zmax>20) length=100;
Plot_axis_arrow("z",length,Scale_xy,verbose);
// PlotCone
TEveStraightLineSet* eve_line_m = new TEveStraightLineSet("BendLines"); // middle vector
eve_line_m->SetLineWidth(1); eve_line_m->SetLineColor(kGray); eve_line_m->SetLineStyle(7); // https://root.cern.ch/root/html/TAttLine.html use 7 to get dashed
const vector<const char*> koord =GetKoordNames(nt);;
const valarray<double> xvec=nt.GetVar(koord[0]);
const valarray<double> yvec=nt.GetVar(koord[1]);
const valarray<double> zvec=nt.GetVar(koord[2]);
const valarray<double> angle=nt.GetVar("ANGLE");
const valarray<double> theta=nt.GetVar("THETA");
const valarray<double> phi=nt.GetVar("PHI");
const valarray<double> psi=nt.GetVar("PSI");
const valarray<double> EcritBend=nt.GetVar("EcritBend");
const valarray<double> ngamBend=nt.GetVar("ngamBend");
Vec3 z_unit_vec(0,0,1);
if(sign<0) z_unit_vec=-1.*z_unit_vec; // (0,0,-1)
if(verbose) cout << __FILE__ << " " << __FUNCTION__ << " line " << __LINE__ << setprecision(3) << " n=" << n << " yvec.size()=" << yvec.size() << '\n';
for(unsigned int i=1; i<n; ++i) // loop over elements
{
if(KeywordCol[i].find("BEND") !=string::npos) // sbend or rbend
{
Vec3 p0_s(xvec[i-1],yvec[i-1],zvec[i-1]); // Start of beand from previous element, ordered by increasing s
Vec3 p0_e( xvec[i], yvec[i], zvec[i]); // End of bend
if(verbose>1) cout << __FILE__ << " " << __FUNCTION__ << " line " << __LINE__ << " i=" << i << " zvec[i]=" << zvec[i] << '\n';
if(zvec[i] > zmin && zvec[i] < zmax) // in z within range
{
// power
double U0=ngamBend[i]*MeanSyn*EcritBend[i];
double PowBend=Mach.ibeam*1.e9*U0;
//---- cone declaration --- here in loop to get separate cones -- change color..
TEveBoxSet* cones = new TEveBoxSet(NameCol[i].c_str()); // see http://root.cern.ch/root/html/TEveBoxSet.html $EDITOR $ROOTSYS/tutorials/eve/boxset_cones.C
// cones->SetPickable(kTRUE);
cones->UseSingleColor(); // seems needed for Transparency, do not make shape too flat, then always grey
cones->SetMainColor(kGreen); // see $EDITOR ~/root_git/include/root/TEveDigitSet.h ~/root_git/src/graf3d/eve/src/TEveDigitSet.cxx void DigitColor(Color_t ci, Char_t transparency); https://root.cern.ch/root/html/TColor.html
Char_t MaxTransp=99; // seen in boxset.C boxset_single_color 50 is half transparent, 90 is very transparent https://root.cern.ch/root/html/TEveElement.html
Char_t DeltaTransp=1;
cones->SetMainTransparency(MaxTransp);
if(PowBend>1.e1) { cones->SetMainTransparency(MaxTransp-1*DeltaTransp); }
if(PowBend>1.e2) { cones->SetMainTransparency(MaxTransp-2*DeltaTransp); }
if(PowBend>1.e3) { cones->SetMainTransparency(MaxTransp-3*DeltaTransp); cones->SetMainColor(kRed-2); }
if(PowBend>1.e4) { cones->SetMainTransparency(MaxTransp-4*DeltaTransp); cones->SetMainColor(kRed-3); }
if(PowBend>1.e5) { cones->SetMainTransparency(MaxTransp-5*DeltaTransp); cones->SetMainColor(kRed-4); }
cones->Reset(TEveBoxSet::kBT_EllipticCone, kFALSE, 64); // EllipticCone
//-----
Mat3x3 WCS_s(WCS_mat3(theta[i-1],phi[i-1],psi[i-1])); // matrix, bend start
Mat3x3 WCS_e(WCS_mat3(theta[i], phi[i], psi[i])); // matrix, bend end
Vec3 dirvec_s=WCS_s*z_unit_vec; // tanget bend start
Vec3 dirvec_e=WCS_e*z_unit_vec; // tanget bend end
if(verbose>1) cout << left << setw(12) << NameCol[i] << setw(6) << " zvec[i]=" << setw(6) << zvec[i] << " dirvec_s " << dirvec_s.Print() << '\n' << WCS_s.Print();
if(verbose>1) cout << left << setw(12) << NameCol[i] << setw(6) << " zvec[i]=" << setw(6) << zvec[i] << " dirvec_e " << dirvec_e.Print() << '\n' << WCS_e.Print();
Vec3 p1_s=p0_s+dirvec_s; // start bend vector + direction unit vector
Vec3 p1_e=p0_e+dirvec_e; // end bend vector + direction unit vector
double len_s=1;
double len_e=1;
if(fabs(p1_e.r[2])<fabs(p0_e.r[2])) // get length to reach to 0 in z
{
len_s=fabs(p0_s.r[2]/(p1_s.r[2]-p0_s.r[2]));
len_e=fabs(p0_e.r[2]/(p1_e.r[2]-p0_e.r[2]));
if(verbose>1) cout << " pointing in z to 0 len_s=" << setw(10) << len_s << " p1_s.r[2]-p0_s.r[2]= " << setw(10) << p1_s.r[2]-p0_s.r[2] << '\n';
if(verbose>1) cout << " pointing in z to 0 len_e=" << setw(10) << len_e << " p1_e.r[2]-p0_e.r[2]= " << setw(10) << p1_e.r[2]-p0_e.r[2] << '\n';
}
p1_s=p0_s+len_s*dirvec_s;
p1_e=p0_e+len_e*dirvec_e;
if(verbose) cout << setw(5) << i << setw(15) << NameCol[i-1] << " line from " << p0_s.Print() << " to " << p1_s.Print() << " theta=" << setw(12) << theta[i-1] << '\n';
if(verbose) cout << setw(5) << i << setw(15) << NameCol[i] << " line from " << p0_e.Print() << " to " << p1_e.Print() << " theta=" << setw(12) << theta[i] << '\n';
if(fabs(p1_e.r[0])> zmax)
{
if(verbose) cout << " x1=p1_e.r[0]=" << p1_e.r[0] << " larger than zmax=" << zmax << " skip" << '\n';
continue;
}
if(abs(p1_s)>zmax || abs(p1_e)>zmax)
{
if(verbose) cout << " abs(p1_s)=" << abs(p1_s) << " abs(p1_e)=" << abs(p1_e) << " too large, skip" << '\n';
continue;
}
// now Scale_xy
p0_s.r[0]*=Scale_xy; p0_s.r[1]*=Scale_xy; p1_s.r[0]*=Scale_xy; p1_s.r[1]*=Scale_xy;
p0_e.r[0]*=Scale_xy; p0_e.r[1]*=Scale_xy; p1_e.r[0]*=Scale_xy; p1_e.r[1]*=Scale_xy;
Vec3 p0_m = 0.5*(p0_s+p0_e); // middle of magnet
Vec3 v_s=(p1_s-p0_m); // from middle of magnet to left end of cone, same length as v_e
Vec3 v_e= p1_e-p0_m; // from middle of magnet to right end of cone
v_s=(len_e/len_s)*v_s; // same unscaled length as v_e
if(verbose>1) cout << "p0_m magnet middle " << p0_m.Print() << '\n';
if(verbose>1) cout << "v_s to left end of cone " << v_s.Print() << " len=" << abs(v_s) << '\n';
if(verbose>1) cout << "v_e to right end of cone " << v_e.Print() << " len=" << abs(v_e) << '\n';
if(abs(v_s)>Scale_xy*zmax || abs(v_e)>Scale_xy*zmax)
{
if(verbose) cout << " abs(v_s)=" << abs(v_s) << " abs(v_e)=" << abs(v_e) << " too large, skip" << '\n';
continue;
}
// angle between vectors after scaling
double cos_angle_cone=(v_s*v_e)/(abs(v_s)*abs(v_e));
double angle_cone=acos(cos_angle_cone);
if(verbose) cout << " cos_angle_cone=" << cos_angle_cone << " angle_cone=" << angle_cone << " Scale_xy*angle[i]=" << Scale_xy*angle[i] << " v_s*v_e=" << v_s*v_e << " abs(v_s)=" << abs(v_s) << " abs(v_e)=" << abs(v_e)
<< " PowBend=" << PowerWithUnits(PowBend) << '\n' << '\n';
TEveVectorT<double> P0_s=to_TEveVector(p0_s), P1_s=to_TEveVector(p1_s);
TEveVectorT<double> P0_e=to_TEveVector(p0_e), P1_e=to_TEveVector(p1_e);
if(goto_CM_units) // change to CM just before plotting
{
P0_s*=100; P1_s*=100;
P0_e*=100; P1_e*=100;
}
eve_line_s->AddLine( P0_s,P1_s);
eve_line_e->AddLine( P0_e,P1_e);
// draw also cones
double cone_len=abs(v_s+v_e)/2;
if(goto_CM_units) cone_len*=100;
// middle vector
TEveVectorT<double> P0_m=0.5*(P0_s +P0_e);
TEveVectorT<double> P1_m=0.5*(P1_s +P1_e); // tricky part -- take simple approx
eve_line_m->AddLine( P0_m,P1_s );
eve_line_m->AddLine( P0_m,P1_e );
eve_line_m->AddLine( P0_m,P1_m );
double r1=angle_cone*cone_len/2;
double r2=r1/10; // color effects better visible when not too small
cones->AddEllipticCone(P0_m,P1_m-P0_m,r1,r2,0);
gEve->AddElement(cones);
}
}
}
gEve->AddElement(eve_line_s); // show start lines
gEve->AddElement(eve_line_e); // show end lines
gEve->AddElement(eve_line_m); // show middle lines
gEve->Redraw3D(kTRUE);
PlotGuidesAtOrigin();
}
unsigned dataSize(){return data.size();};
double fraction(const valarray<bool>& v) {
return fraction(count(v),v.size(),0);
}
double odds(const valarray<bool>& v) {
double y = count(v);
double n = v.size() - count(v);
return y/n;
}
valarray<T> wjacobi(const matrix_crs<T>& A, const valarray<T>& f,
const valarray<T>& v0, const T w, const T resid_tol, int& num_itr) {
// weighted Jacobi method
// pass in the matrix, RHS vector
// pass in an initial guess (there is another interface which creates a
// random initial guess)
// weight is optional. defaults to 2./3.
// resid_tol is the norm(.,0) residual tolerance
// num_itr is the number of iterations to do. defaults to -1, which will
// run until we reach the maximum number of iterations or reach tolerance
// On return, num_itr will have the number of iterations completed.
unsigned num_its_done;
// check sizes
if ( A.m != f.size() ) {
cerr << "error: classical_solvers:wjacobi: dimension mismatch" << endl;
exit(-1);
}
// initial guess
valarray<T> v = v0;
valarray<T> v_prev = v0;
valarray<T> resid = f-A*v;
// number of iterations to do
if ( num_itr == -1 ) { // just do normal solve until we reach tol or
// _WJ_MAX_ITR_
// do iterations
// norm(...,0) is infinity norm
num_its_done = 0;
while ( norm(resid,0) > resid_tol && num_its_done < _WJ_MAX_ITR_ ) {
v_prev = v;
wjacobi_it(A,f,v_prev,v,w);
resid = f-A*v;
++num_its_done;
}
}
else if ( num_itr >= 0 ) { // do exactly num_itr iterations
// do iterations
// norm(...,0) is infinity norm
unsigned num_its_todo = static_cast<unsigned>(num_itr);
num_its_done = 0;
while ( num_its_done < num_its_todo ) {
v_prev = v;
wjacobi_it(A,f,v_prev,v,w);
++num_its_done;
}
}
else {
cerr << "error: classical_solvers:wjacobi: bad input number of "
<< "iterations" << endl;
exit(-1);
}
num_itr = static_cast<int>(num_its_done);
return v;
}
valarray<T> operator-(valarray<T> const& lhs, valarray<T> const& rhs) {
valarray<T> result(std::min(lhs.size(), rhs.size()));
apply_op<std::minus<>>(result, lhs, rhs);
return result;
}
template<class T> void print(valarray<T> const &v) {ostringstream os; for(int i=0; i<v.size(); ++i){if(i)os<<' ';os<<v[i];} cout<<os.str()<<endl;}
void natural_spline(valarray<double>x, valarray<double> y, valarray<double>& b,valarray<double>& c,valarray<double>& d)
{
int nm1, i;
double t;
int n=x.size();
// x--; y--; b--; c--; d--;
if(n < 2) {
throw(domain_error("not enough number of points"));
return;
}
if(n < 3) {
t = (y[1] - y[0]);
b[0] = t / (x[1]-x[0]);
b[1] = b[0];
c[0] = c[1] = d[0] = d[1] = 0.0;
return;
}
nm1 = n - 2;
/* Set up the tridiagonal system */
/* b = diagonal, d = offdiagonal, c = right hand side */
d[0] = x[1] - x[0];
c[1] = (y[1] - y[0])/d[0];
for( i=1 ; i<n-1 ; i++) {
d[i] = x[i+1] - x[i];
b[i] = 2.0 * (d[i-1] + d[i]);
c[i+1] = (y[i+1] - y[i])/d[i];
c[i] = c[i+1] - c[i];
}
/* Gaussian elimination */
for(i=2 ; i<n-1 ; i++) {
t = d[i-1]/b[i-1];
b[i] = b[i] - t*d[i-1];
c[i] = c[i] - t*c[i-1];
}
/* Backward substitution */
c[nm1] = c[nm1]/b[nm1];
for(i=n-2-1 ; i>0 ; i--)
c[i] = (c[i]-d[i]*c[i+1])/b[i];
/* End conditions */
c[0] = c[n-1] = 0.0;
/* Get cubic coefficients */
b[0] = (y[1] - y[0])/d[0] - d[0] * c[1];
c[0] = 0.0;
d[0] = c[1]/d[0];
b[n-1] = (y[n-1] - y[nm1])/d[nm1] + d[nm1] * c[nm1];
// COUT<<"loop to Get cubic coefficients"<<endl;
for(i=1 ; i<n-1 ; i++) {
b[i] = (y[i+1]-y[i])/d[i] - d[i]*(c[i+1]+2.0*c[i]);
d[i] = (c[i+1]-c[i])/d[i];
c[i] = 3.0*c[i];
}
// COUT<<"end loop"<<endl;
c[n-1] = 0.0;
d[n-1] = 0.0;
}
valarray<double> safe_count(valarray<double> n)
{
for(int i=0; i<n.size(); i++)
if (n[i] < 1.0) n[i] = 1.0;
return n;
}
void ReadCLResult(valarray<cl_float> & output)
{
unsigned int test_size = output.size();
int err = CL_SUCCESS;
err = clEnqueueReadBuffer(cl->GetQueue(), output_cl, CL_TRUE, 0, sizeof(cl_float) * test_size, &output[0], 0, NULL, NULL);
}
void FullCovariance::calcCholesky( valarray<double>& VAChol ) const
{
//------------------------------------------------------------
// Preliminaries.
//------------------------------------------------------------
// Initially set the Cholesky factor Chol equal to the current value
// for the covariance matrix Cov.
doCov( VAChol );
DoubleMatrix dmatChol( VAChol, static_cast<int>(sqrt( static_cast<double>(VAChol.size()))) );
int nChol = dmatChol.nr();
assert( VAChol.size() == nChol * nChol );
//------------------------------------------------------------
// Validate the inputs (debug version only).
//------------------------------------------------------------
assert( dmatChol.nc() == nChol ); // Cov must be square.
assert( nChol > 0 ); // Cov must have at least one row.
assert( isSymmetric( dmatChol ) ); // Cov must be symmetric.
//------------------------------------------------------------
// Define the parameters for the function nag_real_cholesky (f03aec),
// which computes a Cholesky factorization of a real symmetric
// positive-definite matrix, and evaluates its determinant.
//------------------------------------------------------------
// Parameter: n.
// Input: n, the order of the matrix Cov.
// Output: unspecified.
// Constraint: n >= 1.
Integer n = nChol;
assert( n >= 1 );
// Parameter: tdchol.
// Input: the last dimension of the array chol as declared in the
// function from which nag_real_cholesky is called.
// Output: unspecified.
// Constraint: tdchol >= n.
Integer tdchol = n;
// Parameter: chol.
// Input: the upper triangle of the n by n positive-definite
// symmetric matrix Cov. The elements of the array below the
// diagonal need not be set.
// Output: the sub-diagonal elements of the lower triangular
// matrix Chol. The upper triangle of Cov is unchanged.
// Note: The data elements of DoubleMatrix matrices are stored
// in column-major order while NAG routines expect arrays to be
// stored in row-major order.
double* chol = dmatChol.data();
// Parameter: p
// Input: unspecified.
// Output: the reciprocals of the diagonal elements of Chol.
dvecP.resize( nChol, 1);
double* p = dvecP.data();
// Parameter: detf.
// Parameter: dete.
// Input: unspecified.
// Output: the determinant of Cov is given by detf * 2.0^dete.
// It is given in this form to avoid overflow or underflow.
double detf;
Integer dete;
//------------------------------------------------------------
// Perform the Cholesky factorization.
//------------------------------------------------------------
// Revisit - Exceptions - Mitch: if an error occurs in this
// NAG routine, the program will be stopped using exit or abort.
// Brad: changed to an assert for tracking in debugger 12/12/00
// Sachiko: Changed to exception throwing 10/15/2002
static NagError fail;
INIT_FAIL(fail);
nag_real_cholesky( n, chol, tdchol, p, &detf, &dete, &fail);
if( fail.code != NE_NOERROR )
{
switch( fail.code )
{
case NE_NOT_POS_DEF:
throw SpkException( SpkError::SPK_NOT_POS_DEF_ERR,
"The matrix is not positive-definite, possibly due to round-ing errors.",
__LINE__, __FILE__ );
break;
default:
throw SpkException( SpkError::SPK_UNKNOWN_ERR,
"Failed to compute a Cholesky factorization.",
__LINE__, __FILE__ );
break;
}
}
//------------------------------------------------------------
// Transform the Cholesky factor for output.
//------------------------------------------------------------
// ************************************************************
// * Note: The data elements of DoubleMatrix matrices are *
// * stored in column-major order while NAG routines expect *
// * arrays to be stored in row-major order. *
// ************************************************************
// Set the final values for the elements of the DoubleMatrix that
// contains the lower triangular Cholesky factor Chol.
for ( int i = 0; i < nChol; i++ )
{
// Set the diagonal elements.
chol[i + i * nChol] = 1.0 / p[i];
for ( int j = 0; j < i; j++ )
{
// Copy the super-diagonal elements to the sub-diagonal.
chol[i + j * nChol] = chol[j + i * nChol];
// Zero the super-diagonal elements.
chol[j + i * nChol] = 0.0;
}
}
VAChol = dmatChol.toValarray();
}
/*
* Compute the forces associated with each EdgePoint (bend or end point) along each each
* on the nodes/rectangles in the graph.
*/
void TopologyConstraints::computeForces(valarray<double>& gradient,
cola::SparseMap& hessian)
{
FILE_LOG(logDEBUG1) << "TopologyConstraints::computeForces";
SparseMapMap H(hessian);
ArrayMap<double> g(gradient);
const EdgePoint *u,*v,*w;
for(Edges::const_iterator i=edges.begin();i!=edges.end();i++) {
//printf("Straightening path:\n");
//edges[i]->print();
Edge* e=*i;
ConstEdgePoints path;
e->getPath(path);
unsigned n=path.size();
FILE_LOG(logDEBUG2) << " path: n="<<n;
COLA_ASSERT(n>=2);
double d=e->idealLength;
double weight=2.0/(d*d);
double dl=d-e->pathLength();
if(dl>=0) continue;
// first and last entries
// gradient
u=path[0]; v=path[1];
COLA_ASSERT(v->inSegment->length()>0);
double h=weight*hRuleD1(dim,u,v,dl);
H(u,u)+=h;
double g1=weight*dl*gRule1(dim,u,v);
g[u]-=g1;
if(n==2||dl>0) {
// rule 1
H(v,v)+=h;
g[v]+=g1;
H(u,v)-=h;
H(v,u)-=h;
continue;
}
u=path[n-2]; v=path[n-1];
g[v]+=weight*dl*gRule1(dim,u,v);
H(v,v)+=weight*hRuleD1(dim,u,v,dl);
// remaining diagonal entries
for(unsigned j=1;j<n-1;j++) {
u=path[j-1], v=path[j], w=path[j+1];
COLA_ASSERT(v->inSegment->length()>0);
COLA_ASSERT(w->inSegment->length()>0);
H(v,v)+=weight*hRuleD2(dim,u,v,w,dl);
g[v]+=weight*dl*gRule2(dim,u,v,w);
}
// off diagonal entries
// hRule 2
u=path[0], v=path[1], w=path[2];
h=weight*hRule2(dim,u,v,w,dl);
H(u,v)+=h;
H(v,u)+=h;
// hRule 3
u=path[n-3], v=path[n-2], w=path[n-1];
h=weight*hRule3(dim,u,v,w,dl);
H(v,w)+=h;
H(w,v)+=h;
// hRule 4
u=path[0], v=path[n-1];
h=weight*hRule4(dim,u,path[1],path[n-2],v);
H(u,v)+=h;
H(v,u)+=h;
if(n==3) continue;
for(unsigned j=2;j<n-1;j++) {
// hRule 5
u=path[0],v=path[j];
h=weight*hRule56(dim,u,path[1],path[j-1],v,path[j+1]);
H(u,v)+=h;
H(v,u)+=h;
// hRule 6
u=path[n-1], v=path[n-1-j];
h=weight*hRule56(dim,u,path[n-2],path[n-1-j-1],v,path[n-1-j+1]);
H(u,v)+=h;
H(v,u)+=h;
// hRule 7
u=path[j-1], v=path[j];
h=weight*hRule7(dim,path[j-2],u,v,path[j+1],dl);
H(u,v)+=h;
H(v,u)+=h;
}
for(unsigned j=1;j<n-3;j++) {
for(unsigned k=j+2;k<n-1;k++) {
u=path[j]; v=path[k];
h=weight*hRule8(dim,path[j-1],u,path[j+1],path[k-1],v,path[k+1]);
H(u,v)+=h;
H(v,u)+=h;
}
}
}
for(unsigned i=0;i<gradient.size();i++) {
COLA_ASSERT(gradient[i]==gradient[i]);
}
/*
for(unsigned i=0;i<edges.size();i++) {
Edge* e=edges[i];
vector<unsigned>& path=e->path;
unsigned n=path.size();
printf("d=%f;\n",e->idealLength);
printf("X={");
for(unsigned j=0;j<n;j++) {
printf("%f",nodes[path[j]]->x);
if(j<n-1) {
printf(",");
}
}
printf("};\n");
printf("Y={");
for(unsigned j=0;j<n;j++) {
printf("%f",nodes[path[j]]->y);
if(j<n-1) {
printf(",");
}
}
printf("};\n");
printf("H=\n");
for(unsigned j=0;j<n;j++) {
for(unsigned k=0;k<n;k++) {
unsigned u=path[j], v=path[k];
printf("%f ",H(u,v));
}
printf("\n");
}
printf("g=");
for(unsigned j=0;j<n;j++) {
printf("%f ",g[path[j]]);
}
printf("\n");
}
*/
}
/*
* Use gradient-projection to solve an instance of
* the Variable Placement with Separation Constraints problem.
*/
unsigned GradientProjection::solve(
valarray<double> const &linearCoefficients,
valarray<double> &x) {
COLA_ASSERT(linearCoefficients.size()==x.size());
COLA_ASSERT(x.size()==denseSize);
COLA_ASSERT(numStaticVars>=denseSize);
COLA_ASSERT(sparseQ==nullptr ||
(sparseQ!=nullptr && (vars.size()==sparseQ->rowSize())) );
if(max_iterations==0) return 0;
bool converged=false;
solver = setupVPSC();
#ifdef MOSEK_AVAILABLE
if(solveWithMosek==Outer) {
float* ba=new float[vars.size()];
float* xa=new float[vars.size()];
for(unsigned i=0;i<vars.size();i++) {
ba[i]=-linearCoefficients[i];
}
mosek_quad_solve_sep(menv,ba,xa);
for(unsigned i=0;i<vars.size();i++) {
//printf("mosek result x[%d]=%f\n",i,xa[i]);
x[i]=xa[i];
}
delete [] ba;
delete [] xa;
return 1;
}
#endif
// it may be that we have to consider dummy vars, which the caller didn't know
// about. Thus vars.size() may not equal x.size()
unsigned n = vars.size();
valarray<double> b(n);
result.resize(n);
// load desired positions into vars, note that we keep desired positions
// already calculated for dummy vars
for (unsigned i=0;i<x.size();i++) {
COLA_ASSERT(!isNaN(x[i]));
COLA_ASSERT(isFinite(x[i]));
b[i]=i<linearCoefficients.size()?linearCoefficients[i]:0;
result[i]=x[i];
if(scaling) {
b[i]*=vars[i]->scale;
result[i]/=vars[i]->scale;
}
if(!vars[i]->fixedDesiredPosition) vars[i]->desiredPosition=result[i];
}
runSolver(result);
valarray<double> g(n); /* gradient */
valarray<double> previous(n); /* stored positions */
valarray<double> d(n); /* actual descent vector */
#ifdef CHECK_CONVERGENCE_BY_COST
double previousCost = DBL_MAX;
#endif
unsigned counter=0;
double stepSize;
for (; counter<max_iterations&&!converged; counter++) {
previous=result;
stepSize=0;
double alpha=computeSteepestDescentVector(b,result,g);
//printf("Iteration[%d]\n",counter);
// move to new unconstrained position
for (unsigned i=0; i<n; i++) {
// dividing by variable weight is a cheap trick to make these
// weights mean something in terms of the descent vector
double step=alpha*g[i]/vars[i]->weight;
result[i]+=step;
//printf(" after unconstrained step: x[%d]=%f\n",i,result[i]);
stepSize+=step*step;
COLA_ASSERT(!isNaN(result[i]));
COLA_ASSERT(isFinite(result[i]));
if(!vars[i]->fixedDesiredPosition) vars[i]->desiredPosition=result[i];
}
//project to constraint boundary
bool constrainedOptimum = false;
constrainedOptimum=runSolver(result);
stepSize=0;
for (unsigned i=0;i<n;i++) {
double step = previous[i]-result[i];
stepSize+=step*step;
}
//constrainedOptimum=false;
// beta seems, more often than not, to be >1!
if(constrainedOptimum) {
// The following step limits the step-size in the feasible
// direction
d = result - previous;
const double beta = 0.5*computeStepSize(g, d);
// beta > 1.0 takes us back outside the feasible region
// beta < 0 clearly not useful and may happen due to numerical imp.
//printf("beta=%f\n",beta);
if(beta>0&&beta<0.99999) {
stepSize=0;
for (unsigned i=0; i<n; i++) {
double step=beta*d[i];
result[i]=previous[i]+step;
stepSize+=step*step;
}
}
}
#ifdef CHECK_CONVERGENCE_BY_COST
/* This would be the slow way to detect convergence */
//if(counter%2) {
double cost = computeCost(b,result);
printf(" gp[%d] %.15f %.15f\n",counter,previousCost,cost);
//COLA_ASSERT(previousCost>cost);
if(fabs(previousCost - cost) < tolerance) {
converged = true;
}
previousCost = cost;
//}
#else
if(stepSize<tolerance) converged = true;
#endif
}
//printf("GP[%d] converged after %d iterations.\n",k,counter);
for(unsigned i=0;i<x.size();i++) {
x[i]=result[i];
if(scaling) {
x[i]*=vars[i]->scale;
}
}
destroyVPSC(solver);
return counter;
}
valarray(const valarray<T> &other)
: m_buffer(other.m_buffer.get_context(), other.size() * sizeof(T))
{
}
inline void func(valarray<double> &r, valarray<double> &k, const int dim)
{
//constants
const double m = 1.0;
const double c = 1.0;
const double e = -1.0;
const double fac = 1. / (2.* m * m * m * c * c);
const double fac1 = 1. / (2.* m * m * c * c);
int k_size = k.size();
//reset k vector
for (int i = 0; i < k_size; i++)
{
k[i] = 0.0;
}
//initialize vector to contain n_ij values
double *n_ij = new double[dim];
//get number of particles
const int N_p = r.size() / (2 * dim);
//arrays arrive in 1D form, where an element is identified by: i_par * 2 * dim + j_dim
for (int i = 0; i < N_p; i++)
{
//calculate p_i^2
double p_sq = 0.0;
for (int d = 0; d < dim; d++)
{
p_sq += r[(2 * i + 1) * dim + d] * r[(2 * i + 1) * dim + d];
}
//evaluate equations of motion: -dH/dx = dp/dt; dH/dp = dx/dt
for (int d = 0; d < dim; d++)
{
//dH/dp, 1st part
k[i * 2 * dim + d] = r[(2 * i + 1) * dim + d] / m - p_sq * fac * r[(2 * i + 1) * dim + d];
}
//sum part of eom evaluation. Note: this probably needs a more efficient implementation
for (int j = 0; j < N_p; j++)
{
double R_ij = 0.0;
double pjdotn = 0.0;
double pidotn = 0.0;
double pidotpj = 0.0;
//don't count ith particle
if (i != j)
{
//calculate R_ij
for (int d = 0; d < dim; d++)
{
R_ij += (r[i * 2 * dim + d] - r[j * 2 * dim + d]) * (r[i * 2 * dim + d] - r[j * 2 * dim + d]);
}
R_ij = sqrt(R_ij);
//calculate n_ij[d]
for (int d = 0; d < dim; d++)
{
n_ij[d] = (r[i * 2 * dim + d] - r[j * 2 * dim + d]) / R_ij;
}
//calculate some dot products
for (int d = 0; d < dim; d++)
{
pjdotn += n_ij[d] * r[(2 * j + 1) * dim + d];
pidotn += n_ij[d] * r[(2 * i + 1) * dim + d];
pidotpj += r[(2 * i + 1) * dim + d] * r[(2 * j + 1) * dim + d];
}
for (int d = 0; d < dim; d++)
{
//rest of dH/dp
k[i * 2 * dim + d] -= (e * e / R_ij) * fac1 * (r[(2 * j + 1) * dim + d] + pjdotn * n_ij[d]);
//-dH/dx
double A = (e * e) / (R_ij * R_ij);
k[(2 * i + 1)*dim + d] += A * n_ij[d] * (1. - fac1 * pidotpj) - 3.*fac1 * A * n_ij[d] * pidotn * pjdotn;
k[(2 * i + 1)*dim + d] += fac1 * A * (r[(2 * i + 1) * dim + d] * pjdotn + r[(2 * j + 1) * dim + d] * pidotn);
}
}
}
}
//free n_ij
delete [] n_ij;
n_ij = NULL;
}
pair<double, double> mean_variance(valarray<T> &sample) {
unsigned int size = sample.size();
double mean = sample_mean_sub(sample, 0, size-1);
double variance = sample_variance_sub(mean, sample, 0, size-1);
return pair<double, double>(mean, variance);
}