<commit_before>// Copyright (c) 2010-2021, Lawrence Livermore National Security, LLC. Produced
// at the Lawrence Livermore National Laboratory. All Rights reserved. See files
// LICENSE and NOTICE for details. LLNL-CODE-806117.
//
// This file is part of the MFEM library. For more information and source code
// availability visit https://mfem.org.
//
// MFEM is free software; you can redistribute it and/or modify it under the
// terms of the BSD-3 license. We welcome feedback and contributions, see file
// CONTRIBUTING.md for details.

#include "lor.hpp"
#include "pbilinearform.hpp"

namespace mfem
{

void LORBase::AddIntegrators(BilinearForm &a_from,
                             BilinearForm &a_to,
                             GetIntegratorsFn get_integrators,
                             AddIntegratorFn add_integrator,
                             const IntegrationRule *ir)
{
   Array<BilinearFormIntegrator*> *integrators = (a_from.*get_integrators)();
   for (int i=0; i<integrators->Size(); ++i)
   {
      (a_to.*add_integrator)((*integrators)[i]);
      ir_map[(*integrators)[i]] = ((*integrators)[i])->GetIntegrationRule();
      if (ir) { ((*integrators)[i])->SetIntegrationRule(*ir); }
   }
}

void LORBase::AddIntegratorsAndMarkers(BilinearForm &a_from,
                                       BilinearForm &a_to,
                                       GetIntegratorsFn get_integrators,
                                       GetMarkersFn get_markers,
                                       AddIntegratorMarkersFn add_integrator,
                                       const IntegrationRule *ir)
{
   Array<BilinearFormIntegrator*> *integrators = (a_from.*get_integrators)();
   Array<Array<int>*> *markers = (a_from.*get_markers)();

   for (int i=0; i<integrators->Size(); ++i)
   {
      (a_to.*add_integrator)((*integrators)[i], *(*markers[i]));
      ir_map[(*integrators)[i]] = ((*integrators)[i])->GetIntegrationRule();
      if (ir) { ((*integrators)[i])->SetIntegrationRule(*ir); }
   }
}

void LORBase::ResetIntegrationRules(GetIntegratorsFn get_integrators)
{
   Array<BilinearFormIntegrator*> *integrators = (a->*get_integrators)();
   for (int i=0; i<integrators->Size(); ++i)
   {
      ((*integrators)[i])->SetIntegrationRule(*ir_map[(*integrators)[i]]);
   }
}

LORBase::FESpaceType LORBase::GetFESpaceType() const
{
   const FiniteElementCollection *fec = fes_ho.FEColl();
   if (dynamic_cast<const H1_FECollection*>(fec)) { return H1; }
   else if (dynamic_cast<const ND_FECollection*>(fec)) { return ND; }
   else if (dynamic_cast<const RT_FECollection*>(fec)) { return RT; }
   else if (dynamic_cast<const L2_FECollection*>(fec)) { return L2; }
   else { MFEM_ABORT("Bad LOR space type."); }
   return INVALID;
}

int LORBase::GetLOROrder() const
{
   FESpaceType type = GetFESpaceType();
   return (type == L2 || type == RT) ? 0 : 1;
}

void LORBase::ConstructLocalDofPermutation(Array<int> &perm_) const
{
   FESpaceType type = GetFESpaceType();
   MFEM_VERIFY(type != H1 && type != L2, "");

   auto get_dof_map = [](FiniteElementSpace &fes, int i)
   {
      const FiniteElement *fe = fes.GetFE(i);
      auto tfe = dynamic_cast<const TensorBasisElement*>(fe);
      MFEM_ASSERT(tfe != NULL, "");
      return tfe->GetDofMap();
   };

   FiniteElementSpace &fes_lor = *fes;
   Mesh &mesh_lor = *fes_lor.GetMesh();
   int dim = mesh_lor.Dimension();
   const CoarseFineTransformations &cf_tr = mesh_lor.GetRefinementTransforms();

   perm_.SetSize(fes_lor.GetVSize());

   Array<int> vdof_ho, vdof_lor;
   for (int ilor=0; ilor<mesh_lor.GetNE(); ++ilor)
   {
      int iho = cf_tr.embeddings[ilor].parent;
      int lor_index = cf_tr.embeddings[ilor].matrix;

      fes_ho.GetElementVDofs(iho, vdof_ho);
      fes_lor.GetElementVDofs(ilor, vdof_lor);

      if (type == L2)
      {
         perm_[vdof_lor[0]] = vdof_ho[lor_index];
         continue;
      }

      int p = fes_ho.GetOrder(iho);
      int p1 = p+1;
      int ndof_per_dim = (dim == 2) ? p*p1 : type == ND ? p*p1*p1 : p*p*p1;

      const Array<int> &dofmap_ho = get_dof_map(fes_ho, iho);
      const Array<int> &dofmap_lor = get_dof_map(fes_lor, ilor);

      int off_x = lor_index % p;
      int off_y = (lor_index / p) % p;
      int off_z = (lor_index / p) / p;

      auto set_perm = [&](int off_lor, int off_ho, int n1, int n2)
      {
         for (int i1=0; i1<2; ++i1)
         {
            int m = (dim == 2 || type == RT) ? 1 : 2;
            for (int i2=0; i2<m; ++i2)
            {
               int i;
               i = dofmap_lor[off_lor + i1 + i2*2];
               int s1 = i < 0 ? -1 : 1;
               int idof_lor = vdof_lor[absdof(i)];
               i = dofmap_ho[off_ho + i1*n1 + i2*n2];
               int s2 = i < 0 ? -1 : 1;
               int idof_ho = vdof_ho[absdof(i)];
               int s3 = idof_lor < 0 ? -1 : 1;
               int s4 = idof_ho < 0 ? -1 : 1;
               int s = s1*s2*s3*s4;
               i = absdof(idof_ho);
               perm_[absdof(idof_lor)] = s < 0 ? -1-absdof(i) : absdof(i);
            }
         }
      };

      int offset;

      if (type == ND)
      {
         // x
         offset = off_x + off_y*p + off_z*p*p1;
         set_perm(0, offset, p, p*p1);
         // y
         offset = ndof_per_dim + off_x + off_y*(p1) + off_z*p1*p;
         set_perm(dim == 2 ? 2 : 4, offset, 1, p*p1);
         // z
         if (dim == 3)
         {
            offset = 2*ndof_per_dim + off_x + off_y*p1 + off_z*p1*p1;
            set_perm(8, offset, 1, p+1);
         }
      }
      else if (type == RT)
      {
         // x
         offset = off_x + off_y*p1 + off_z*p*p1;
         set_perm(0, offset, 1, 0);
         // y
         offset = ndof_per_dim + off_x + off_y*p + off_z*p1*p;
         set_perm(2, offset, p, 0);
         // z
         if (dim == 3)
         {
            offset = 2*ndof_per_dim + off_x + off_y*p + off_z*p*p;
            set_perm(4, offset, p*p, 0);
         }
      }
   }
}

void LORBase::ConstructDofPermutation() const
{
   FESpaceType type = GetFESpaceType();
   if (type == H1 || type == L2)
   {
      // H1 and L2: no permutation necessary, return identity
      perm.SetSize(fes->GetTrueVSize());
      for (int i=0; i<perm.Size(); ++i) { perm[i] = i; }
      return;
   }

#ifdef MFEM_USE_MPI
   ParFiniteElementSpace *pfes_ho
      = dynamic_cast<ParFiniteElementSpace*>(&fes_ho);
   ParFiniteElementSpace *pfes_lor = dynamic_cast<ParFiniteElementSpace*>(fes);
   if (pfes_ho && pfes_lor)
   {
      Array<int> l_perm;
      ConstructLocalDofPermutation(l_perm);
      perm.SetSize(pfes_lor->GetTrueVSize());
      for (int i=0; i<l_perm.Size(); ++i)
      {
         int j = l_perm[i];
         int s = j < 0 ? -1 : 1;
         int t_i = pfes_lor->GetLocalTDofNumber(i);
         int t_j = pfes_ho->GetLocalTDofNumber(absdof(j));
         // Either t_i and t_j both -1, or both non-negative
         MFEM_ASSERT(t_i*t_j >= 0, "");
         if (t_i < 0) { continue; }
         perm[t_i] = s < 0 ? -1 - t_j : t_j;
      }
   }
   else
#endif
   {
      ConstructLocalDofPermutation(perm);
   }
}

const Array<int> &LORBase::GetDofPermutation() const
{
   if (perm.Size() == 0) { ConstructDofPermutation(); }
   return perm;
}

bool LORBase::RequiresDofPermutation() const
{
   // TODO: check if there are cases where L2 requires permutation, or can
   // return false in L2 case too.
   FESpaceType type = GetFESpaceType();
   return (type == H1 || type == L2) ? false : true;
}

const OperatorHandle &LORBase::GetAssembledSystem() const
{
   MFEM_VERIFY(a != NULL, "No LOR system assembled");
   return A;
}

void LORBase::AssembleSystem(BilinearForm &a_ho, const Array<int> &ess_dofs)
{
   a->UseExternalIntegrators();
   AddIntegrators(a_ho, *a, &BilinearForm::GetDBFI,
                  &BilinearForm::AddDomainIntegrator, ir_el);
   AddIntegrators(a_ho, *a, &BilinearForm::GetFBFI,
                  &BilinearForm::AddInteriorFaceIntegrator, ir_face);

   AddIntegratorsAndMarkers(a_ho, *a, &BilinearForm::GetBBFI,
                            &BilinearForm::GetBBFI_Marker,
                            &BilinearForm::AddBoundaryIntegrator, ir_face);
   AddIntegratorsAndMarkers(a_ho, *a, &BilinearForm::GetBFBFI,
                            &BilinearForm::GetBFBFI_Marker,
                            &BilinearForm::AddBdrFaceIntegrator, ir_face);
   a->Assemble();
   if (RequiresDofPermutation())
   {
      const Array<int> &p = GetDofPermutation();
      // Form inverse permutation: given high-order dof i, pi[i] is corresp. LO
      Array<int> pi(p.Size());
      for (int i=0; i<p.Size(); ++i)
      {
         pi[absdof(p[i])] = i;
      }
      Array<int> ess_dofs_perm(ess_dofs.Size());
      for (int i=0; i<ess_dofs.Size(); ++i)
      {
         ess_dofs_perm[i] = pi[ess_dofs[i]];
      }
      a->FormSystemMatrix(ess_dofs_perm, A);
   }
   else
   {
      a->FormSystemMatrix(ess_dofs, A);
   }
   ResetIntegrationRules(&BilinearForm::GetDBFI);
   ResetIntegrationRules(&BilinearForm::GetFBFI);
   ResetIntegrationRules(&BilinearForm::GetBBFI);
   ResetIntegrationRules(&BilinearForm::GetBFBFI);
}

LORBase::LORBase(FiniteElementSpace &fes_ho_)
   : irs(0, Quadrature1D::GaussLobatto), fes_ho(fes_ho_)
{
   Mesh &mesh_ = *fes_ho_.GetMesh();
   int dim = mesh_.Dimension();
   Array<Geometry::Type> geoms;
   mesh_.GetGeometries(dim, geoms);
   if (geoms.Size() == 1 && Geometry::IsTensorProduct(geoms[0]))
   {
      ir_el = &irs.Get(geoms[0], 1);
      ir_face = &irs.Get(Geometry::TensorProductGeometry(dim-1), 1);
   }
   else
   {
      ir_el = NULL;
      ir_face = NULL;
   }
   a = NULL;
}

LORBase::~LORBase()
{
   delete a;
   delete fes;
   delete fec;
   delete mesh;
}

LOR::LOR(BilinearForm &a_ho_, const Array<int> &ess_tdof_list, int ref_type)
   : LOR(*a_ho_.FESpace(), ref_type)
{
   a = new BilinearForm(fes);
   A.SetType(Operator::MFEM_SPARSEMAT);
   AssembleSystem(a_ho_, ess_tdof_list);
}

LOR::LOR(FiniteElementSpace &fes_ho, int ref_type) : LORBase(fes_ho)
{
   // TODO: support variable-order spaces
   MFEM_VERIFY(!fes_ho.IsVariableOrder(),
               "Cannot construct LOR operators on variable-order spaces");

   int order = fes_ho.GetMaxElementOrder();
   if (GetFESpaceType() == L2) { ++order; }

   Mesh &mesh_ho = *fes_ho.GetMesh();
   mesh = new Mesh(Mesh::MakeRefined(mesh_ho, order, ref_type));

   fec = fes_ho.FEColl()->Clone(GetLOROrder());
   fes = new FiniteElementSpace(mesh, fec);
}

SparseMatrix &LOR::GetAssembledMatrix() const
{
   MFEM_VERIFY(a != NULL, "No LOR system assembled");
   return *A.As<SparseMatrix>();
}

#ifdef MFEM_USE_MPI

ParLOR::ParLOR(ParBilinearForm &a_ho_, const Array<int> &ess_tdof_list,
               int ref_type) : ParLOR(*a_ho_.ParFESpace(), ref_type)
{
   a = new ParBilinearForm(static_cast<ParFiniteElementSpace*>(fes));
   A.SetType(Operator::Hypre_ParCSR);
   AssembleSystem(a_ho_, ess_tdof_list);
}

ParLOR::ParLOR(ParFiniteElementSpace &fes_ho, int ref_type) : LORBase(fes_ho)
{
   // TODO: support variable-order spaces
   MFEM_VERIFY(!fes_ho.IsVariableOrder(),
               "Cannot construct LOR operators on variable-order spaces");

   int order = fes_ho.GetMaxElementOrder();
   if (GetFESpaceType() == L2) { ++order; }

   ParMesh &mesh_ho = *fes_ho.GetParMesh();
   ParMesh *pmesh = new ParMesh(ParMesh::MakeRefined(mesh_ho, order, ref_type));
   mesh = pmesh;

   fec = fes_ho.FEColl()->Clone(GetLOROrder());
   ParFiniteElementSpace *pfes = new ParFiniteElementSpace(pmesh, fec);
   fes = pfes;
}

HypreParMatrix &ParLOR::GetAssembledMatrix() const
{
   MFEM_VERIFY(a != NULL, "No LOR system assembled");
   return *A.As<HypreParMatrix>();
}

ParFiniteElementSpace &ParLOR::GetParFESpace() const
{
   return static_cast<ParFiniteElementSpace&>(*fes);
}

#endif

} // namespace mfem
<commit_msg>Fix possible overflow<commit_after>// Copyright (c) 2010-2021, Lawrence Livermore National Security, LLC. Produced
// at the Lawrence Livermore National Laboratory. All Rights reserved. See files
// LICENSE and NOTICE for details. LLNL-CODE-806117.
//
// This file is part of the MFEM library. For more information and source code
// availability visit https://mfem.org.
//
// MFEM is free software; you can redistribute it and/or modify it under the
// terms of the BSD-3 license. We welcome feedback and contributions, see file
// CONTRIBUTING.md for details.

