// Calculate Bernstein basis
double Bernstein(int n, int i, double t)
{
double basis;
double ti; /* t^i */
double tni; /* (1 - t)^i */
if (t == 0.0 && i == 0) ti = 1.0; else ti = pow(t, i);
if (n == i && t == 1.0) tni = 1.0; else tni = pow((1 - t), (n - i));
basis = Ni(n, i) * ti * tni;
return basis;
}
SUMOReal Basis(int n,int i,SUMOReal t) {
SUMOReal basis;
SUMOReal ti; /* this is t^i */
SUMOReal tni; /* this is (1 - t)^i */
/* handle the special cases to avoid domain problem with pow */
if (t==0. && i == 0) ti=1.0;
else ti = pow(t,i);
if (n==i && t==1.) tni=1.0;
else tni = pow((1-t),(n-i));
basis = Ni(n,i)*ti*tni; /* calculate Bernstein basis function */
return basis;
}
int GModel::readP3D(const std::string &name)
{
FILE *fp = Fopen(name.c_str(), "r");
if(!fp) {
Msg::Error("Unable to open file '%s'", name.c_str());
return 0;
}
int numBlocks = 0;
if(fscanf(fp, "%d", &numBlocks) != 1 || numBlocks <= 0) {
fclose(fp);
return 0;
}
std::vector<int> Ni(numBlocks), Nj(numBlocks), Nk(numBlocks);
for(int n = 0; n < numBlocks; n++)
if(fscanf(fp, "%d %d %d", &Ni[n], &Nj[n], &Nk[n]) != 3) {
fclose(fp);
return 0;
}
for(int n = 0; n < numBlocks; n++) {
if(Nk[n] == 1) {
GFace *gf = new discreteFace(this, getMaxElementaryNumber(2) + 1);
add(gf);
gf->transfinite_vertices.resize(Ni[n]);
for(int i = 0; i < Ni[n]; i++) gf->transfinite_vertices[i].resize(Nj[n]);
for(int coord = 0; coord < 3; coord++) {
for(int j = 0; j < Nj[n]; j++) {
for(int i = 0; i < Ni[n]; i++) {
double d;
if(fscanf(fp, "%lf", &d) != 1) {
fclose(fp);
return 0;
}
if(coord == 0) {
MVertex *v = new MVertex(d, 0., 0., gf);
gf->transfinite_vertices[i][j] = v;
gf->mesh_vertices.push_back(v);
}
else if(coord == 1) {
gf->transfinite_vertices[i][j]->y() = d;
}
else if(coord == 2) {
gf->transfinite_vertices[i][j]->z() = d;
}
}
}
}
for(std::size_t i = 0; i < gf->transfinite_vertices.size() - 1; i++)
for(std::size_t j = 0; j < gf->transfinite_vertices[0].size() - 1; j++)
gf->quadrangles.push_back(new MQuadrangle(
gf->transfinite_vertices[i][j], gf->transfinite_vertices[i + 1][j],
gf->transfinite_vertices[i + 1][j + 1],
gf->transfinite_vertices[i][j + 1]));
}
else {
GRegion *gr = new discreteRegion(this, getMaxElementaryNumber(3) + 1);
add(gr);
gr->transfinite_vertices.resize(Ni[n]);
for(int i = 0; i < Ni[n]; i++) {
gr->transfinite_vertices[i].resize(Nj[n]);
for(int j = 0; j < Nj[n]; j++) {
gr->transfinite_vertices[i][j].resize(Nk[n]);
}
}
for(int coord = 0; coord < 3; coord++) {
for(int k = 0; k < Nk[n]; k++) {
for(int j = 0; j < Nj[n]; j++) {
for(int i = 0; i < Ni[n]; i++) {
double d;
if(fscanf(fp, "%lf", &d) != 1) {
fclose(fp);
return 0;
}
if(coord == 0) {
MVertex *v = new MVertex(d, 0., 0., gr);
gr->transfinite_vertices[i][j][k] = v;
gr->mesh_vertices.push_back(v);
}
else if(coord == 1) {
gr->transfinite_vertices[i][j][k]->y() = d;
}
else if(coord == 2) {
gr->transfinite_vertices[i][j][k]->z() = d;
}
}
}
}
}
for(std::size_t i = 0; i < gr->transfinite_vertices.size() - 1; i++)
for(std::size_t j = 0; j < gr->transfinite_vertices[0].size() - 1; j++)
for(std::size_t k = 0; k < gr->transfinite_vertices[0][0].size() - 1;
k++)
gr->hexahedra.push_back(
new MHexahedron(gr->transfinite_vertices[i][j][k],
gr->transfinite_vertices[i + 1][j][k],
gr->transfinite_vertices[i + 1][j + 1][k],
gr->transfinite_vertices[i][j + 1][k],
gr->transfinite_vertices[i][j][k + 1],
gr->transfinite_vertices[i + 1][j][k + 1],
gr->transfinite_vertices[i + 1][j + 1][k + 1],
gr->transfinite_vertices[i][j + 1][k + 1]));
}
}
fclose(fp);
return 1;
}
int main(int argc, char const *argv[]) {
Eigen::setNbThreads(NumCores);
#ifdef MKL
mkl_set_num_threads(NumCores);
#endif
INFO("Eigen3 uses " << Eigen::nbThreads() << " threads.");
int L;
RealType J12ratio;
int OBC;
int N;
RealType Uin, phi;
std::vector<RealType> Vin;
LoadParameters( "conf.h5", L, J12ratio, OBC, N, Uin, Vin, phi);
HDF5IO file("BSSH.h5");
// const int L = 5;
// const bool OBC = true;
// const RealType J12ratio = 0.010e0;
INFO("Build Lattice - ");
std::vector<ComplexType> J;
if ( OBC ){
J = std::vector<ComplexType>(L - 1, ComplexType(1.0, 0.0));
for (size_t cnt = 0; cnt < L-1; cnt+=2) {
J.at(cnt) *= J12ratio;
}
} else{
J = std::vector<ComplexType>(L, ComplexType(1.0, 0.0));
for (size_t cnt = 0; cnt < L; cnt+=2) {
J.at(cnt) *= J12ratio;
}
if ( std::abs(phi) > 1.0e-10 ){
J.at(L-1) *= exp( ComplexType(0.0e0, 1.0e0) * phi );
// INFO(exp( ComplexType(0.0e0, 1.0e0) * phi ));
}
}
for ( auto &val : J ){
INFO_NONEWLINE(val << " ");
}
INFO("");
const std::vector< Node<ComplexType>* > lattice = NN_1D_Chain(L, J, OBC);
file.saveNumber("1DChain", "L", L);
file.saveNumber("1DChain", "U", Uin);
file.saveStdVector("1DChain", "J", J);
for ( auto < : lattice ){
if ( !(lt->VerifySite()) ) RUNTIME_ERROR("Wrong lattice setup!");
}
INFO("DONE!");
INFO("Build Basis - ");
// int N1 = (L+1)/2;
Basis B1(L, N);
B1.Boson();
// std::vector< std::vector<int> > st = B1.getBStates();
// std::vector< RealType > tg = B1.getBTags();
// for (size_t cnt = 0; cnt < tg.size(); cnt++) {
// INFO_NONEWLINE( std::setw(3) << cnt << " - ");
// for (auto &j : st.at(cnt)){
// INFO_NONEWLINE(j << " ");
// }
// INFO("- " << tg.at(cnt));
// }
file.saveNumber("1DChain", "N", N);
// file.saveStdVector("Basis", "States", st);
// file.saveStdVector("Basis", "Tags", tg);
INFO("DONE!");
INFO_NONEWLINE("Build Hamiltonian - ");
std::vector<Basis> Bases;
Bases.push_back(B1);
Hamiltonian<ComplexType> ham( Bases );
std::vector< std::vector<ComplexType> > Vloc;
std::vector<ComplexType> Vtmp;//(L, 1.0);
for ( RealType &val : Vin ){
Vtmp.push_back((ComplexType)val);
}
Vloc.push_back(Vtmp);
std::vector< std::vector<ComplexType> > Uloc;
// std::vector<ComplexType> Utmp(L, ComplexType(10.0e0, 0.0e0) );
std::vector<ComplexType> Utmp(L, (ComplexType)Uin);
Uloc.push_back(Utmp);
ham.BuildLocalHamiltonian(Vloc, Uloc, Bases);
ham.BuildHoppingHamiltonian(Bases, lattice);
ham.BuildTotalHamiltonian();
INFO("DONE!");
INFO_NONEWLINE("Diagonalize Hamiltonian - ");
std::vector<RealType> Val;
Hamiltonian<ComplexType>::VectorType Vec;
ham.eigh(Val, Vec);
INFO("GS energy = " << Val.at(0));
file.saveVector("GS", "EVec", Vec);
file.saveStdVector("GS", "EVal", Val);
INFO("DONE!");
std::vector<ComplexType> Nbi = Ni( Bases, Vec );
for (auto &n : Nbi ){
INFO( n << " " );
}
ComplexMatrixType Nij = NiNj( Bases, Vec );
INFO(Nij);
INFO(Nij.diagonal());
file.saveStdVector("Obs", "Nb", Nbi);
file.saveMatrix("Obs", "Nij", Nij);
return 0;
}
int main(int argc, char const *argv[]) {
Eigen::setNbThreads(NumCores);
#ifdef MKL
mkl_set_num_threads(NumCores);
#endif
INFO("Eigen3 uses " << Eigen::nbThreads() << " threads.");
int L;
RealType J12ratio;
int OBC;
int N1, N2;
int dynamics, Tsteps;
RealType Uin, phi, dt;
std::vector<RealType> Vin;
LoadParameters( "conf.h5", L, J12ratio, OBC, N1, N2, Uin, Vin, phi, dynamics, Tsteps, dt);
HDF5IO *file = new HDF5IO("FSSH.h5");
// const int L = 5;
// const bool OBC = true;
// const RealType J12ratio = 0.010e0;
INFO("Build Lattice - ");
std::vector<DT> J;
if ( OBC ){
// J = std::vector<DT>(L - 1, 0.0);// NOTE: Atomic limit testing
J = std::vector<DT>(L - 1, 1.0);
if ( J12ratio > 1.0e0 ) {
for (size_t cnt = 1; cnt < L-1; cnt+=2) {
J.at(cnt) /= J12ratio;
}
} else{
for (size_t cnt = 0; cnt < L-1; cnt+=2) {
J.at(cnt) *= J12ratio;
}
}
} else{
J = std::vector<DT>(L, 1.0);
if ( J12ratio > 1.0e0 ) {
for (size_t cnt = 1; cnt < L; cnt+=2) {
J.at(cnt) /= J12ratio;
}
} else{
for (size_t cnt = 0; cnt < L; cnt+=2) {
J.at(cnt) *= J12ratio;
}
}
#ifndef DTYPE
if ( std::abs(phi) > 1.0e-10 ){
J.at(L-1) *= exp( DT(0.0, 1.0) * phi );
}
#endif
}
for ( auto &val : J ){
INFO_NONEWLINE(val << " ");
}
INFO("");
const std::vector< Node<DT>* > lattice = NN_1D_Chain(L, J, OBC);
file->saveNumber("1DChain", "L", L);
file->saveStdVector("1DChain", "J", J);
for ( auto < : lattice ){
if ( !(lt->VerifySite()) ) RUNTIME_ERROR("Wrong lattice setup!");
}
INFO("DONE!");
INFO("Build Basis - ");
// int N1 = (L+1)/2;
Basis F1(L, N1, true);
F1.Fermion();
std::vector<int> st1 = F1.getFStates();
std::vector<size_t> tg1 = F1.getFTags();
// for (size_t cnt = 0; cnt < st1.size(); cnt++) {
// INFO_NONEWLINE( std::setw(3) << st1.at(cnt) << " - ");
// F1.printFermionBasis(st1.at(cnt));
// INFO("- " << tg1.at(st1.at(cnt)));
// }
// int N2 = (L-1)/2;
Basis F2(L, N2, true);
F2.Fermion();
std::vector<int> st2 = F2.getFStates();
std::vector<size_t> tg2 = F2.getFTags();
// for (size_t cnt = 0; cnt < st2.size(); cnt++) {
// INFO_NONEWLINE( std::setw(3) << st2.at(cnt) << " - ");
// F2.printFermionBasis(st2.at(cnt));
// INFO("- " << tg2.at(st2.at(cnt)));
// }
file->saveNumber("Basis", "N1", N1);
file->saveStdVector("Basis", "F1States", st1);
file->saveStdVector("Basis", "F1Tags", tg1);
file->saveNumber("Basis", "N2", N2);
file->saveStdVector("Basis", "F2States", st2);
file->saveStdVector("Basis", "F2Tags", tg2);
INFO("DONE!");
INFO_NONEWLINE("Build Hamiltonian - ");
std::vector<Basis> Bases;
Bases.push_back(F1);
Bases.push_back(F2);
Hamiltonian<DT> ham( Bases );
std::vector< std::vector<DT> > Vloc;
std::vector<DT> Vtmp;//(L, 1.0);
for ( RealType &val : Vin ){
Vtmp.push_back((DT)val);
}
Vloc.push_back(Vtmp);
Vloc.push_back(Vtmp);
std::vector< std::vector<DT> > Uloc;
// std::vector<DT> Utmp(L, DT(10.0e0, 0.0e0) );
std::vector<DT> Utmp(L, (DT)Uin);
Uloc.push_back(Utmp);
Uloc.push_back(Utmp);
ham.BuildLocalHamiltonian(Vloc, Uloc, Bases);
INFO(" - BuildLocalHamiltonian DONE!");
ham.BuildHoppingHamiltonian(Bases, lattice);
INFO(" - BuildHoppingHamiltonian DONE!");
ham.BuildTotalHamiltonian();
INFO("DONE!");
INFO_NONEWLINE("Diagonalize Hamiltonian - ");
std::vector<RealType> Val;
Hamiltonian<DT>::VectorType Vec;
ham.eigh(Val, Vec);
INFO("GS energy = " << Val.at(0));
file->saveVector("GS", "EVec", Vec);
file->saveStdVector("GS", "EVal", Val);
INFO("DONE!");
std::vector< DTV > Nfi = Ni( Bases, Vec, ham );
INFO(" Up Spin - ");
INFO(Nfi.at(0));
INFO(" Down Spin - ");
INFO(Nfi.at(1));
INFO(" N_i - ");
DTV Niall = Nfi.at(0) + Nfi.at(1);
INFO(Niall);
DTM Nud = NupNdn( Bases, Vec, ham );
INFO(" Correlation NupNdn");
INFO(Nud);
DTM Nuu = NupNup( Bases, Vec, ham );
INFO(" Correlation NupNup");
INFO(Nuu);
DTM Ndd = NdnNdn( Bases, Vec, ham );
INFO(" Correlation NdnNdn");
INFO(Ndd);
INFO(" N_i^2 - ");
DTM Ni2 = Nuu.diagonal() + Ndd.diagonal() + 2.0e0 * Nud.diagonal();
INFO(Ni2);
file->saveVector("Obs", "Nup", Nfi.at(0));
file->saveVector("Obs", "Ndn", Nfi.at(1));
file->saveMatrix("Obs", "NupNdn", Nud);
file->saveMatrix("Obs", "NupNup", Nuu);
file->saveMatrix("Obs", "NdnNdn", Ndd);
delete file;
if ( dynamics ){
ComplexType Prefactor = ComplexType(0.0, -1.0e0*dt);/* NOTE: hbar = 1 */
std::cout << "Begin dynamics......" << std::endl;
std::cout << "Cut the boundary." << std::endl;
J.pop_back();
std::vector< Node<DT>* > lattice2 = NN_1D_Chain(L, J, true);// cut to open
ham.BuildHoppingHamiltonian(Bases, lattice2);
INFO(" - Update Hopping Hamiltonian DONE!");
ham.BuildTotalHamiltonian();
INFO(" - Update Total Hamiltonian DONE!");
for (size_t cntT = 1; cntT <= Tsteps; cntT++) {
ham.expH(Prefactor, Vec);
if ( cntT % 2 == 0 ){
HDF5IO file2("DYN.h5");
std::string gname = "Obs-";
gname.append(std::to_string((unsigned long long)cntT));
gname.append("/");
Nfi = Ni( Bases, Vec, ham );
Nud = NupNdn( Bases, Vec, ham );
Nuu = NupNup( Bases, Vec, ham );
Ndd = NdnNdn( Bases, Vec, ham );
file2.saveVector(gname, "Nup", Nfi.at(0));
file2.saveVector(gname, "Ndn", Nfi.at(1));
file2.saveMatrix(gname, "NupNdn", Nud);
file2.saveMatrix(gname, "NupNup", Nuu);
file2.saveMatrix(gname, "NdnNdn", Ndd);
}
}
}
return 0;
}
rspfSensorModelTuple::IntersectStatus rspfSensorModelTuple::
intersect(const DptSet_t obs,
rspfEcefPoint& pt,
NEWMAT::Matrix& covMat) const
{
IntersectStatus opOK = OP_FAIL;
bool covOK = true;
bool epOK;
rspf_int32 nImages = (rspf_int32)obs.size();
NEWMAT::SymmetricMatrix N(3);
NEWMAT::SymmetricMatrix BtWB(3);
NEWMAT::Matrix Ni(3,3);
NEWMAT::ColumnVector C(3);
NEWMAT::ColumnVector BtWF(3);
NEWMAT::ColumnVector F(2);
NEWMAT::ColumnVector dR(3);
NEWMAT::Matrix B(2,3);
NEWMAT::SymmetricMatrix W(2);
rspfGpt estG;
theImages[0]->lineSampleHeightToWorld(obs[0], rspf::nan(), estG);
for (int iter=0; iter<3; iter++)
{
N = 0.0;
C = 0.0;
for (int i=0; i<nImages; i++)
{
rspfDpt resid;
if (!getGroundObsEqComponents(i, iter, obs[i], estG, resid, B, W))
covOK = false;
F[0] = resid.x;
F[1] = resid.y;
BtWF << B.t() * W * F;
BtWB << B.t() * W * B;
C += BtWF;
N += BtWB;
}
Ni = invert(N);
dR = Ni * C;
rspfEcefPoint estECF(estG);
for (rspf_int32 i=0; i<3; i++)
estECF[i] += dR[i];
rspfGpt upd(estECF);
estG = upd;
if (traceDebug())
{
rspfNotify(rspfNotifyLevel_DEBUG)
<< "DEBUG: intersect:\n"
<< " iteration:\n" << iter
<< " C:\n" << C
<< " Ni:\n" << Ni
<< " dR:\n" << dR <<std::endl;
}
} // iterative loop
rspfEcefPoint finalEst(estG);
pt = finalEst;
if (covOK)
{
covMat = Ni;
epOK = true;
}
else
epOK = false;
if (epOK)
opOK = OP_SUCCESS;
else
opOK = ERROR_PROP_FAIL;
return opOK;
}
