// 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;
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
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 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 &lt : 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 &lt : 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;
}
Ejemplo n.º 6
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;
}