#include "lor.hpp"
#include "pbilinearform.hpp"

namespace mfem
{

void LORBase::AddIntegrators(BilinearForm &a_from,
                             BilinearForm &a_to,
                             GetIntegratorsFn get_integrators,
                             AddIntegratorFn add_integrator,
                             const IntegrationRule *ir)
{
   Array<BilinearFormIntegrator*> *integrators = (a_from.*get_integrators)();
   for (int i=0; i<integrators->Size(); ++i)
   {
      (a_to.*add_integrator)((*integrators)[i]);
      ir_map[(*integrators)[i]] = ((*integrators)[i])->GetIntegrationRule();
      if (ir) { ((*integrators)[i])->SetIntegrationRule(*ir); }
   }
}

void LORBase::AddIntegratorsAndMarkers(BilinearForm &a_from,
                                       BilinearForm &a_to,
                                       GetIntegratorsFn get_integrators,
                                       GetMarkersFn get_markers,
                                       AddIntegratorMarkersFn add_integrator,
                                       const IntegrationRule *ir)
{
   Array<BilinearFormIntegrator*> *integrators = (a_from.*get_integrators)();
   Array<Array<int>*> *markers = (a_from.*get_markers)();

   for (int i=0; i<integrators->Size(); ++i)
   {
      (a_to.*add_integrator)((*integrators)[i], *(*markers[i]));
      ir_map[(*integrators)[i]] = ((*integrators)[i])->GetIntegrationRule();
      if (ir) { ((*integrators)[i])->SetIntegrationRule(*ir); }
   }
}

void LORBase::ResetIntegrationRules(GetIntegratorsFn get_integrators)
{
   Array<BilinearFormIntegrator*> *integrators = (a->*get_integrators)();
   for (int i=0; i<integrators->Size(); ++i)
   {
      ((*integrators)[i])->SetIntegrationRule(*ir_map[(*integrators)[i]]);
   }
}

LORBase::FESpaceType LORBase::GetFESpaceType() const
{
   const FiniteElementCollection *fec = fes_ho.FEColl();
   if (dynamic_cast<const H1_FECollection*>(fec)) { return H1; }
   else if (dynamic_cast<const ND_FECollection*>(fec)) { return ND; }
   else if (dynamic_cast<const RT_FECollection*>(fec)) { return RT; }
   else if (dynamic_cast<const L2_FECollection*>(fec)) { return L2; }
   else { MFEM_ABORT("Bad LOR space type."); }
   return INVALID;
}

int LORBase::GetLOROrder() const
{
   FESpaceType type = GetFESpaceType();
   return (type == L2 || type == RT) ? 0 : 1;
}

void LORBase::ConstructLocalDofPermutation(Array<int> &perm_) const
{
   FESpaceType type = GetFESpaceType();
   MFEM_VERIFY(type != H1 && type != L2, "");

   auto get_dof_map = [](FiniteElementSpace &fes, int i)
   {
      const FiniteElement *fe = fes.GetFE(i);
      auto tfe = dynamic_cast<const TensorBasisElement*>(fe);
      MFEM_ASSERT(tfe != NULL, "");
      return tfe->GetDofMap();
   };

   FiniteElementSpace &fes_lor = *fes;
   Mesh &mesh_lor = *fes_lor.GetMesh();
   int dim = mesh_lor.Dimension();
   const CoarseFineTransformations &cf_tr = mesh_lor.GetRefinementTransforms();

   perm_.SetSize(fes_lor.GetVSize());

   Array<int> vdof_ho, vdof_lor;
   for (int ilor=0; ilor<mesh_lor.GetNE(); ++ilor)
   {
      int iho = cf_tr.embeddings[ilor].parent;
      int lor_index = cf_tr.embeddings[ilor].matrix;

      fes_ho.GetElementVDofs(iho, vdof_ho);
      fes_lor.GetElementVDofs(ilor, vdof_lor);

      if (type == L2)
      {
         perm_[vdof_lor[0]] = vdof_ho[lor_index];
         continue;
      }

      int p = fes_ho.GetOrder(iho);
      int p1 = p+1;
      int ndof_per_dim = (dim == 2) ? p*p1 : type == ND ? p*p1*p1 : p*p*p1;

      const Array<int> &dofmap_ho = get_dof_map(fes_ho, iho);
      const Array<int> &dofmap_lor = get_dof_map(fes_lor, ilor);

      int off_x = lor_index % p;
      int off_y = (lor_index / p) % p;
      int off_z = (lor_index / p) / p;

      auto set_perm = [&](int off_lor, int off_ho, int n1, int n2)
      {
         for (int i1=0; i1<2; ++i1)
         {
            int m = (dim == 2 || type == RT) ? 1 : 2;
            for (int i2=0; i2<m; ++i2)
            {
               int i;
               i = dofmap_lor[off_lor + i1 + i2*2];
               int s1 = i < 0 ? -1 : 1;
               int idof_lor = vdof_lor[absdof(i)];
               i = dofmap_ho[off_ho + i1*n1 + i2*n2];
               int s2 = i < 0 ? -1 : 1;
               int idof_ho = vdof_ho[absdof(i)];
               int s3 = idof_lor < 0 ? -1 : 1;
               int s4 = idof_ho < 0 ? -1 : 1;
               int s = s1*s2*s3*s4;
               i = absdof(idof_ho);
               perm_[absdof(idof_lor)] = s < 0 ? -1-absdof(i) : absdof(i);
            }
         }
      };

      int offset;

      if (type == ND)
      {
         // x
         offset = off_x + off_y*p + off_z*p*p1;
         set_perm(0, offset, p, p*p1);
         // y
         offset = ndof_per_dim + off_x + off_y*(p1) + off_z*p1*p;
         set_perm(dim == 2 ? 2 : 4, offset, 1, p*p1);
         // z
         if (dim == 3)
         {
            offset = 2*ndof_per_dim + off_x + off_y*p1 + off_z*p1*p1;
            set_perm(8, offset, 1, p+1);
         }
      }
      else if (type == RT)
      {
         // x
         offset = off_x + off_y*p1 + off_z*p*p1;
         set_perm(0, offset, 1, 0);
         // y
         offset = ndof_per_dim + off_x + off_y*p + off_z*p1*p;
         set_perm(2, offset, p, 0);
         // z
         if (dim == 3)
         {
            offset = 2*ndof_per_dim + off_x + off_y*p + off_z*p*p;
            set_perm(4, offset, p*p, 0);
         }
      }
   }
}

void LORBase::ConstructDofPermutation() const
{
   FESpaceType type = GetFESpaceType();
   if (type == H1 || type == L2)
   {
      // H1 and L2: no permutation necessary, return identity
      perm.SetSize(fes->GetTrueVSize());
      for (int i=0; i<perm.Size(); ++i) { perm[i] = i; }
      return;
   }

#ifdef MFEM_USE_MPI
   ParFiniteElementSpace *pfes_ho
      = dynamic_cast<ParFiniteElementSpace*>(&fes_ho);
   ParFiniteElementSpace *pfes_lor = dynamic_cast<ParFiniteElementSpace*>(fes);
   if (pfes_ho && pfes_lor)
   {
      Array<int> l_perm;
      ConstructLocalDofPermutation(l_perm);
      perm.SetSize(pfes_lor->GetTrueVSize());
      for (int i=0; i<l_perm.Size(); ++i)
      {
         int j = l_perm[i];
         int s = j < 0 ? -1 : 1;
         int t_i = pfes_lor->GetLocalTDofNumber(i);
         int t_j = pfes_ho->GetLocalTDofNumber(absdof(j));
         // Either t_i and t_j both -1, or both non-negative
         if ((t_i < 0 && t_j >=0) || (t_j < 0 && t_i >= 0))
         {
            MFEM_ABORT("Inconsistent DOF numbering");
         }
         if (t_i < 0) { continue; }
         perm[t_i] = s < 0 ? -1 - t_j : t_j;
      }
   }
   else
#endif
   {
      ConstructLocalDofPermutation(perm);
   }
}

const Array<int> &LORBase::GetDofPermutation() const
{
   if (perm.Size() == 0) { ConstructDofPermutation(); }
   return perm;
}

bool LORBase::RequiresDofPermutation() const
{
   // TODO: check if there are cases where L2 requires permutation, or can
   // return false in L2 case too.
   FESpaceType type = GetFESpaceType();
   return (type == H1 || type == L2) ? false : true;
}

const OperatorHandle &LORBase::GetAssembledSystem() const
{
   MFEM_VERIFY(a != NULL, "No LOR system assembled");
   return A;
}

void LORBase::AssembleSystem(BilinearForm &a_ho, const Array<int> &ess_dofs)
{
   a->UseExternalIntegrators();
   AddIntegrators(a_ho, *a, &BilinearForm::GetDBFI,
                  &BilinearForm::AddDomainIntegrator, ir_el);
   AddIntegrators(a_ho, *a, &BilinearForm::GetFBFI,
                  &BilinearForm::AddInteriorFaceIntegrator, ir_face);

   AddIntegratorsAndMarkers(a_ho, *a, &BilinearForm::GetBBFI,
                            &BilinearForm::GetBBFI_Marker,
                            &BilinearForm::AddBoundaryIntegrator, ir_face);
   AddIntegratorsAndMarkers(a_ho, *a, &BilinearForm::GetBFBFI,
                            &BilinearForm::GetBFBFI_Marker,
                            &BilinearForm::AddBdrFaceIntegrator, ir_face);
   a->Assemble();
   if (RequiresDofPermutation())
   {
      const Array<int> &p = GetDofPermutation();
      // Form inverse permutation: given high-order dof i, pi[i] is corresp. LO
      Array<int> pi(p.Size());
      for (int i=0; i<p.Size(); ++i)
      {
         pi[absdof(p[i])] = i;
      }
      Array<int> ess_dofs_perm(ess_dofs.Size());
      for (int i=0; i<ess_dofs.Size(); ++i)
      {
         ess_dofs_perm[i] = pi[ess_dofs[i]];
      }
      a->FormSystemMatrix(ess_dofs_perm, A);
   }
   else
   {
      a->FormSystemMatrix(ess_dofs, A);
   }
   ResetIntegrationRules(&BilinearForm::GetDBFI);
   ResetIntegrationRules(&BilinearForm::GetFBFI);
   ResetIntegrationRules(&BilinearForm::GetBBFI);
   ResetIntegrationRules(&BilinearForm::GetBFBFI);
}

LORBase::LORBase(FiniteElementSpace &fes_ho_)
   : irs(0, Quadrature1D::GaussLobatto), fes_ho(fes_ho_)
{
   Mesh &mesh_ = *fes_ho_.GetMesh();
   int dim = mesh_.Dimension();
   Array<Geometry::Type> geoms;
   mesh_.GetGeometries(dim, geoms);
   if (geoms.Size() == 1 && Geometry::IsTensorProduct(geoms[0]))
   {
      ir_el = &irs.Get(geoms[0], 1);
      ir_face = &irs.Get(Geometry::TensorProductGeometry(dim-1), 1);
   }
   else
   {
      ir_el = NULL;
      ir_face = NULL;
   }
   a = NULL;
}

LORBase::~LORBase()
{
   delete a;
   delete fes;
   delete fec;
   delete mesh;
}

LOR::LOR(BilinearForm &a_ho_, const Array<int> &ess_tdof_list, int ref_type)
   : LOR(*a_ho_.FESpace(), ref_type)
{
   a = new BilinearForm(fes);
   A.SetType(Operator::MFEM_SPARSEMAT);
   AssembleSystem(a_ho_, ess_tdof_list);
}

LOR::LOR(FiniteElementSpace &fes_ho, int ref_type) : LORBase(fes_ho)
{
   // TODO: support variable-order spaces
   MFEM_VERIFY(!fes_ho.IsVariableOrder(),
               "Cannot construct LOR operators on variable-order spaces");

   int order = fes_ho.GetMaxElementOrder();
   if (GetFESpaceType() == L2) { ++order; }

   Mesh &mesh_ho = *fes_ho.GetMesh();
   mesh = new Mesh(Mesh::MakeRefined(mesh_ho, order, ref_type));

   fec = fes_ho.FEColl()->Clone(GetLOROrder());
   fes = new FiniteElementSpace(mesh, fec);
}

SparseMatrix &LOR::GetAssembledMatrix() const
{
   MFEM_VERIFY(a != NULL, "No LOR system assembled");
   return *A.As<SparseMatrix>();
}

#ifdef MFEM_USE_MPI

ParLOR::ParLOR(ParBilinearForm &a_ho_, const Array<int> &ess_tdof_list,
               int ref_type) : ParLOR(*a_ho_.ParFESpace(), ref_type)
{
   a = new ParBilinearForm(static_cast<ParFiniteElementSpace*>(fes));
   A.SetType(Operator::Hypre_ParCSR);
   AssembleSystem(a_ho_, ess_tdof_list);
}

ParLOR::ParLOR(ParFiniteElementSpace &fes_ho, int ref_type) : LORBase(fes_ho)
{
   // TODO: support variable-order spaces
   MFEM_VERIFY(!fes_ho.IsVariableOrder(),
               "Cannot construct LOR operators on variable-order spaces");

   int order = fes_ho.GetMaxElementOrder();
   if (GetFESpaceType() == L2) { ++order; }

   ParMesh &mesh_ho = *fes_ho.GetParMesh();
   ParMesh *pmesh = new ParMesh(ParMesh::MakeRefined(mesh_ho, order, ref_type));
   mesh = pmesh;

   fec = fes_ho.FEColl()->Clone(GetLOROrder());
   ParFiniteElementSpace *pfes = new ParFiniteElementSpace(pmesh, fec);
   fes = pfes;
}

HypreParMatrix &ParLOR::GetAssembledMatrix() const
{
   MFEM_VERIFY(a != NULL, "No LOR system assembled");
   return *A.As<HypreParMatrix>();
}

ParFiniteElementSpace &ParLOR::GetParFESpace() const
{
   return static_cast<ParFiniteElementSpace&>(*fes);
}

#endif

} // namespace mfem
<|endoftext|>


