void test_tovector(quotek::data::records& recs) {
Eigen::VectorXd v = recs.to_vector();
assert( v.rows() == 1600 );
assert(v.row(1599)[0] = 1599);
}
void ExperimentalTrajectory::addNewWaypoint(Eigen::VectorXd newWaypoint, double waypointTime)
{
std::cout << "Adding waypoint at: " << waypointTime << " seconds..." << std::endl;
if ( (newWaypoint.rows() == nDoF) && (waypointTime>=0.0) && (waypointTime<=kernelCenters.maxCoeff()) )
{
//Find out where the new T falls...
for (int i=0; i<kernelCenters.size(); i++)
{
std::cout << "i = " << i << std::endl;
if (kernelCenters(i)>waypointTime) {
youngestWaypointIndex=i;
std::cout << "youngestWaypointIndex" << youngestWaypointIndex << std::endl;
i = kernelCenters.size();
}
}
Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints+1);
newWaypointsMat.leftCols(youngestWaypointIndex) = waypoints.leftCols(youngestWaypointIndex);
newWaypointsMat.col(youngestWaypointIndex) = newWaypoint;
newWaypointsMat.rightCols(nWaypoints - youngestWaypointIndex) = waypoints.rightCols(nWaypoints - youngestWaypointIndex);
waypoints.resize(nDoF, nWaypoints+1);
waypoints = newWaypointsMat;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints);
std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;
durationVectorTmp.head(youngestWaypointIndex-1) = pointToPointDurationVector.head(youngestWaypointIndex-1);
durationVectorTmp.row(youngestWaypointIndex-1) << waypointTime - kernelCenters(youngestWaypointIndex-1);
durationVectorTmp.tail(nWaypoints - youngestWaypointIndex) = pointToPointDurationVector.tail(nWaypoints - youngestWaypointIndex);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;
reinitialize();
}
else{
if (newWaypoint.rows() != nDoF){
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint is not the right size. Should have dim = " <<nDoF << "x1." << std::endl;
}
if ((waypointTime<=0.0) || (waypointTime>=kernelCenters.maxCoeff())){
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of time bounds. Should have fall between 0.0 and " << kernelCenters.maxCoeff() << " seconds." << std::endl;
}
}
}
void Lanczos_Diag::Diagonalize(const Hamiltonian &Ham, Hamiltonian &tb)
{
bool Converged = false; //used to exit while loop should I set it to false here?
Eigen::MatrixXd Work_Mat;
//Eigen::VectorXd Eval;
Eigen::VectorXd Eval_P;
std::vector<Eigen::VectorXd> K_Mat;
Eigen::MatrixXd Evec_Mat;
Eigen::VectorXd Eval;
Eigen::VectorXd Evec;
//#ifdef TESTMAT
//Lanczos_Vec = Test_Lanczos;
// Eigen::Tridiagonalization<Eigen::MatrixXd> triOfHam(Test_Ham);
//#endif
//cout << "Lanczos not norm: " << Lanczos_Vec << endl;
Lanczos_Vec.normalize();
//cout << "Lanczos norm: " << Lanczos_Vec << endl;
K_Mat.push_back(Lanczos_Vec);//error here
int it = 0;
do
{
if(it == 0)
{
r_vec = Ham.Ham_Tot*K_Mat[it]; //how can I use ifdef here?
//#ifdef TESTMAT
//r_vec = Test_Ham*K_Mat[it];
//#endif
}
else
{
r_vec = Ham.Ham_Tot*K_Mat[it]-(beta*K_Mat[it-1]);
// #ifdef TESTMAT
// r_vec = Test_Ham*K_Mat[it]-(beta*K_Mat[it-1]);
// #endif
//cout << "Beta is: " << beta << endl;
}
// cout << K_Mat[it] << " " << K_Mat[it].adjoint() << endl;
alpha = K_Mat[it].dot( r_vec );//this is correct
//cout << "alpha: " << alpha << endl;
r_vec -= (alpha.real()*K_Mat[it]); //= r_vec changed to -=
beta = r_vec.norm();
//cout << "Beta: " << beta << endl;
TriDiag(it,it) = alpha.real();
TriDiag(it+1,it)=beta; //self adjoint eigensolver only uses lower triangle
TriDiag(it,it+1)=beta;
r_vec.normalize(); //this is the new Lanczos Vector
K_Mat.push_back(r_vec);
it++;
if (it > 1)
{
Work_Mat = TriDiag.block(0,0,it,it);
//cout <<"Work mat: \n" << Work_Mat << endl;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> DiagMe(Work_Mat);
Eval = DiagMe.eigenvalues();
Evec_Mat = DiagMe.eigenvectors();
//cout << "Eigenvalues on " << it << " iteration /n" << Eval << endl;
if(it > 2)
{
double Eval_Dif = Eval_P(0) - Eval(0);
if(Eval_Dif < .0000000000001)
{
cout << "Dif of Eval is zero \n";
Converged = true;
}
}
Eval_P = Eval;
}
if( (beta < .00000000000001) )
{
cout << "Beta is zero \n";
Converged = true;
}
if(it == (tb.Tot_base-2)|| it == itmax)
{
cout << "Not Converged \n";
Converged = true;
}
}while(!(Converged));
//cout << "T mat: " << TriDiag << endl;
cnt = it;
Evec.resize(cnt);
Evec = Evec_Mat.col(0);
TriDiag.resize(10,10);
//cout << "We have Eigenvectors \n" << Evec_Mat << endl;
for(int i = 0; i < cnt; i++)
{
G_state.real() += K_Mat[i]*Evec.row(i);//Evec.row(i)**K_Mat[i]
}
}
