double HFSolve::calc_energy(mat C, basis Bs){
mat Ci = zeros(Nstates,Nstates);
double s;
for(int p= 0; p<Nstates;p++){
for(int q=0; q<Nstates; q++){
s = 0;
for(int k=0; k<N; k++){
s+=C.at(k,p) * C.at(k,q);
Ci.at(p,q) = s;
}
}
}
double Energy = 0;
for(int alpha = 0; alpha < Nstates; alpha++){
Energy += Bs.h0(alpha,alpha)*Ci.at(alpha,alpha);
for(int beta = 0; beta < Nstates; beta++){
for(int gamma = 0; gamma<Nstates; gamma++){
for(int delta = 0; delta <Nstates; delta++){
Energy += 0.5 * Ci.at(alpha, gamma) * Ci.at(beta, delta) * Bs.V(alpha, beta)(gamma, delta);
}
}
}
}
return Energy;
}
void pushMatrix_row(mat& MATRIX, mat ToBePushed) {
// cout<< "MATRIX" << MATRIX.n_rows<< ", " <<MATRIX.n_cols <<endl;
// cout<< "ToBePushed" << ToBePushed.n_rows << ", " << ToBePushed.n_cols <<endl;
if (MATRIX.is_empty()) {
// cout <<__LINE__<<endl;
MATRIX.resize(ToBePushed.n_rows, ToBePushed.n_cols);
for(int i = 0; i < ToBePushed.n_rows; i ++) {
for (int j = 0; j < ToBePushed.n_cols; j ++) {
MATRIX.at(i, j) = ToBePushed.at(i, j);
}
}
return;
}
if (MATRIX.n_rows != ToBePushed.n_rows) {
cout << " wrong dimension!" <<endl;
abort();
}
mat tmp;
tmp.zeros(MATRIX.n_rows, MATRIX.n_cols + ToBePushed.n_cols);
// cout<<"tmp: " << tmp.n_rows <<", " <<tmp.n_cols <<endl;
for(int i = 0; i < MATRIX.n_rows; i ++) {
// cout << i <<endl;
for(int j = 0; j < MATRIX.n_cols; j ++) {
tmp.at(i,j) = MATRIX.at(i,j);
}
}
for(int i = 0; i < ToBePushed.n_rows; i ++) {
for(int j = 0; j < ToBePushed.n_cols; j ++) {
tmp.at(i, j + MATRIX.n_cols) = ToBePushed.at(i, j);
// cout<<__LINE__<<endl;
}
}
MATRIX = tmp;
}
void blkdiag(mat A, mat B, mat& C) {
mat C_tmp;
C_tmp.resize(A.n_rows + B.n_rows, A.n_cols + B.n_cols);
C_tmp.zeros();
for( int i = 0; i < A.n_rows; i ++) {
for (int j = 0; j < A.n_cols; j ++) {
C_tmp.at(i,j) = A.at(i,j);
}
}
for( int i = 0; i < B.n_rows; i ++) {
for( int j = 0; j < B.n_cols; j ++ ) {
C_tmp.at( i + A.n_rows, j + A.n_cols ) = B.at(i,j);
}
}
C = C_tmp;
}
mat abs(const mat &m) {
mat res(m.n_rows, m.n_cols);
uint size = m.n_cols * m.n_rows;
for(uint i = 0; i < size; i++) {
res.at(i) = (m.at(i) <= 0.0) ? 0.0 - m.at(i) : m.at(i);
}
return res;
}
void pushMatrix_col(mat& MATRIX, mat ToBePushed) {
if (MATRIX.is_empty()) {
MATRIX.resize(ToBePushed.n_rows, ToBePushed.n_cols);
for(int i = 0; i < ToBePushed.n_rows; i ++) {
for (int j = 0; j < ToBePushed.n_cols; j ++) {
MATRIX.at(i, j) = ToBePushed.at(i, j);
}
}
return;
}
if(MATRIX.n_cols != ToBePushed.n_cols) {
cout << " wrong dimension!" <<endl;
abort();
}
mat tmp;
tmp.zeros(MATRIX.n_rows + ToBePushed.n_rows, MATRIX.n_cols);
for (int i = 0; i < MATRIX.n_rows; i ++) {
for (int j = 0; j < MATRIX.n_cols; j ++) {
tmp.at(i,j) = MATRIX.at(i,j);
}
}
for (int i = 0; i < ToBePushed.n_rows; i ++) {
for ( int j = 0; j < ToBePushed.n_rows; j ++) {
tmp.at(i + MATRIX.n_rows, j) = ToBePushed.at(i,j);
}
}
MATRIX = tmp;
}
void updateStateThread() {
while(0 == lcm1.handle()) {
x0.at(0,0) = current_state.position[0]/1000.0;
x0.at(3,0) = current_state.position[1]/1000.0;
x0.at(6,0) = -(current_state.position[2]/1000.0 + simu_height) + better_height;
x0.at(1,0) = current_state.velocity[0];
x0.at(4,0) = current_state.velocity[1];
x0.at(7,0) = -current_state.velocity[2];
accumu_err_x = accumu_err_x + x0.at(0,0) * dT;
accumu_err_y = accumu_err_y + x0.at(3,0) * dT;
accumu_err_z = accumu_err_z + x0.at(6,0) * dT;
x0.at(2,0) = accumu_err_x;
x0.at(5,0) = accumu_err_y;
x0.at(8,0) = accumu_err_z;
usleep(5e4);
// cout <<__LINE__<<endl;
}
}
double sum(const mat &m) {
double res = 0.0;
uint size = m.n_cols * m.n_rows;
for(uint i = 0; i < size; i++) {
res += m.at(i);
}
return res;
}
void bldMatrixDiag(float *Vec, mat& MATRIX, int Length) {
MATRIX.zeros(Length, Length);
for (int i = 0; i < Length; i ++) {
MATRIX.at(i,i) = Vec[i];
}
}
void insertColIntoMatrix(int c, const vec& v, mat& m) {
assert(v.n_rows == m.n_rows);
for(uint r = 0; r < v.n_rows; r++) {
m.at(r,c) = v.at(r);
}
}
inline
typename detail::enable_if_c< ( N > 4 ),
typename detail::enable_if< is_floating_point< T >, T >::type
>::type
invert( mat< T, N >& m )
{
mat< T, N > res = mat< T, N >::eye;
T det = static_cast< T >( 1 );
for ( std::size_t i = 0; i < N; ++i )
{
T min = static_cast< T >( -1 );
std::size_t min_index = 0;
for ( std::size_t r = i; r < N; ++r )
{
T curr = std::abs( m.at( r, i ) );
if ( curr > min )
{
min = curr;
min_index = r;
}
}
if ( min <= std::numeric_limits< T >::epsilon() )
{
return 0;
}
if ( min_index != i )
{
m.swap_rows( i, min_index, i, N-1 );
res.swap_rows( i, min_index );
det = -det;
}
T pivot = m.at( i, i );
if ( std::abs( pivot ) <= std::numeric_limits< T >::epsilon() )
{
return 0;
}
det *= pivot;
T inv_pivot = static_cast< T >( 1 ) / pivot;
for ( std::size_t k = i + 1; k < N; ++k )
{
m.at( i, k ) *= inv_pivot;
}
for ( std::size_t k = 0; k < N; ++k )
{
res.at( i, k ) *= inv_pivot;
}
for ( std::size_t j = i + 1; j < N; ++j )
{
T v = m.at( j, i );
for ( std::size_t k = i + 1; k < N; ++k )
{
m.at( j, k ) -= m.at( i, k ) * v;
}
for ( std::size_t k = 0; k < N; ++k )
{
res.at( j, k ) -= res.at( i, k ) * v;
}
}
}
for ( std::size_t i = N-1; i > 0; --i )
{
for ( std::size_t j = 0; j < i; ++j )
{
T v = m.at( j, i );
for ( std::size_t k = 0; k < N; ++k )
{
res.at( j, k ) -= res.at( i, k ) * v;
}
}
}
m = res;
return det;
}