<commit_before>#include <cmath>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <limits>
#include <vector>

#include <ffi.h>
#include <dlfcn.h>

struct Value {
  ffi_type *type;
  union {
    uint8_t u8;
    uint16_t u16;
    uint32_t u32;
    uint64_t u64;
    int8_t s8;
    int16_t s16;
    int32_t s32;
    int64_t s64;
    float f;
    double d;
    void *ptr;
  } value;
};

std::ostream &operator<<(std::ostream &out, const Value &v) {
  if (v.type == &ffi_type_uint8) {
    out << v.value.u8;
  } else if (v.type == &ffi_type_uint16) {
    out << v.value.u16;
  } else if (v.type == &ffi_type_uint32) {
    out << v.value.u32;
  } else if (v.type == &ffi_type_uint64) {
    out << v.value.u64;
  } else if (v.type == &ffi_type_sint8) {
    out << v.value.s8;
  } else if (v.type == &ffi_type_sint16) {
    out << v.value.s16;
  } else if (v.type == &ffi_type_sint32) {
    out << v.value.s32;
  } else if (v.type == &ffi_type_sint64) {
    out << v.value.s64;
  } else if (v.type == &ffi_type_float) {
	float value = v.value.f;
	if(std::isnan(value)) {
		out << "0.0e+NaN";
	} else if(std::isinf(value)) {
		if(value < 0) {
			out << "-1.0e+INF";
		} else {
			out << "1.0e+INF";
		}
	} else {
	    out << value;
	}
  } else if (v.type == &ffi_type_double) {
    out << v.value.d;
  } else if (v.type == &ffi_type_pointer) {
    out << "\\" << v.value.ptr;
  } else if (v.type == &ffi_type_void) {
    out << ":void";
  }
  out << "\n$";
  return out;
}

class FFIStack {
 public:
  void push(uint8_t v) {
    stack_.push_back(Value{&ffi_type_uint8, {.u8 = v}});
  }
  void push(uint16_t v) {
    stack_.push_back(Value{&ffi_type_uint16, {.u16 = v}});
  }
  void push(uint32_t v) {
    stack_.push_back(Value{&ffi_type_uint32, {.u32 = v}});
  }
  void push(uint64_t v) {
    stack_.push_back(Value{&ffi_type_uint64, {.u64 = v}});
  }
  void push(int8_t v) {
    stack_.push_back(Value{&ffi_type_sint8, {.s8 = v}});
  }
  void push(int16_t v) {
    stack_.push_back(Value{&ffi_type_sint16, {.s16 = v}});
  }
  void push(int32_t v) {
    stack_.push_back(Value{&ffi_type_sint32, {.s32 = v}});
  }
  void push(int64_t v) {
    stack_.push_back(Value{&ffi_type_sint64, {.s64 = v}});
  }
  void push(double v) {
    stack_.push_back(Value{&ffi_type_double, {.d = v}});
  }
  void push(float v) {
    stack_.push_back(Value{&ffi_type_float, {.f = v}});
  }
  void push(void *v) {
    stack_.push_back(Value{&ffi_type_pointer, {.ptr = v}});
  }
  void push(Value &v) { stack_.push_back(v); }
  void push() {
    stack_.push_back(Value{&ffi_type_void, {0}});
  }

  Value pop() {
    Value v = stack_.back();
    stack_.pop_back();
    return v;
  }

  Value peek() { return stack_.back(); }

  Value &operator[](int i) { return stack_[stack_.size() - i - 1]; }

  void dup() {
    stack_.push_back(stack_.back());
  }

  size_t size() {
    return stack_.size();
  }

 private:
  std::vector<Value> stack_;
};

class Machine {
 public:
  Machine(std::ostream &out) : out_(out) {
    out.precision(std::numeric_limits<double>::digits10);
  };
  ~Machine() {
    for (auto &i : cifs_) {
      delete i->arg_types;
      delete i;
    }
    for (auto &i : libs_) {
      if (i) dlclose(i);
    }
  };

  FFIStack stack;

  void pop() { out_ << stack.pop(); }

  void peek() { out_ << stack.peek(); }

  void dlopen() {
    const char *name = static_cast<const char *>(stack.pop().value.ptr);
    void *handle = ::dlopen(name, RTLD_LAZY);
    stack.push(handle);
    libs_.push_back(handle);
  }

  void dlsym() {
    const char *name = static_cast<const char *>(stack.pop().value.ptr);
    void *handle = stack.pop().value.ptr;
    void *ptr = ::dlsym(handle, name);
    stack.push(ptr);
  }

  void cif() {
    ffi_cif *cif = new ffi_cif;
    uint32_t nargs = stack.pop().value.u32;
    ffi_type *rtype = stack.pop().type;
    ffi_type **types = new ffi_type*[nargs];
    for (uint32_t i = 0; i < nargs; i++) {
      types[i] = stack.pop().type;
    }
    ffi_prep_cif(cif, FFI_DEFAULT_ABI, nargs, rtype, types);
    stack.push(cif);
    cifs_.push_back(cif);
  }

  void call(bool push_errno) {
    void (*function)() = reinterpret_cast<void (*)()>(stack.pop().value.ptr);
    ffi_cif *cif = static_cast<ffi_cif *>(stack.pop().value.ptr);
    int nargs = cif->nargs;
    void *args[nargs];
    Value result {cif->rtype, {0}};
    for (int i = 0; i < nargs; i++) {
      args[i] = &stack[i].value;
    }
    ffi_call(cif, function, &result.value, args);
    for (int i = 0; i < nargs; i++) {
      stack.pop();
    }
	if(push_errno)
    	stack.push(errno);
    stack.push(result);
  }

  void strlen() {
    char *str = static_cast<char *>(stack.peek().value.ptr);
    uint32_t size = std::strlen(str);
    stack.push(size);
  }

  void dump() {
    uint32_t size = stack.pop().value.u32;
    char *data = static_cast<char *>(stack.pop().value.ptr);
    out_.write(data, size);
  }

  void free() { delete static_cast<char *>(stack.pop().value.ptr); }

  void size() {
    uint32_t size = stack.size();
    stack.push(size);
  }

 private:
  std::ostream &out_;
  std::vector<ffi_cif *> cifs_;
  std::vector<void *> libs_;
};

class Reader {
 public:
  Reader(Machine &vm, std::istream &in) : vm_(vm), in_(in) {};

  void read_uint8() {
    uint8_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_uint16() {
    uint16_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_uint32() {
    uint32_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_uint64() {
    uint64_t i;
    in_ >> i;
    vm_.stack.push(i);
  }

  void read_sint8() {
    int8_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_sint16() {
    int16_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_sint32() {
    int32_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_sint64() {
    int64_t i;
    in_ >> i;
    vm_.stack.push(i);
  }

  void read_float() {
    float f;
    in_ >> f;
    vm_.stack.push(f);
  }
  void read_double() {
    double d;
    in_ >> d;
    vm_.stack.push(d);
  }

  void read_pointer() {
    void *ptr = nullptr;
    in_ >> ptr;
    vm_.stack.push(ptr);
  }

  void read(uint32_t size) {
    char *buffer = new char[size + 1];
    buffer[size] = '\0';
    in_.read(buffer, size);
    vm_.stack.push(buffer);
  }

 private:
  Machine &vm_;
  std::istream &in_;
};

int main() {
  Machine vm{std::cout};
  Reader reader{vm, std::cin};

  while (!std::cin.eof()) {
    int command = std::cin.get();
    switch (command) {
      /* push signed integer */
      case 'i' + 0: /* i */
        reader.read_sint8();
        break;
      case 'i' + 1: /* j */
        reader.read_sint16();
        break;
      case 'i' + 2: /* k */
        reader.read_sint32();
        break;
      case 'i' + 3: /* l */
        reader.read_sint64();
        break;

      /* push unsigned integer */
      case 'u' + 0: /* u */
        reader.read_uint8();
        break;
      case 'u' + 1: /* v */
        reader.read_uint16();
        break;
      case 'u' + 2: /* w */
        reader.read_uint32();
        break;
      case 'u' + 3: /* x */
        reader.read_uint64();
        break;

      /* push float */
      case 'f':
        reader.read_float();
        break;
      case 'd':
        reader.read_double();
        break;

      /* push pointer */
      case 'p':
        reader.read_pointer();
        break;

      /* push void */
      case 'V':
        vm.stack.push();
        break;

      /* peek */
      case 'e':
        vm.peek();
        break;

      /* pop */
      case 'o':
        vm.pop();
        break;

      /* call */
      case 'c':
        vm.call(false);
        break;

      /* call with errno */
      case 'E':
        vm.call(true);
        break;

      /* call */
      case 'C':
        vm.cif();
        break;

      /* "malloc" */
      case 'M':
        reader.read(vm.stack.pop().value.u32);
        break;

      /* free */
      case 'F':
        vm.free();
        break;

      /* dl calls */
      case 'O':
        vm.dlopen();
        break;
      case 'S':
        vm.dlsym();
        break;

      /* strlen */
      case 'L':
        vm.strlen();
        break;

      /* dump */
      case 'D':
        vm.dump();
        break;

      /* duplicate top value */
      case '!':
        vm.stack.dup();
        break;

      /* get stack size */
      case '?':
        vm.size();
        break;

      /* no-op */
      case ' ':
      case '\n':
      case '\r':
      case -1:
        break;

      default:
        std::cerr << "invalid op: " << static_cast<char>(command) << std::endl;
        break;
    }
    std::cout.flush();
  }

  std::cout << std::endl;
  return 0;
}
<commit_msg>Fixes #11.<commit_after>#include <cmath>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <limits>
#include <vector>

#include <ffi.h>
#include <dlfcn.h>

struct Value {
  ffi_type *type;
  union {
    uint8_t u8;
    uint16_t u16;
    uint32_t u32;
    uint64_t u64;
    int8_t s8;
    int16_t s16;
    int32_t s32;
    int64_t s64;
    float f;
    double d;
    void *ptr;
  } value;
};

std::ostream &operator<<(std::ostream &out, const Value &v) {
  if (v.type == &ffi_type_uint8) {
    out << v.value.u8;
  } else if (v.type == &ffi_type_uint16) {
    out << v.value.u16;
  } else if (v.type == &ffi_type_uint32) {
    out << v.value.u32;
  } else if (v.type == &ffi_type_uint64) {
    out << v.value.u64;
  } else if (v.type == &ffi_type_sint8) {
    out << v.value.s8;
  } else if (v.type == &ffi_type_sint16) {
    out << v.value.s16;
  } else if (v.type == &ffi_type_sint32) {
    out << v.value.s32;
  } else if (v.type == &ffi_type_sint64) {
    out << v.value.s64;
  } else if (v.type == &ffi_type_float) {
	float value = v.value.f;
	if(std::isnan(value)) {
		out << "0.0e+NaN";
	} else if(std::isinf(value)) {
		if(value < 0) {
			out << "-1.0e+INF";
		} else {
			out << "1.0e+INF";
		}
	} else {
	    out << value;
	}
  } else if (v.type == &ffi_type_double) {
	double value = v.value.d;
	if(std::isnan(value)) {
		out << "0.0e+NaN";
	} else if(std::isinf(value)) {
		if(value < 0) {
			out << "-1.0e+INF";
		} else {
			out << "1.0e+INF";
		}
	} else {
	    out << value;
	}
  } else if (v.type == &ffi_type_pointer) {
    out << "\\" << v.value.ptr;
  } else if (v.type == &ffi_type_void) {
    out << ":void";
  }
  out << "\n$";
  return out;
}

class FFIStack {
 public:
  void push(uint8_t v) {
    stack_.push_back(Value{&ffi_type_uint8, {.u8 = v}});
  }
  void push(uint16_t v) {
    stack_.push_back(Value{&ffi_type_uint16, {.u16 = v}});
  }
  void push(uint32_t v) {
    stack_.push_back(Value{&ffi_type_uint32, {.u32 = v}});
  }
  void push(uint64_t v) {
    stack_.push_back(Value{&ffi_type_uint64, {.u64 = v}});
  }
  void push(int8_t v) {
    stack_.push_back(Value{&ffi_type_sint8, {.s8 = v}});
  }
  void push(int16_t v) {
    stack_.push_back(Value{&ffi_type_sint16, {.s16 = v}});
  }
  void push(int32_t v) {
    stack_.push_back(Value{&ffi_type_sint32, {.s32 = v}});
  }
  void push(int64_t v) {
    stack_.push_back(Value{&ffi_type_sint64, {.s64 = v}});
  }
  void push(double v) {
    stack_.push_back(Value{&ffi_type_double, {.d = v}});
  }
  void push(float v) {
    stack_.push_back(Value{&ffi_type_float, {.f = v}});
  }
  void push(void *v) {
    stack_.push_back(Value{&ffi_type_pointer, {.ptr = v}});
  }
  void push(Value &v) { stack_.push_back(v); }
  void push() {
    stack_.push_back(Value{&ffi_type_void, {0}});
  }

  Value pop() {
    Value v = stack_.back();
    stack_.pop_back();
    return v;
  }

  Value peek() { return stack_.back(); }

  Value &operator[](int i) { return stack_[stack_.size() - i - 1]; }

  void dup() {
    stack_.push_back(stack_.back());
  }

  size_t size() {
    return stack_.size();
  }

 private:
  std::vector<Value> stack_;
};

class Machine {
 public:
  Machine(std::ostream &out) : out_(out) {
    out.precision(std::numeric_limits<double>::digits10);
  };
  ~Machine() {
    for (auto &i : cifs_) {
      delete i->arg_types;
      delete i;
    }
    for (auto &i : libs_) {
      if (i) dlclose(i);
    }
  };

  FFIStack stack;

  void pop() { out_ << stack.pop(); }

  void peek() { out_ << stack.peek(); }

  void dlopen() {
    const char *name = static_cast<const char *>(stack.pop().value.ptr);
    void *handle = ::dlopen(name, RTLD_LAZY);
    stack.push(handle);
    libs_.push_back(handle);
  }

  void dlsym() {
    const char *name = static_cast<const char *>(stack.pop().value.ptr);
    void *handle = stack.pop().value.ptr;
    void *ptr = ::dlsym(handle, name);
    stack.push(ptr);
  }

  void cif() {
    ffi_cif *cif = new ffi_cif;
    uint32_t nargs = stack.pop().value.u32;
    ffi_type *rtype = stack.pop().type;
    ffi_type **types = new ffi_type*[nargs];
    for (uint32_t i = 0; i < nargs; i++) {
      types[i] = stack.pop().type;
    }
    ffi_prep_cif(cif, FFI_DEFAULT_ABI, nargs, rtype, types);
    stack.push(cif);
    cifs_.push_back(cif);
  }

  void call(bool push_errno) {
    void (*function)() = reinterpret_cast<void (*)()>(stack.pop().value.ptr);
    ffi_cif *cif = static_cast<ffi_cif *>(stack.pop().value.ptr);
    int nargs = cif->nargs;
    void *args[nargs];
    Value result {cif->rtype, {0}};
    for (int i = 0; i < nargs; i++) {
      args[i] = &stack[i].value;
    }
    ffi_call(cif, function, &result.value, args);
    for (int i = 0; i < nargs; i++) {
      stack.pop();
    }
	if(push_errno)
    	stack.push(errno);
    stack.push(result);
  }

  void strlen() {
    char *str = static_cast<char *>(stack.peek().value.ptr);
    uint32_t size = std::strlen(str);
    stack.push(size);
  }

  void dump() {
    uint32_t size = stack.pop().value.u32;
    char *data = static_cast<char *>(stack.pop().value.ptr);
    out_.write(data, size);
  }

  void free() { delete static_cast<char *>(stack.pop().value.ptr); }

  void size() {
    uint32_t size = stack.size();
    stack.push(size);
  }

 private:
  std::ostream &out_;
  std::vector<ffi_cif *> cifs_;
  std::vector<void *> libs_;
};

class Reader {
 public:
  Reader(Machine &vm, std::istream &in) : vm_(vm), in_(in) {};

  void read_uint8() {
    uint8_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_uint16() {
    uint16_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_uint32() {
    uint32_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_uint64() {
    uint64_t i;
    in_ >> i;
    vm_.stack.push(i);
  }

  void read_sint8() {
    int8_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_sint16() {
    int16_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_sint32() {
    int32_t i;
    in_ >> i;
    vm_.stack.push(i);
  }
  void read_sint64() {
    int64_t i;
    in_ >> i;
    vm_.stack.push(i);
  }

  void read_float() {
    float f;
    in_ >> f;
    vm_.stack.push(f);
  }
  void read_double() {
    double d;
    in_ >> d;
    vm_.stack.push(d);
  }

  void read_pointer() {
    void *ptr = nullptr;
    in_ >> ptr;
    vm_.stack.push(ptr);
  }

  void read(uint32_t size) {
    char *buffer = new char[size + 1];
    buffer[size] = '\0';
    in_.read(buffer, size);
    vm_.stack.push(buffer);
  }

 private:
  Machine &vm_;
  std::istream &in_;
};

int main() {
  Machine vm{std::cout};
  Reader reader{vm, std::cin};

  while (!std::cin.eof()) {
    int command = std::cin.get();
    switch (command) {
      /* push signed integer */
      case 'i' + 0: /* i */
        reader.read_sint8();
        break;
      case 'i' + 1: /* j */
        reader.read_sint16();
        break;
      case 'i' + 2: /* k */
        reader.read_sint32();
        break;
      case 'i' + 3: /* l */
        reader.read_sint64();
        break;

      /* push unsigned integer */
      case 'u' + 0: /* u */
        reader.read_uint8();
        break;
      case 'u' + 1: /* v */
        reader.read_uint16();
        break;
      case 'u' + 2: /* w */
        reader.read_uint32();
        break;
      case 'u' + 3: /* x */
        reader.read_uint64();
        break;

      /* push float */
      case 'f':
        reader.read_float();
        break;
      case 'd':
        reader.read_double();
        break;

      /* push pointer */
      case 'p':
        reader.read_pointer();
        break;

      /* push void */
      case 'V':
        vm.stack.push();
        break;

      /* peek */
      case 'e':
        vm.peek();
        break;

      /* pop */
      case 'o':
        vm.pop();
        break;

      /* call */
      case 'c':
        vm.call(false);
        break;

      /* call with errno */
      case 'E':
        vm.call(true);
        break;

      /* call */
      case 'C':
        vm.cif();
        break;

      /* "malloc" */
      case 'M':
        reader.read(vm.stack.pop().value.u32);
        break;

      /* free */
      case 'F':
        vm.free();
        break;

      /* dl calls */
      case 'O':
        vm.dlopen();
        break;
      case 'S':
        vm.dlsym();
        break;

      /* strlen */
      case 'L':
        vm.strlen();
        break;

      /* dump */
      case 'D':
        vm.dump();
        break;

      /* duplicate top value */
      case '!':
        vm.stack.dup();
        break;

      /* get stack size */
      case '?':
        vm.size();
        break;

      /* no-op */
      case ' ':
      case '\n':
      case '\r':
      case -1:
        break;

      default:
        std::cerr << "invalid op: " << static_cast<char>(command) << std::endl;
        break;
    }
    std::cout.flush();
  }

  std::cout << std::endl;
  return 0;
}
<|endoftext|>


<commit_before>//===----------------------------------------------------------------------===//
//
//                         PelotonDB
//
// transaction_manager_factory.cpp
//
// Identification: src/backend/concurrency/transaction_manager_factory.cpp
//
// Copyright (c) 2015, Carnegie Mellon University Database Group
//
//===----------------------------------------------------------------------===//

#include "transaction_manager_factory.h"

namespace peloton {
namespace concurrency {
ConcurrencyType TransactionManagerFactory::protocol_ =
    CONCURRENCY_TYPE_EAGER_WRITE;
IsolationLevelType TransactionManagerFactory::isolation_level_ =
    ISOLATION_LEVEL_TYPE_FULL;
}
}
<commit_msg>set default protocol to OCC<commit_after>//===----------------------------------------------------------------------===//
//
//                         PelotonDB
//
// transaction_manager_factory.cpp
//
// Identification: src/backend/concurrency/transaction_manager_factory.cpp
//
// Copyright (c) 2015, Carnegie Mellon University Database Group
//
//===----------------------------------------------------------------------===//

#include "transaction_manager_factory.h"

namespace peloton {
namespace concurrency {
ConcurrencyType TransactionManagerFactory::protocol_ =
    CONCURRENCY_TYPE_OPTIMISTIC;
IsolationLevelType TransactionManagerFactory::isolation_level_ =
    ISOLATION_LEVEL_TYPE_FULL;
}
}
<|endoftext|>


<commit_before>/* v.0.16 25 August 2015
 * Kevin CAIN, www.insightdigital.org
 * Adapted from the openMVG libraries,
 * Copyright (c) 2012-2015 Pierre MOULON.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#include "openMVG/sfm/sfm.hpp"
#include "openMVG/image/image.hpp"

using namespace openMVG;
using namespace openMVG::cameras;
using namespace openMVG::geometry;
using namespace openMVG::image;
using namespace openMVG::sfm;
using namespace openMVG::features;

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/progress/progress.hpp"
#include <stdlib.h>
#include <stdio.h>
#include <cmath>
#include <iterator>
#include <iomanip>

/// Naive image bilinear resampling of an image for thumbnail generation
template <typename ImageT>
ImageT
create_thumbnail
(
  const ImageT & image,
  int thumb_width,
  int thumb_height
);

/* Notes:
 * - An MVE2 scene appears to duplicate camera rot matrix and trans vector per-view data in 'meta.ini'
 *   within the first section of 'synth_0.out'.
 * - We do not save the original, instead we rely on the undistorted image from openMVG.
 * - We do not output thumbnails or EXIF blobs, as these appear only to be used only for the GUI UMVE.
 * - To avoid encoding loss, openMVG images should be written as .PNG if undistorted images are *not* computed.
 * - In OpenMVG, some views may have some missing poses; MVE does *not* require a contiguous camera index.
 *
 *  For information on the target for this conversion, please see the MVE (v2) File format:
 *  https://github.com/simonfuhrmann/mve/wiki/MVE-File-Format
 */

bool exportToMVE2Format(
  const SfM_Data & sfm_data,
  const std::string & sOutDirectory // Output MVE2 files directory
  )
{
  bool bOk = true;
  // Create basis directory structure
  if (!stlplus::is_folder(sOutDirectory))
  {
    cout << "\033[1;31mCreating directory:  " << sOutDirectory << "\033[0m\n";
    stlplus::folder_create(sOutDirectory);
    bOk = stlplus::is_folder(sOutDirectory);
  }

  if (!bOk)
  {
    std::cerr << "Cannot access one of the desired output directories" << std::endl;
	  return false;
  }

  // Export the SfM_Data scene to the MVE2 format
  {
    // Create 'views' subdirectory
    const string sOutViewsDirectory = stlplus::folder_append_separator(sOutDirectory) + "views";
    if (!stlplus::folder_exists(sOutViewsDirectory))
    {
      cout << "\033[1;31mCreating directory:  " << sOutViewsDirectory << "\033[0m\n";
      stlplus::folder_create(sOutViewsDirectory);
    }

    // Prepare to write bundle file
    // Get cameras and features from OpenMVG
    const size_t cameraCount = std::distance(sfm_data.GetViews().begin(), sfm_data.GetViews().end());
    // Tally global set of feature landmarks
    const Landmarks & landmarks = sfm_data.GetLandmarks();
    const size_t featureCount = std::distance(landmarks.begin(), landmarks.end());
    const std::string filename = "synth_0.out";
    std::cout << "Writing bundle (" << cameraCount << " cameras, "
        << featureCount << " features): to " << filename << "...\n";
    std::ofstream out(stlplus::folder_append_separator(sOutDirectory) + filename);
    out << "drews 1.0\n";  // MVE expects this header
    out << cameraCount << " " << featureCount << "\n";

    // Export (calibrated) views as undistorted images
    C_Progress_display my_progress_bar(sfm_data.GetViews().size());
    std::pair<int,int> w_h_image_size;
    Image<RGBColor> image, image_ud, thumbnail;
    std::string sOutViewIteratorDirectory;
    for(Views::const_iterator iter = sfm_data.GetViews().begin();
      iter != sfm_data.GetViews().end(); ++iter, ++my_progress_bar)
    {
      const View * view = iter->second.get();

      // Create current view subdirectory 'view_xxxx.mve'
      std::ostringstream padding;
      padding << std::setw(4) << std::setfill('0') << view->id_view;
      sOutViewIteratorDirectory = stlplus::folder_append_separator(sOutViewsDirectory) + "view_" + padding.str() + ".mve";
      if (!stlplus::folder_exists(sOutViewIteratorDirectory))
      {
        stlplus::folder_create(sOutViewIteratorDirectory);
      }

      // We have a valid view with a corresponding camera & pose
      const std::string srcImage = stlplus::create_filespec(sfm_data.s_root_path, view->s_Img_path);
      const std::string dstImage =
        stlplus::create_filespec(stlplus::folder_append_separator(sOutViewIteratorDirectory), "undistorted","png");

      if (sfm_data.IsPoseAndIntrinsicDefined(view))
      {
        Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
        const IntrinsicBase * cam = iterIntrinsic->second.get();
        if (cam->have_disto())
        {
          // Undistort and save the image
          ReadImage(srcImage.c_str(), &image);
          UndistortImage(image, cam, image_ud, BLACK);
          WriteImage(dstImage.c_str(), image_ud);
        }
        else // (no distortion)
        {
          // If extensions match, copy the PNG image
          if (stlplus::extension_part(srcImage) == "PNG" ||
            stlplus::extension_part(srcImage) == "png")
          {
            stlplus::file_copy(srcImage, dstImage);
          }
          else
          {
            ReadImage( srcImage.c_str(), &image);
            WriteImage( dstImage.c_str(), image);
          }
        }

        // Prepare to write an MVE 'meta.ini' file for the current view
        const Pose3 pose = sfm_data.GetPoseOrDie(view);
        const Pinhole_Intrinsic * pinhole_cam = static_cast<const Pinhole_Intrinsic *>(cam);

        const Mat3 rotation = pose.rotation();
        const Vec3 translation = pose.translation();
        // Pixel aspect: assuming square pixels
        const float pixelAspect = 1.f;
        // Focal length and principal point must be normalized (0..1)
        const float flen = pinhole_cam->focal() / static_cast<double>(std::max(cam->w(), cam->h()));
        const float ppX = std::abs(pinhole_cam->principal_point()(0)/cam->w());
        const float ppY = std::abs(pinhole_cam->principal_point()(1)/cam->h());

        // For each camera, write to bundle:  focal length, radial distortion[0-1], rotation matrix[0-8], translation vector[0-2]
        std::ostringstream fileOut;
        fileOut
          << "#MVE view meta data is stored in INI-file syntax." << fileOut.widen('\n')
          << "#This file is generated, formatting will get lost." << fileOut.widen('\n')
          << fileOut.widen('\n')
          << "[camera]" << fileOut.widen('\n')
          << "focal_length = " << flen << fileOut.widen('\n')
          << "pixel_aspect = " << pixelAspect << fileOut.widen('\n')
          << "principal_point = " << ppX << " " << ppY << fileOut.widen('\n')
          << "rotation = " << rotation(0, 0) << " " << rotation(0, 1) << " " << rotation(0, 2) << " "
          << rotation(1, 0) << " " << rotation(1, 1) << " " << rotation(1, 2) << " "
          << rotation(2, 0) << " " << rotation(2, 1) << " " << rotation(2, 2) << fileOut.widen('\n')
          << "translation = " << translation[0] << " " << translation[1] << " "
          << translation[2] << " " << fileOut.widen('\n')
          << "[view]" << fileOut.widen('\n')
          << "id = " << view->id_view << fileOut.widen('\n')
          << "name = " << srcImage.c_str() << fileOut.widen('\n');

        // To do:  trim any extra separator(s) from openMVG name we receive, e.g.:
        // '/home/insight/openMVG_KevinCain/openMVG_Build/software/SfM/ImageDataset_SceauxCastle/images//100_7100.JPG'
        std::ofstream file(
          stlplus::create_filespec(stlplus::folder_append_separator(sOutViewIteratorDirectory),
          "meta","ini").c_str());
        file << fileOut.str();
        file.close();

        out
          << flen << " " << "0" << " " << "0" << "\n"  // Write '0' distortion values for pre-corrected images
          << rotation(0, 0) << " " << rotation(0, 1) << " " << rotation(0, 2) << "\n"
          << rotation(1, 0) << " " << rotation(1, 1) << " " << rotation(1, 2) << "\n"
          << rotation(2, 0) << " " << rotation(2, 1) << " " << rotation(2, 2) << "\n"
          << translation[0] << " " << translation[1] << " " << translation[2] << "\n";
      }
      else
      {
        // export a camera without pose & intrinsic info (export {0})
        // see: https://github.com/simonfuhrmann/mve/blob/952a80b0be48e820b8c72de1d3df06efc3953bd3/libs/mve/bundle_io.cc#L448
        for (int i = 0; i < 5 * 3; ++i)
          out << "0" << (i % 3 == 2 ? "\n" : " ");
        continue;
      }

      // Save a thumbnail image "thumbnail.png", 50x50 pixels
      thumbnail = create_thumbnail(image, 50, 50);
      const std::string dstThumbnailImage =
        stlplus::create_filespec(stlplus::folder_append_separator(sOutViewIteratorDirectory), "thumbnail","png");
      WriteImage(dstThumbnailImage.c_str(), thumbnail);
    }

    // For each feature, write to bundle:  position XYZ[0-3], color RGB[0-2], all ref.view_id & ref.feature_id
    // The following method is adapted from Simon Fuhrmann's MVE project:
    // https://github.com/simonfuhrmann/mve/blob/e3db7bc60ce93fe51702ba77ef480e151f927c23/libs/mve/bundle_io.cc

    for (Landmarks::const_iterator iterLandmarks = landmarks.begin(); iterLandmarks != landmarks.end(); ++iterLandmarks)
    {
      const Vec3 exportPoint = iterLandmarks->second.X;
      out << exportPoint.x() << " " << exportPoint.y() << " " << exportPoint.z() << "\n";
      out << 250 << " " << 100 << " " << 150 << "\n";  // Write arbitrary RGB color, see above note

      // Tally set of feature observations
      const Observations & obs = iterLandmarks->second.obs;
      const size_t featureCount = std::distance(obs.begin(), obs.end());
      out << featureCount;

      for (Observations::const_iterator itObs = obs.begin(); itObs != obs.end(); ++itObs)
      {
          const IndexT viewId = itObs->first;
          const IndexT featId = itObs->second.id_feat;
          out << " " << viewId << " " << featId << " 0";
      }
      out << "\n";
    }
    out.close();
  }
  return bOk;
}

int main(int argc, char *argv[])
{

  CmdLine cmd;
  std::string sSfM_Data_Filename;
  std::string sOutDir = "";
  cmd.add( make_option('i', sSfM_Data_Filename, "sfmdata") );
  cmd.add( make_option('o', sOutDir, "outdir") );
  std::cout << "Note:  this program writes output in MVE file format.\n";

  try {
      if (argc == 1) throw std::string("Invalid command line parameter.");
      cmd.process(argc, argv);
  } catch(const std::string& s) {
      std::cerr << "Usage: " << argv[0] << '\n'
      << "[-i|--sfmdata] filename, the SfM_Data file to convert\n"
      << "[-o|--outdir] path\n"
      << std::endl;

      std::cerr << s << std::endl;
      return EXIT_FAILURE;
  }

  // Create output dir
  if (!stlplus::folder_exists(sOutDir))
    stlplus::folder_create(sOutDir);

  // Read the input SfM scene
  SfM_Data sfm_data;
  if (!Load(sfm_data, sSfM_Data_Filename, ESfM_Data(ALL))) {
    std::cerr << std::endl
      << "The input SfM_Data file \""<< sSfM_Data_Filename << "\" cannot be read." << std::endl;
    return EXIT_FAILURE;
  }

  if (exportToMVE2Format(sfm_data, stlplus::folder_append_separator(sOutDir) + "MVE"))
    return( EXIT_SUCCESS );
  else
    return( EXIT_FAILURE );
}

/// Naive image bilinear resampling of an image for thumbnail generation
/// Inspired by create_thumbnail from MVE (cropping is here ignored)
template <typename ImageT>
ImageT
create_thumbnail
(
  const ImageT & image,
  int thumb_width,
  int thumb_height
)
{
  const int width = image.Width();
  const int height = image.Height();
  const float image_aspect = static_cast<float>(width) / height;
  const float thumb_aspect = static_cast<float>(thumb_width) / thumb_height;

  int rescale_width, rescale_height;
  if (image_aspect > thumb_aspect)
  {
    rescale_width = std::ceil(thumb_height * image_aspect);
    rescale_height = thumb_height;
  }
  else
  {
    rescale_width = thumb_width;
    rescale_height = std::ceil(thumb_width / image_aspect);
  }

  // Generation of the sampling grid
  std::vector< std::pair<float,float> > sampling_grid;
  sampling_grid.reserve(rescale_height * rescale_width);
  for ( int i = 0 ; i < rescale_height ; ++i )
  {
    for ( int j = 0 ; j < rescale_width ; ++j )
    {
      const float dx = static_cast<float>(j) * width / rescale_width;
      const float dy = static_cast<float>(i) * height / rescale_height;
      sampling_grid.push_back( std::make_pair( dy , dx ) ) ;
    }
  }

  const Sampler2d<SamplerLinear> sampler;
  ImageT imageOut;
  GenericRessample(image, sampling_grid, rescale_width, rescale_height, sampler, imageOut);
  return imageOut;
}
<commit_msg>[mve] fix export for unreconstructed views<commit_after>/* v.0.16 25 August 2015
 * Kevin CAIN, www.insightdigital.org
 * Adapted from the openMVG libraries,
 * Copyright (c) 2012-2015 Pierre MOULON.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#include "openMVG/sfm/sfm.hpp"
#include "openMVG/image/image.hpp"

using namespace openMVG;
using namespace openMVG::cameras;
using namespace openMVG::geometry;
using namespace openMVG::image;
using namespace openMVG::sfm;
using namespace openMVG::features;

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/progress/progress.hpp"
#include <stdlib.h>
#include <stdio.h>
#include <cmath>
#include <iterator>
#include <iomanip>

/// Naive image bilinear resampling of an image for thumbnail generation
template <typename ImageT>
ImageT
create_thumbnail
(
  const ImageT & image,
  int thumb_width,
  int thumb_height
);

/* Notes:
 * - An MVE2 scene appears to duplicate camera rot matrix and trans vector per-view data in 'meta.ini'
 *   within the first section of 'synth_0.out'.
 * - We do not save the original, instead we rely on the undistorted image from openMVG.
 * - We do not output thumbnails or EXIF blobs, as these appear only to be used only for the GUI UMVE.
 * - To avoid encoding loss, openMVG images should be written as .PNG if undistorted images are *not* computed.
 * - In OpenMVG, some views may have some missing poses; MVE does *not* require a contiguous camera index.
 *
 *  For information on the target for this conversion, please see the MVE (v2) File format:
 *  https://github.com/simonfuhrmann/mve/wiki/MVE-File-Format
 */

bool exportToMVE2Format(
  const SfM_Data & sfm_data,
  const std::string & sOutDirectory // Output MVE2 files directory
  )
{
  bool bOk = true;
  // Create basis directory structure
  if (!stlplus::is_folder(sOutDirectory))
  {
    cout << "\033[1;31mCreating directory:  " << sOutDirectory << "\033[0m\n";
    stlplus::folder_create(sOutDirectory);
    bOk = stlplus::is_folder(sOutDirectory);
  }

  if (!bOk)
  {
    std::cerr << "Cannot access one of the desired output directories" << std::endl;
	  return false;
  }

  // Export the SfM_Data scene to the MVE2 format
  {
    // Create 'views' subdirectory
    const string sOutViewsDirectory = stlplus::folder_append_separator(sOutDirectory) + "views";
    if (!stlplus::folder_exists(sOutViewsDirectory))
    {
      cout << "\033[1;31mCreating directory:  " << sOutViewsDirectory << "\033[0m\n";
      stlplus::folder_create(sOutViewsDirectory);
    }

    // Prepare to write bundle file
    // Get cameras and features from OpenMVG
    const size_t cameraCount = std::distance(sfm_data.GetViews().begin(), sfm_data.GetViews().end());
    // Tally global set of feature landmarks
    const Landmarks & landmarks = sfm_data.GetLandmarks();
    const size_t featureCount = std::distance(landmarks.begin(), landmarks.end());
    const std::string filename = "synth_0.out";
    std::cout << "Writing bundle (" << cameraCount << " cameras, "
        << featureCount << " features): to " << filename << "...\n";
    std::ofstream out(stlplus::folder_append_separator(sOutDirectory) + filename);
    out << "drews 1.0\n";  // MVE expects this header
    out << cameraCount << " " << featureCount << "\n";

    // Export (calibrated) views as undistorted images
    C_Progress_display my_progress_bar(sfm_data.GetViews().size());
    std::pair<int,int> w_h_image_size;
    Image<RGBColor> image, image_ud, thumbnail;
    std::string sOutViewIteratorDirectory;
    for(Views::const_iterator iter = sfm_data.GetViews().begin();
      iter != sfm_data.GetViews().end(); ++iter, ++my_progress_bar)
    {
      const View * view = iter->second.get();

      if (sfm_data.IsPoseAndIntrinsicDefined(view))
      {
        // Create current view subdirectory 'view_xxxx.mve'
        std::ostringstream padding;
        padding << std::setw(4) << std::setfill('0') << view->id_view;
        sOutViewIteratorDirectory = stlplus::folder_append_separator(sOutViewsDirectory) + "view_" + padding.str() + ".mve";
        if (!stlplus::folder_exists(sOutViewIteratorDirectory))
        {
          stlplus::folder_create(sOutViewIteratorDirectory);
        }

        // We have a valid view with a corresponding camera & pose
        const std::string srcImage = stlplus::create_filespec(sfm_data.s_root_path, view->s_Img_path);
        const std::string dstImage =
          stlplus::create_filespec(stlplus::folder_append_separator(sOutViewIteratorDirectory), "undistorted","png");

        Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
        const IntrinsicBase * cam = iterIntrinsic->second.get();
        if (cam->have_disto())
        {
          // Undistort and save the image
          ReadImage(srcImage.c_str(), &image);
          UndistortImage(image, cam, image_ud, BLACK);
          WriteImage(dstImage.c_str(), image_ud);
        }
        else // (no distortion)
        {
          // If extensions match, copy the PNG image
          if (stlplus::extension_part(srcImage) == "PNG" ||
            stlplus::extension_part(srcImage) == "png")
          {
            stlplus::file_copy(srcImage, dstImage);
          }
          else
          {
            ReadImage( srcImage.c_str(), &image);
            WriteImage( dstImage.c_str(), image);
          }
        }

        // Prepare to write an MVE 'meta.ini' file for the current view
        const Pose3 pose = sfm_data.GetPoseOrDie(view);
        const Pinhole_Intrinsic * pinhole_cam = static_cast<const Pinhole_Intrinsic *>(cam);

        const Mat3 rotation = pose.rotation();
        const Vec3 translation = pose.translation();
        // Pixel aspect: assuming square pixels
        const float pixelAspect = 1.f;
        // Focal length and principal point must be normalized (0..1)
        const float flen = pinhole_cam->focal() / static_cast<double>(std::max(cam->w(), cam->h()));
        const float ppX = std::abs(pinhole_cam->principal_point()(0)/cam->w());
        const float ppY = std::abs(pinhole_cam->principal_point()(1)/cam->h());

        // For each camera, write to bundle:  focal length, radial distortion[0-1], rotation matrix[0-8], translation vector[0-2]
        std::ostringstream fileOut;
        fileOut
          << "#MVE view meta data is stored in INI-file syntax." << fileOut.widen('\n')
          << "#This file is generated, formatting will get lost." << fileOut.widen('\n')
          << fileOut.widen('\n')
          << "[camera]" << fileOut.widen('\n')
          << "focal_length = " << flen << fileOut.widen('\n')
          << "pixel_aspect = " << pixelAspect << fileOut.widen('\n')
          << "principal_point = " << ppX << " " << ppY << fileOut.widen('\n')
          << "rotation = " << rotation(0, 0) << " " << rotation(0, 1) << " " << rotation(0, 2) << " "
          << rotation(1, 0) << " " << rotation(1, 1) << " " << rotation(1, 2) << " "
          << rotation(2, 0) << " " << rotation(2, 1) << " " << rotation(2, 2) << fileOut.widen('\n')
          << "translation = " << translation[0] << " " << translation[1] << " "
          << translation[2] << " " << fileOut.widen('\n')
          << "[view]" << fileOut.widen('\n')
          << "id = " << view->id_view << fileOut.widen('\n')
          << "name = " << srcImage.c_str() << fileOut.widen('\n');

        // To do:  trim any extra separator(s) from openMVG name we receive, e.g.:
        // '/home/insight/openMVG_KevinCain/openMVG_Build/software/SfM/ImageDataset_SceauxCastle/images//100_7100.JPG'
        std::ofstream file(
          stlplus::create_filespec(stlplus::folder_append_separator(sOutViewIteratorDirectory),
          "meta","ini").c_str());
        file << fileOut.str();
        file.close();

        out
          << flen << " " << "0" << " " << "0" << "\n"  // Write '0' distortion values for pre-corrected images
          << rotation(0, 0) << " " << rotation(0, 1) << " " << rotation(0, 2) << "\n"
          << rotation(1, 0) << " " << rotation(1, 1) << " " << rotation(1, 2) << "\n"
          << rotation(2, 0) << " " << rotation(2, 1) << " " << rotation(2, 2) << "\n"
          << translation[0] << " " << translation[1] << " " << translation[2] << "\n";
      }
      else
      {
        // export a camera without pose & intrinsic info (export {0})
        // see: https://github.com/simonfuhrmann/mve/blob/952a80b0be48e820b8c72de1d3df06efc3953bd3/libs/mve/bundle_io.cc#L448
        for (int i = 0; i < 5 * 3; ++i)
          out << "0" << (i % 3 == 2 ? "\n" : " ");
        continue;
      }

      // Save a thumbnail image "thumbnail.png", 50x50 pixels
      thumbnail = create_thumbnail(image, 50, 50);
      const std::string dstThumbnailImage =
        stlplus::create_filespec(stlplus::folder_append_separator(sOutViewIteratorDirectory), "thumbnail","png");
      WriteImage(dstThumbnailImage.c_str(), thumbnail);
    }

    // For each feature, write to bundle:  position XYZ[0-3], color RGB[0-2], all ref.view_id & ref.feature_id
    // The following method is adapted from Simon Fuhrmann's MVE project:
    // https://github.com/simonfuhrmann/mve/blob/e3db7bc60ce93fe51702ba77ef480e151f927c23/libs/mve/bundle_io.cc

    for (Landmarks::const_iterator iterLandmarks = landmarks.begin(); iterLandmarks != landmarks.end(); ++iterLandmarks)
    {
      const Vec3 exportPoint = iterLandmarks->second.X;
      out << exportPoint.x() << " " << exportPoint.y() << " " << exportPoint.z() << "\n";
      out << 250 << " " << 100 << " " << 150 << "\n";  // Write arbitrary RGB color, see above note

      // Tally set of feature observations
      const Observations & obs = iterLandmarks->second.obs;
      const size_t featureCount = std::distance(obs.begin(), obs.end());
      out << featureCount;

      for (Observations::const_iterator itObs = obs.begin(); itObs != obs.end(); ++itObs)
      {
          const IndexT viewId = itObs->first;
          const IndexT featId = itObs->second.id_feat;
          out << " " << viewId << " " << featId << " 0";
      }
      out << "\n";
    }
    out.close();
  }
  return bOk;
}

int main(int argc, char *argv[])
{

  CmdLine cmd;
  std::string sSfM_Data_Filename;
  std::string sOutDir = "";
  cmd.add( make_option('i', sSfM_Data_Filename, "sfmdata") );
  cmd.add( make_option('o', sOutDir, "outdir") );
  std::cout << "Note:  this program writes output in MVE file format.\n";

  try {
      if (argc == 1) throw std::string("Invalid command line parameter.");
      cmd.process(argc, argv);
  } catch(const std::string& s) {
      std::cerr << "Usage: " << argv[0] << '\n'
      << "[-i|--sfmdata] filename, the SfM_Data file to convert\n"
      << "[-o|--outdir] path\n"
      << std::endl;

      std::cerr << s << std::endl;
      return EXIT_FAILURE;
  }

  // Create output dir
  if (!stlplus::folder_exists(sOutDir))
    stlplus::folder_create(sOutDir);

  // Read the input SfM scene
  SfM_Data sfm_data;
  if (!Load(sfm_data, sSfM_Data_Filename, ESfM_Data(ALL))) {
    std::cerr << std::endl
      << "The input SfM_Data file \""<< sSfM_Data_Filename << "\" cannot be read." << std::endl;
    return EXIT_FAILURE;
  }

  if (exportToMVE2Format(sfm_data, stlplus::folder_append_separator(sOutDir) + "MVE"))
    return( EXIT_SUCCESS );
  else
    return( EXIT_FAILURE );
}

/// Naive image bilinear resampling of an image for thumbnail generation
/// Inspired by create_thumbnail from MVE (cropping is here ignored)
template <typename ImageT>
ImageT
create_thumbnail
(
  const ImageT & image,
  int thumb_width,
  int thumb_height
)
{
  const int width = image.Width();
  const int height = image.Height();
  const float image_aspect = static_cast<float>(width) / height;
  const float thumb_aspect = static_cast<float>(thumb_width) / thumb_height;

  int rescale_width, rescale_height;
  if (image_aspect > thumb_aspect)
  {
    rescale_width = std::ceil(thumb_height * image_aspect);
    rescale_height = thumb_height;
  }
  else
  {
    rescale_width = thumb_width;
    rescale_height = std::ceil(thumb_width / image_aspect);
  }

  // Generation of the sampling grid
  std::vector< std::pair<float,float> > sampling_grid;
  sampling_grid.reserve(rescale_height * rescale_width);
  for ( int i = 0 ; i < rescale_height ; ++i )
  {
    for ( int j = 0 ; j < rescale_width ; ++j )
    {
      const float dx = static_cast<float>(j) * width / rescale_width;
      const float dy = static_cast<float>(i) * height / rescale_height;
      sampling_grid.push_back( std::make_pair( dy , dx ) ) ;
    }
  }

  const Sampler2d<SamplerLinear> sampler;
  ImageT imageOut;
  GenericRessample(image, sampling_grid, rescale_width, rescale_height, sampler, imageOut);
  return imageOut;
}
<|endoftext|>


<commit_before>#include "TMC2130Stepper.h"

#define REG_COOLCONF 0X6D

///////////////////////////////////////////////////////////////////////////////////////
// REG_COOLCONF

void TMC2130Stepper::COOLCONF(uint32_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set COOLCONF: ");
	Serial.println(value);
#endif
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, value, 0xFFFFFFFF);
}

uint8_t TMC2130Stepper::sg_min() {return val_semin;}

void TMC2130Stepper::sg_min(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set semin: ");
	Serial.println(value);
#endif
	if (value > 15) value = 15;
	val_semin = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, value, 0xF);
}

uint8_t TMC2130Stepper::sg_step_width() {return val_seup;}

void TMC2130Stepper::sg_step_width(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set seup: ");
	Serial.println(value);
#endif
	uint32_t v;
	switch(value) {
		case 1: v = 0x0; break;
		case 2: v = 0x1; break;
		case 4: v = 0x2; break;
		case 8: v = 0x3; break;
		default: return;
	}
	val_seup = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, v << 5, 0x60);
}

uint8_t TMC2130Stepper::sg_max() {return val_semax;}

void TMC2130Stepper::sg_max(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set semax: ");
	Serial.println(value);
#endif
	if (value > 15) value = 15;
	val_semin = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)value << 8, 0xF00);
}

uint8_t TMC2130Stepper::sg_current_decrease() {return val_sedn;}

void TMC2130Stepper::sg_current_decrease(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set sedn: ");
	Serial.println(value);
#endif

	uint32_t v;
	switch(value) {
		case 32: v = 0b00; break;
		case  8: v = 0b01; break;
		case  2: v = 0b10; break;
		case  1: v = 0b11; break;
		default: return;
	}

	val_sedn = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, v << 13, 0x6000);
}

uint8_t TMC2130Stepper::smart_min_current() {return val_seimin;}

void TMC2130Stepper::smart_min_current(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set seimin: ");
	Serial.println(value);
#endif
	if (value > 1) value = 1;
	val_seimin = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)value << 15, 0x8000);
}

int8_t TMC2130Stepper::sg_stall_value() {return val_sgt;}

void TMC2130Stepper::sg_stall_value(int8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set sgt: ");
	Serial.println(value);
#endif
	if (value < -64) value = -64;
	else if (value > 63) value = 63;
	val_sgt = value;
	if (value < 0)
		send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)(value & 0x40) << 16, 0x7F0000UL);
	else
		send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)(value) << 16, 0x7F0000UL);
}

bool TMC2130Stepper::sg_filter() {return val_sfilt;}

void TMC2130Stepper::sg_filter(bool value) {
#ifdef TMC2130DEBUG
	Serial.print("Set sfilt: ");
	Serial.println(value);
#endif
	if (value > 1) value = 1;
	val_sfilt = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)value << 24, 0b1UL << 24);
}<commit_msg>Fix setting SGT<commit_after>#include "TMC2130Stepper.h"

#define REG_COOLCONF 0X6D

///////////////////////////////////////////////////////////////////////////////////////
// REG_COOLCONF

void TMC2130Stepper::COOLCONF(uint32_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set COOLCONF: ");
	Serial.println(value);
#endif
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, value, 0xFFFFFFFF);
}

uint8_t TMC2130Stepper::sg_min() {return val_semin;}

void TMC2130Stepper::sg_min(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set semin: ");
	Serial.println(value);
#endif
	if (value > 15) value = 15;
	val_semin = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, value, 0xF);
}

uint8_t TMC2130Stepper::sg_step_width() {return val_seup;}

void TMC2130Stepper::sg_step_width(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set seup: ");
	Serial.println(value);
#endif
	uint32_t v;
	switch(value) {
		case 1: v = 0x0; break;
		case 2: v = 0x1; break;
		case 4: v = 0x2; break;
		case 8: v = 0x3; break;
		default: return;
	}
	val_seup = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, v << 5, 0x60);
}

uint8_t TMC2130Stepper::sg_max() {return val_semax;}

void TMC2130Stepper::sg_max(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set semax: ");
	Serial.println(value);
#endif
	if (value > 15) value = 15;
	val_semin = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)value << 8, 0xF00);
}

uint8_t TMC2130Stepper::sg_current_decrease() {return val_sedn;}

void TMC2130Stepper::sg_current_decrease(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set sedn: ");
	Serial.println(value);
#endif

	uint32_t v;
	switch(value) {
		case 32: v = 0b00; break;
		case  8: v = 0b01; break;
		case  2: v = 0b10; break;
		case  1: v = 0b11; break;
		default: return;
	}

	val_sedn = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, v << 13, 0x6000);
}

uint8_t TMC2130Stepper::smart_min_current() {return val_seimin;}

void TMC2130Stepper::smart_min_current(uint8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set seimin: ");
	Serial.println(value);
#endif
	if (value > 1) value = 1;
	val_seimin = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)value << 15, 0x8000);
}

int8_t TMC2130Stepper::sg_stall_value() {return val_sgt;}

void TMC2130Stepper::sg_stall_value(int8_t value) {
#ifdef TMC2130DEBUG
	Serial.print("Set sgt: ");
	Serial.println(value);
#endif
	if (value < -64) value = -64;
	else if (value > 63) value = 63;
	val_sgt = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)(value) << 16, 0x7F0000UL);
}

bool TMC2130Stepper::sg_filter() {return val_sfilt;}

void TMC2130Stepper::sg_filter(bool value) {
#ifdef TMC2130DEBUG
	Serial.print("Set sfilt: ");
	Serial.println(value);
#endif
	if (value > 1) value = 1;
	val_sfilt = value;
	send2130(WRITE|REG_COOLCONF, &cur_COOLCONF, (uint32_t)value << 24, 0b1UL << 24);
}<|endoftext|>


<commit_before>#include <unit_test.h>
#include <lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp>
#include <float.h>

class SmoothZTest : public UnitTest
{
public:
	virtual bool run_tests();

	bool brakeUpward();
	bool brakeDownward();
	bool accelerateUpwardFromBrake();
	bool accelerateDownwardFromBrake();

};

bool SmoothZTest::run_tests()
{
	ut_run_test(brakeUpward);
	ut_run_test(brakeDownward);
	ut_run_test(accelerateUpwardFromBrake);
	ut_run_test(accelerateDownwardFromBrake);

	return (_tests_failed == 0);
}

bool SmoothZTest::brakeUpward()
{
	/* Downward flight and want to stop */
	float stick_current = 0.0f; // sticks are at zero position
	float vel_sp_current = 0.0f; // desired velocity is at 0m/s
	float vel_sp_previous = 5.0f; // the demanded previous setpoint was 5m/s downwards
	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
	float acc_max_up = 5.0f;
	float acc_max_down = 2.0f;

	ManualSmoothingZ smooth(vel, stick_current);

	/* overwrite parameters since they might change depending on configuration */
	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
	smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
	smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1

	float dt = 0.1f; // dt is set to 0.1s

	/* It should start with acceleration */
	ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);

	for (int i = 0; i < 100; i++) {

		smooth.smoothVelFromSticks(vel_sp_current, dt);

		/* Test if intention is brake */
		ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);

		/* we should always use upward acceleration */
		ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON));

		/* New setpoint has to be lower than previous setpoint (NED frame) or equal 0. 0 velocity
		 * occurs once the vehicle is at perfect rest. */
		ut_assert_true((vel_sp_current < vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));

		/* We reset the previou setpoint to newest setpoint
		 * and set the current setpoint to 0 because we still want to brake.
		 * We also set vel to previous setpoint where we make the assumption that
		 * the vehicle can perfectly track the setpoints.
		 */
		vel_sp_previous = vel_sp_current;
		vel_sp_current = 0.0f;
		vel = vel_sp_previous;

	}

	return true;
}

bool SmoothZTest::brakeDownward()
{
	/* Downward flight and want to stop */
	float stick_current = 0.0f; // sticks are at zero position
	float vel_sp_current = 0.0f; // desired velocity is 0m/s
	float vel_sp_previous = -5.0f; // the demanded previous setpoint was -5m/s downwards
	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
	float acc_max_up = 5.0f;
	float acc_max_down = 2.0f;

	ManualSmoothingZ smooth(vel, stick_current);

	/* overwrite parameters since they might change depending on configuration */
	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
	smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
	smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1

	float dt = 0.1f; // dt is set to 0.1s

	/* It should start with acceleration */
	ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);

	for (int i = 0; i < 100; i++) {

		smooth.smoothVelFromSticks(vel_sp_current, dt);

		/* Test if intention is brake */
		ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);

		/* New setpoint has to be larger than previous setpoint (NED frame) or equal 0. 0 velocity
		 * occurs once the vehicle is at perfect rest. */
		ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));

		/* we should always use downward acceleration except when vehicle is at rest*/
		if (fabsf(vel_sp_current) < FLT_EPSILON) {
			ut_assert_true(fabsf((smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON));

		} else {
			ut_assert_true(fabsf((smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON));
		}

		/* We reset the previou setpoint to newest setpoint
		 * and set the current setpoint to 0 because we still want to brake.
		 * We also set vel to previous setpoint where we make the assumption that
		 * the vehicle can perfectly track the setpoints.
		 */
		vel_sp_previous = vel_sp_current;
		vel_sp_current = 0.0f;
		vel = vel_sp_previous;

	}

	return true;
}

bool SmoothZTest::accelerateUpwardFromBrake()
{
	/* Downward flight and want to stop */
	float stick_current = -1.0f; // sticks are at full upward position
	float vel_sp_target = -5.0f; // desired velocity is at -5m/s
	float vel_sp_current = vel_sp_target;
	float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
	float acc_max_up = 5.0f;
	float acc_max_down = 2.0f;

	ManualSmoothingZ smooth(vel, stick_current);

	/* overwrite parameters since they might change depending on configuration */
	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
	smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
	smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1

	float dt = 0.1f; // dt is set to 0.1s

	for (int i = 0; i < 100; i++) {

		smooth.smoothVelFromSticks(vel_sp_current, dt);

		/* Test if intention is acceleration */
		ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);

		/* we should always use upward acceleration */
		ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON));

		/* New setpoint has to be larger than previous setpoint or equal to target velocity
		 * vel_sp_current. The negative sign is because of NED frame.
		 */
		ut_assert_true((-vel_sp_current > -vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_previous) < FLT_EPSILON));

		/* We reset the previous setpoint to newest setpoint and reset the current setpoint.
		 * We also set the current velocity to the previous setpoint with the assumption that
		 * the vehicle does perfect tracking.
		 */
		vel_sp_previous = vel_sp_current;
		vel_sp_current = vel_sp_target;
		vel = vel_sp_previous;

	}

	return true;
}

bool SmoothZTest::accelerateDownwardFromBrake()
{
	/* Downward flight and want to stop */
	float stick_current = 1.0f; // sticks are at full downward position
	float vel_sp_target =  5.0f; // desired velocity is at 5m/s
	float vel_sp_current = vel_sp_target;
	float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
	float acc_max_up = 5.0f;
	float acc_max_down = 2.0f;

	ManualSmoothingZ smooth(vel, stick_current);

	/* overwrite parameters since they might change depending on configuration */
	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
	smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
	smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1

	float dt = 0.1f; // dt is set to 0.1s

	for (int i = 0; i < 100; i++) {

		smooth.smoothVelFromSticks(vel_sp_current, dt);

		/* Test if intention is acceleration */
		ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);

		/* we should always use downward acceleration except when target velocity is reached */
		if (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON) {
			ut_assert_true(smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON);

		} else {
			ut_assert_true(smooth.getMaxAcceleration() - acc_max_down < FLT_EPSILON);
		}

		/* New setpoint has to be larger than previous setpoint or equal to target velocity
		 * vel_sp_current (NED frame).
		 */
		ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON));

		/* We reset the previous setpoint to newest setpoint and reset the current setpoint.
		 * We also set the current velocity to the previous setpoint with the assumption that
		 * the vehicle does perfect tracking.
		 */
		vel_sp_previous = vel_sp_current;
		vel_sp_current = vel_sp_target;
		vel = vel_sp_previous;

	}

	return true;
}

ut_declare_test_c(test_smooth_z, SmoothZTest)
<commit_msg>tests smoothz: fix fabsf and consider rest once previous velocity is zero<commit_after>#include <unit_test.h>
#include <lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp>
#include <float.h>

class SmoothZTest : public UnitTest
{
public:
	virtual bool run_tests();

	bool brakeUpward();
	bool brakeDownward();
	bool accelerateUpwardFromBrake();
	bool accelerateDownwardFromBrake();

};

bool SmoothZTest::run_tests()
{
	ut_run_test(brakeUpward);
	ut_run_test(brakeDownward);
	ut_run_test(accelerateUpwardFromBrake);
	ut_run_test(accelerateDownwardFromBrake);

	return (_tests_failed == 0);
}

bool SmoothZTest::brakeUpward()
{
	/* Downward flight and want to stop */
	float stick_current = 0.0f; // sticks are at zero position
	float vel_sp_current = 0.0f; // desired velocity is at 0m/s
	float vel_sp_previous = 5.0f; // the demanded previous setpoint was 5m/s downwards
	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
	float acc_max_up = 5.0f;
	float acc_max_down = 2.0f;

	ManualSmoothingZ smooth(vel, stick_current);

	/* overwrite parameters since they might change depending on configuration */
	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
	smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
	smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1

	float dt = 0.1f; // dt is set to 0.1s

	/* It should start with acceleration */
	ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);

	for (int i = 0; i < 100; i++) {

		smooth.smoothVelFromSticks(vel_sp_current, dt);

		/* Test if intention is brake */
		ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);

		/* we should always use upward acceleration */
		ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON));

		/* New setpoint has to be lower than previous setpoint (NED frame) or equal 0. 0 velocity
		 * occurs once the vehicle is at perfect rest. */
		ut_assert_true((vel_sp_current < vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));

		/* We reset the previou setpoint to newest setpoint
		 * and set the current setpoint to 0 because we still want to brake.
		 * We also set vel to previous setpoint where we make the assumption that
		 * the vehicle can perfectly track the setpoints.
		 */
		vel_sp_previous = vel_sp_current;
		vel_sp_current = 0.0f;
		vel = vel_sp_previous;

	}

	return true;
}

bool SmoothZTest::brakeDownward()
{
	/* Downward flight and want to stop */
	float stick_current = 0.0f; // sticks are at zero position
	float vel_sp_current = 0.0f; // desired velocity is 0m/s
	float vel_sp_previous = -5.0f; // the demanded previous setpoint was -5m/s downwards
	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
	float acc_max_up = 5.0f;
	float acc_max_down = 2.0f;

	ManualSmoothingZ smooth(vel, stick_current);

	/* overwrite parameters since they might change depending on configuration */
	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
	smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
	smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1

	float dt = 0.1f; // dt is set to 0.1s

	/* It should start with acceleration */
	ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);

	for (int i = 0; i < 100; i++) {

		smooth.smoothVelFromSticks(vel_sp_current, dt);

		/* Test if intention is brake */
		ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);

		/* New setpoint has to be larger than previous setpoint (NED frame) or equal 0. 0 velocity
		 * occurs once the vehicle is at perfect rest. */
		ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));

		/* we should always use downward acceleration except when vehicle is at rest*/
		if (fabsf(vel_sp_previous) < FLT_EPSILON) {
			ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON);

		} else {
			ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON);
		}

		/* We reset the previou setpoint to newest setpoint
		 * and set the current setpoint to 0 because we still want to brake.
		 * We also set vel to previous setpoint where we make the assumption that
		 * the vehicle can perfectly track the setpoints.
		 */
		vel_sp_previous = vel_sp_current;
		vel_sp_current = 0.0f;
		vel = vel_sp_previous;

	}

	return true;
}

bool SmoothZTest::accelerateUpwardFromBrake()
{
	/* Downward flight and want to stop */
	float stick_current = -1.0f; // sticks are at full upward position
	float vel_sp_target = -5.0f; // desired velocity is at -5m/s
	float vel_sp_current = vel_sp_target;
	float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
	float acc_max_up = 5.0f;
	float acc_max_down = 2.0f;

	ManualSmoothingZ smooth(vel, stick_current);

	/* overwrite parameters since they might change depending on configuration */
	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
	smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
	smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1

	float dt = 0.1f; // dt is set to 0.1s

	for (int i = 0; i < 100; i++) {

		smooth.smoothVelFromSticks(vel_sp_current, dt);

		/* Test if intention is acceleration */
		ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);

		/* we should always use upward acceleration */
		ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON);

		/* New setpoint has to be larger than previous setpoint or equal to target velocity
		 * vel_sp_current. The negative sign is because of NED frame.
		 */
		ut_assert_true((-vel_sp_current > -vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_previous) < FLT_EPSILON));

		/* We reset the previous setpoint to newest setpoint and reset the current setpoint.
		 * We also set the current velocity to the previous setpoint with the assumption that
		 * the vehicle does perfect tracking.
		 */
		vel_sp_previous = vel_sp_current;
		vel_sp_current = vel_sp_target;
		vel = vel_sp_previous;

	}

	return true;
}

bool SmoothZTest::accelerateDownwardFromBrake()
{
	/* Downward flight and want to stop */
	float stick_current = 1.0f; // sticks are at full downward position
	float vel_sp_target =  5.0f; // desired velocity is at 5m/s
	float vel_sp_current = vel_sp_target;
	float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
	float acc_max_up = 5.0f;
	float acc_max_down = 2.0f;

	ManualSmoothingZ smooth(vel, stick_current);

	/* overwrite parameters since they might change depending on configuration */
	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
	smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
	smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1

	float dt = 0.1f; // dt is set to 0.1s

	for (int i = 0; i < 100; i++) {

		smooth.smoothVelFromSticks(vel_sp_current, dt);

		/* Test if intention is acceleration */
		ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);

		/* we should always use downward acceleration except when target velocity is reached */
		if (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON) {
			ut_assert_true(smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON);

		} else {
			ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON);
		}

		/* New setpoint has to be larger than previous setpoint or equal to target velocity
		 * vel_sp_current (NED frame).
		 */
		ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON));

		/* We reset the previous setpoint to newest setpoint and reset the current setpoint.
		 * We also set the current velocity to the previous setpoint with the assumption that
		 * the vehicle does perfect tracking.
		 */
		vel_sp_previous = vel_sp_current;
		vel_sp_current = vel_sp_target;
		vel = vel_sp_previous;

	}

	return true;
}

ut_declare_test_c(test_smooth_z, SmoothZTest)
<|endoftext|>