<commit_before>// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Albrecht
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_ASSEMBLER_SYSTEM_HH
#define DUNE_GDT_ASSEMBLER_SYSTEM_HH

#include <type_traits>
//#include <vector>
#include <memory>

#include <dune/common/dynmatrix.hh>
#include <dune/common/dynvector.hh>
//#include <dune/common/static_assert.hh>
//#include <dune/common/typetraits.hh>

//#include <dune/stuff/la/container/interfaces.hh>
//#include <dune/stuff/la/container/pattern.hh>
//#ifdef DUNE_STUFF_PROFILER_ENABLED
//# include <dune/stuff/common/profiler.hh>
//#endif
#include <dune/gdt/space/interface.hh>
#include <dune/gdt/space/constraints.hh>

#include "local/codim0.hh"
#include "local/codim1.hh"
#include "gridwalker.hh"

namespace Dune {
namespace GDT {

template< class TestSpaceImp,
          class GridViewImp = typename TestSpaceImp::GridViewType,
          class AnsatzSpaceImp = TestSpaceImp >
class SystemAssembler
  : public GridWalker< GridViewImp >
{
  static_assert(std::is_base_of< SpaceInterface< typename TestSpaceImp::Traits >, TestSpaceImp >::value,
                "TestSpaceImp has to be derived from SpaceInterface!");
  static_assert(std::is_base_of< SpaceInterface< typename AnsatzSpaceImp::Traits >, AnsatzSpaceImp >::value,
                "AnsatzSpaceImp has to be derived from SpaceInterface!");
  typedef GridWalker< GridViewImp > BaseType;
public:
  typedef TestSpaceImp    TestSpaceType;
  typedef AnsatzSpaceImp  AnsatzSpaceType;

  typedef typename BaseType::GridViewType     GridViewType;
  typedef typename BaseType::EntityType       EntityType;
  typedef typename BaseType::IntersectionType IntersectionType;
  typedef typename BaseType::BoundaryInfoType BoundaryInfoType;

private:
  typedef typename TestSpaceType::RangeFieldType RangeFieldType;
  typedef Dune::DynamicMatrix< RangeFieldType > LocalMatrixType;
  typedef Dune::DynamicVector< RangeFieldType > LocalVectorType;

public:
  SystemAssembler(const TestSpaceType& test, const AnsatzSpaceType& ansatz, const GridViewType& grid_view)
    : BaseType(grid_view)
    , test_space_(test)
    , ansatz_space_(ansatz)
  {}

  SystemAssembler(const TestSpaceType& test, const AnsatzSpaceType& ansatz)
    : BaseType(*(test.grid_view()))
    , test_space_(test)
    , ansatz_space_(ansatz)
  {}

  SystemAssembler(const TestSpaceType& test)
    : BaseType(*(test.grid_view()))
    , test_space_(test)
    , ansatz_space_(test_space_)
  {}

  SystemAssembler(const TestSpaceType& test, const GridViewType& grid_view)
    : BaseType(grid_view)
    , test_space_(test)
    , ansatz_space_(test_space_)
  {}

  const TestSpaceType& test_space() const
  {
    return test_space_;
  }

  const AnsatzSpaceType& ansatz_space() const
  {
    return ansatz_space_;
  }

  using BaseType::add;

private:
  template< class ConstraintsType, class MatrixType >
  class LocalMatrixConstraintsWrapper
    : public BaseType::Codim0Object
  {
  public:
    LocalMatrixConstraintsWrapper(const TestSpaceType& t_space,
                                  const AnsatzSpaceType& a_space,
                                  const ApplyOn::WhichEntity< GridViewType >* where,
                                  ConstraintsType& constraints,
                                  MatrixType& matrix)
      : t_space_(t_space)
      , a_space_(a_space)
      , where_(where)
      , constraints_(constraints)
      , matrix_(matrix)
    {}

    virtual ~LocalMatrixConstraintsWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const EntityType& entity) const DS_FINAL
    {
      return where_->apply_on(gv, entity);
    }

    virtual void apply_local(const EntityType& entity) DS_FINAL
    {
      t_space_.local_constraints(a_space_, entity, constraints_);
      for (size_t ii = 0; ii < constraints_.rows(); ++ii) {
        const size_t row = constraints_.globalRow(ii);
        for (size_t jj = 0; jj < constraints_.cols(); ++jj) {
          matrix_.set_entry(row, constraints_.globalCol(jj), constraints_.value(ii, jj));
        }
      }
    } // ... apply_local(...)

  private:
    const TestSpaceType& t_space_;
    const AnsatzSpaceType& a_space_;
    std::unique_ptr< const ApplyOn::WhichEntity< GridViewType > > where_;
    ConstraintsType& constraints_;
    MatrixType& matrix_;
  }; // class LocalMatrixConstraintsWrapper

  template< class ConstraintsType, class VectorType >
  class LocalVectorConstraintsWrapper
      : public BaseType::Codim0Object
  {
  public:
    LocalVectorConstraintsWrapper(const TestSpaceType& t_space,
                                  const ApplyOn::WhichEntity< GridViewType >* where,
                                  ConstraintsType& constraints,
                                  VectorType& vector)
      : t_space_(t_space)
      , where_(where)
      , constraints_(constraints)
      , vector_(vector)
    {}

    virtual ~LocalVectorConstraintsWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const EntityType& entity) const DS_FINAL
    {
      return where_->apply_on(gv, entity);
    }

    virtual void apply_local(const EntityType& entity) DS_FINAL
    {
      t_space_.local_constraints(entity, constraints_);
      for (size_t ii = 0; ii < constraints_.rows(); ++ii) {
        vector_.set_entry(constraints_.globalRow(ii), RangeFieldType(0));
      }
    }

  private:
    const TestSpaceType& t_space_;
    std::unique_ptr< const ApplyOn::WhichEntity< GridViewType > > where_;
    ConstraintsType& constraints_;
    VectorType& vector_;
  }; // class LocalVectorConstraintsWrapper

  class NeedsTmpMatrixStorage
  {
  public:
    static size_t safely_get(const std::vector< size_t >& vec, const size_t ii)
    {
      assert(ii < vec.size());
      return vec[ii];
    }

    NeedsTmpMatrixStorage(const std::vector< size_t >& num_tmp_objects,
                          const size_t max_rows,
                          const size_t max_cols)
      : matrices_({ std::vector< LocalMatrixType >(safely_get(num_tmp_objects, 0),
                                                   LocalMatrixType(max_rows, max_cols, RangeFieldType(0)))
                  , std::vector< LocalMatrixType >(safely_get(num_tmp_objects, 1),
                                                   LocalMatrixType(max_rows, max_cols, RangeFieldType(0))) })
      , indices_(4, Dune::DynamicVector< size_t >(std::max(max_rows, max_cols)))
    {}

    virtual ~NeedsTmpMatrixStorage() {}

    std::vector< std::vector< LocalMatrixType > >& matrices()
    {
      return matrices_;
    }

    std::vector< Dune::DynamicVector< size_t > >& indices()
    {
      return indices_;
    }

  protected:
    std::vector< std::vector< LocalMatrixType > > matrices_;
    std::vector< Dune::DynamicVector< size_t > > indices_;
  }; // class NeedsTmpMatrixStorage

  class NeedsTmpVectorStorage
  {
  public:
    static size_t safely_get(const std::vector< size_t >& vec, const size_t ii)
    {
      assert(ii < vec.size());
      return vec[ii];
    }

    NeedsTmpVectorStorage(const std::vector< size_t >& num_tmp_objects,
                          const size_t max_size)
      : vectors_({ std::vector< LocalVectorType >(safely_get(num_tmp_objects, 0),
                                                  LocalVectorType(max_size, RangeFieldType(0)))
                 , std::vector< LocalVectorType >(safely_get(num_tmp_objects, 1),
                                                  LocalVectorType(max_size, RangeFieldType(0))) })
      , indices_(max_size)
    {}

    virtual ~NeedsTmpVectorStorage() {}

    std::vector< std::vector< LocalVectorType > >& vectors()
    {
      return vectors_;
    }

    Dune::DynamicVector< size_t >& indices()
    {
      return indices_;
    }

  protected:
    std::vector< std::vector< LocalVectorType > > vectors_;
    Dune::DynamicVector< size_t > indices_;
  }; // class NeedsTmpVectorStorage

  template< class LocalVolumeMatrixAssembler, class MatrixType >
  class LocalVolumeMatrixAssemblerWrapper
    : public BaseType::Codim0Object
    , NeedsTmpMatrixStorage
  {
  public:
    LocalVolumeMatrixAssemblerWrapper(const TestSpaceType& t_space,
                                      const AnsatzSpaceType& a_space,
                                      const ApplyOn::WhichEntity< GridViewType >* where,
                                      const LocalVolumeMatrixAssembler& localAssembler,
                                      MatrixType& matrix)
      : NeedsTmpMatrixStorage(localAssembler.numTmpObjectsRequired(),
                              t_space.mapper().maxNumDofs(),
                              a_space.mapper().maxNumDofs())
      , t_space_(t_space)
      , a_space_(a_space)
      , where_(where)
      , localMatrixAssembler_(localAssembler)
      , matrix_(matrix)
    {}

    virtual ~LocalVolumeMatrixAssemblerWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const EntityType& entity) const DS_FINAL
    {
      return where_->apply_on(gv, entity);
    }

    virtual void apply_local(const EntityType& entity) DS_FINAL
    {
      localMatrixAssembler_.assembleLocal(t_space_, a_space_, entity, matrix_, this->matrices(), this->indices());
    }

  private:
    const TestSpaceType& t_space_;
    const AnsatzSpaceType& a_space_;
    std::unique_ptr< const ApplyOn::WhichEntity< GridViewType > > where_;
    const LocalVolumeMatrixAssembler& localMatrixAssembler_;
    MatrixType& matrix_;
  }; // class LocalVolumeMatrixAssemblerWrapper

  template< class LocalVolumeVectorAssembler, class VectorType >
  class LocalVolumeVectorAssemblerWrapper
    : public BaseType::Codim0Object
    , NeedsTmpVectorStorage
  {
  public:
    LocalVolumeVectorAssemblerWrapper(const TestSpaceType& space,
                                      const ApplyOn::WhichEntity< GridViewType >* where,
                                      const LocalVolumeVectorAssembler& localAssembler,
                                      VectorType& vector)
      : NeedsTmpVectorStorage(localAssembler.numTmpObjectsRequired(), space.mapper().maxNumDofs())
      , space_(space)
      , where_(where)
      , localVectorAssembler_(localAssembler)
      , vector_(vector)
    {}

    virtual ~LocalVolumeVectorAssemblerWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const EntityType& entity) const DS_FINAL
    {
      return where_->apply_on(gv, entity);
    }

    virtual void apply_local(const EntityType& entity) DS_FINAL
    {
      localVectorAssembler_.assembleLocal(space_, entity, vector_, this->vectors(), this->indices());
    }

  private:
    const TestSpaceType& space_;
    std::unique_ptr< const ApplyOn::WhichEntity< GridViewType > > where_;
    const LocalVolumeVectorAssembler& localVectorAssembler_;
    VectorType& vector_;
  }; // class LocalVolumeVectorAssemblerWrapper

  template< class LocalFaceVectorAssembler, class VectorType >
  class LocalFaceVectorAssemblerWrapper
    : public BaseType::Codim1Object
    , NeedsTmpVectorStorage
  {
  public:
    LocalFaceVectorAssemblerWrapper(const TestSpaceType& space,
                                    const ApplyOn::WhichIntersection< GridViewType >* where,
                                    const LocalFaceVectorAssembler& localAssembler,
                                    VectorType& vector)
      : NeedsTmpVectorStorage(localAssembler.numTmpObjectsRequired(), space.mapper().maxNumDofs())
      , space_(space)
      , where_(where)
      , localVectorAssembler_(localAssembler)
      , vector_(vector)
    {}

    virtual ~LocalFaceVectorAssemblerWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const IntersectionType& intersection) const DS_FINAL
    {
      return where_->apply_on(gv, intersection);
    }

    virtual void apply_local(const IntersectionType& intersection) DS_FINAL
    {
      localVectorAssembler_.assembleLocal(space_, intersection, vector_, this->vectors(), this->indices());
    }

  private:
    const TestSpaceType& space_;
    std::unique_ptr< const ApplyOn::WhichIntersection< GridViewType > > where_;
    const LocalFaceVectorAssembler& localVectorAssembler_;
    VectorType& vector_;
  }; // class LocalFaceVectorAssemblerWrapper

public:
  template< class ConstraintsType, class M >
  void add(ConstraintsType& constraints,
           Dune::Stuff::LA::MatrixInterface< M >& matrix,
           const ApplyOn::WhichEntity< GridViewType >* where = new ApplyOn::AllEntities< GridViewType >())
  {
    typedef typename M::derived_type MatrixType;
    MatrixType& matrix_imp = static_cast< MatrixType& >(matrix);
    assert(matrix_imp.rows() == test_space_.mapper().size());
    assert(matrix_imp.cols() == ansatz_space_.mapper().size());
    typedef LocalMatrixConstraintsWrapper< ConstraintsType, MatrixType > WrapperType;
    this->codim0_functors_.emplace_back(new WrapperType(test_space_, ansatz_space_, where, constraints, matrix_imp));
  }  // ... add(...)

  template< class ConstraintsType, class V >
  void add(ConstraintsType& constraints,
           Dune::Stuff::LA::VectorInterface< V >& vector,
           const ApplyOn::WhichEntity< GridViewType >* where = new ApplyOn::AllEntities< GridViewType >())
  {
    typedef typename V::derived_type VectorType;
    VectorType& vector_imp = static_cast< VectorType& >(vector);
    assert(vector_imp.size() == test_space_.mapper().size());
    typedef LocalVectorConstraintsWrapper< ConstraintsType, VectorType > WrapperType;
    this->codim0_functors_.emplace_back(new WrapperType(test_space_, where, constraints, vector_imp));
  }  // ... add(...)

  template< class L, class M >
  void add(const LocalAssembler::Codim0Matrix< L >& local_assembler,
           Dune::Stuff::LA::MatrixInterface< M >& matrix,
           const ApplyOn::WhichEntity< GridViewType >* where = new ApplyOn::AllEntities< GridViewType >())
  {
    typedef typename M::derived_type MatrixType;
    MatrixType& matrix_imp = static_cast< MatrixType& >(matrix);
    assert(matrix_imp.rows() == test_space_.mapper().size());
    assert(matrix_imp.cols() == ansatz_space_.mapper().size());
    typedef LocalVolumeMatrixAssemblerWrapper< LocalAssembler::Codim0Matrix< L >, MatrixType > WrapperType;
    this->codim0_functors_.emplace_back(new WrapperType(test_space_, ansatz_space_, where, local_assembler, matrix_imp));
  }  // ... add(...)

  template< class L, class V >
  void add(const LocalAssembler::Codim0Vector< L >& local_assembler,
           Dune::Stuff::LA::VectorInterface< V >& vector,
           const ApplyOn::WhichEntity< GridViewType >* where = new ApplyOn::AllEntities< GridViewType >())
  {
    typedef typename V::derived_type VectorType;
    VectorType& vector_imp = static_cast< VectorType& >(vector);
    assert(vector_imp.size() == test_space_.mapper().size());
    typedef LocalVolumeVectorAssemblerWrapper< LocalAssembler::Codim0Vector< L >, VectorType > WrapperType;
    this->codim0_functors_.emplace_back(new WrapperType(test_space_, where, local_assembler, vector_imp));
  }  // ... add(...)

  template< class L, class V >
  void add(const LocalAssembler::Codim1Vector< L >& local_assembler,
           Dune::Stuff::LA::VectorInterface< V >& vector,
           const ApplyOn::WhichIntersection< GridViewType >* where = new ApplyOn::AllIntersections< GridViewType >())
  {
    typedef typename V::derived_type VectorType;
    VectorType& vector_imp = static_cast< VectorType& >(vector);
    assert(vector_imp.size() == test_space_.mapper().size());
    typedef LocalFaceVectorAssemblerWrapper< LocalAssembler::Codim1Vector< L >, VectorType > WrapperType;
    this->codim1_functors_.emplace_back(new WrapperType(test_space_, where, local_assembler, vector_imp));
  }  // ... add(...)

  void assemble()
  {
    this->walk();
  }

private:
  const TestSpaceType& test_space_;
  const AnsatzSpaceType& ansatz_space_;
}; // class SystemAssembler

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_ASSEMBLER_SYSTEM_HH
<commit_msg>[assembler.system] update * moved moved TmpStorageProvider to tmp-storage.hh * add override and final keywords * assemble() now takes an optional clear_stack, just as walk()<commit_after>// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Albrecht
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_ASSEMBLER_SYSTEM_HH
#define DUNE_GDT_ASSEMBLER_SYSTEM_HH

#include <type_traits>
#include <memory>

#include <dune/gdt/space/interface.hh>
#include <dune/gdt/space/constraints.hh>

#include "local/codim0.hh"
#include "local/codim1.hh"
#include "gridwalker.hh"
#include "tmp-storage.hh"

namespace Dune {
namespace GDT {

template< class TestSpaceImp,
          class GridViewImp = typename TestSpaceImp::GridViewType,
          class AnsatzSpaceImp = TestSpaceImp >
class SystemAssembler
  : public GridWalker< GridViewImp >
{
  static_assert(std::is_base_of< SpaceInterface< typename TestSpaceImp::Traits >, TestSpaceImp >::value,
                "TestSpaceImp has to be derived from SpaceInterface!");
  static_assert(std::is_base_of< SpaceInterface< typename AnsatzSpaceImp::Traits >, AnsatzSpaceImp >::value,
                "AnsatzSpaceImp has to be derived from SpaceInterface!");
  typedef GridWalker< GridViewImp > BaseType;
public:
  typedef TestSpaceImp    TestSpaceType;
  typedef AnsatzSpaceImp  AnsatzSpaceType;

  typedef typename BaseType::GridViewType     GridViewType;
  typedef typename BaseType::EntityType       EntityType;
  typedef typename BaseType::IntersectionType IntersectionType;
  typedef typename BaseType::BoundaryInfoType BoundaryInfoType;

private:
  typedef typename TestSpaceType::RangeFieldType RangeFieldType;

public:
  SystemAssembler(const TestSpaceType& test, const AnsatzSpaceType& ansatz, const GridViewType& grid_view)
    : BaseType(grid_view)
    , test_space_(test)
    , ansatz_space_(ansatz)
  {}

  SystemAssembler(const TestSpaceType& test, const AnsatzSpaceType& ansatz)
    : BaseType(*(test.grid_view()))
    , test_space_(test)
    , ansatz_space_(ansatz)
  {}

  SystemAssembler(const TestSpaceType& test)
    : BaseType(*(test.grid_view()))
    , test_space_(test)
    , ansatz_space_(test_space_)
  {}

  SystemAssembler(const TestSpaceType& test, const GridViewType& grid_view)
    : BaseType(grid_view)
    , test_space_(test)
    , ansatz_space_(test_space_)
  {}

  const TestSpaceType& test_space() const
  {
    return test_space_;
  }

  const AnsatzSpaceType& ansatz_space() const
  {
    return ansatz_space_;
  }

  using BaseType::add;

private:
  template< class ConstraintsType, class MatrixType >
  class LocalMatrixConstraintsWrapper
    : public BaseType::Codim0Object
  {
  public:
    LocalMatrixConstraintsWrapper(const TestSpaceType& t_space,
                                  const AnsatzSpaceType& a_space,
                                  const ApplyOn::WhichEntity< GridViewType >* where,
                                  ConstraintsType& constraints,
                                  MatrixType& matrix)
      : t_space_(t_space)
      , a_space_(a_space)
      , where_(where)
      , constraints_(constraints)
      , matrix_(matrix)
    {}

    virtual ~LocalMatrixConstraintsWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const EntityType& entity) const DS_OVERRIDE DS_FINAL
    {
      return where_->apply_on(gv, entity);
    }

    virtual void apply_local(const EntityType& entity) DS_OVERRIDE DS_FINAL
    {
      t_space_.local_constraints(a_space_, entity, constraints_);
      for (size_t ii = 0; ii < constraints_.rows(); ++ii) {
        const size_t row = constraints_.globalRow(ii);
        for (size_t jj = 0; jj < constraints_.cols(); ++jj) {
          matrix_.set_entry(row, constraints_.globalCol(jj), constraints_.value(ii, jj));
        }
      }
    } // ... apply_local(...)

  private:
    const TestSpaceType& t_space_;
    const AnsatzSpaceType& a_space_;
    std::unique_ptr< const ApplyOn::WhichEntity< GridViewType > > where_;
    ConstraintsType& constraints_;
    MatrixType& matrix_;
  }; // class LocalMatrixConstraintsWrapper

  template< class ConstraintsType, class VectorType >
  class LocalVectorConstraintsWrapper
      : public BaseType::Codim0Object
  {
  public:
    LocalVectorConstraintsWrapper(const TestSpaceType& t_space,
                                  const ApplyOn::WhichEntity< GridViewType >* where,
                                  ConstraintsType& constraints,
                                  VectorType& vector)
      : t_space_(t_space)
      , where_(where)
      , constraints_(constraints)
      , vector_(vector)
    {}

    virtual ~LocalVectorConstraintsWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const EntityType& entity) const DS_OVERRIDE DS_FINAL
    {
      return where_->apply_on(gv, entity);
    }

    virtual void apply_local(const EntityType& entity) DS_OVERRIDE DS_FINAL
    {
      t_space_.local_constraints(entity, constraints_);
      for (size_t ii = 0; ii < constraints_.rows(); ++ii) {
        vector_.set_entry(constraints_.globalRow(ii), RangeFieldType(0));
      }
    }

  private:
    const TestSpaceType& t_space_;
    std::unique_ptr< const ApplyOn::WhichEntity< GridViewType > > where_;
    ConstraintsType& constraints_;
    VectorType& vector_;
  }; // class LocalVectorConstraintsWrapper

  template< class LocalVolumeMatrixAssembler, class MatrixType >
  class LocalVolumeMatrixAssemblerWrapper
    : public BaseType::Codim0Object
    , TmpStorageProvider::Matrices< RangeFieldType >
  {
    typedef TmpStorageProvider::Matrices< RangeFieldType > TmpMatricesProvider;
  public:
    LocalVolumeMatrixAssemblerWrapper(const TestSpaceType& t_space,
                                      const AnsatzSpaceType& a_space,
                                      const ApplyOn::WhichEntity< GridViewType >* where,
                                      const LocalVolumeMatrixAssembler& localAssembler,
                                      MatrixType& matrix)
      : TmpMatricesProvider(localAssembler.numTmpObjectsRequired(),
                            t_space.mapper().maxNumDofs(),
                            a_space.mapper().maxNumDofs())
      , t_space_(t_space)
      , a_space_(a_space)
      , where_(where)
      , localMatrixAssembler_(localAssembler)
      , matrix_(matrix)
    {}

    virtual ~LocalVolumeMatrixAssemblerWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const EntityType& entity) const DS_OVERRIDE DS_FINAL
    {
      return where_->apply_on(gv, entity);
    }

    virtual void apply_local(const EntityType& entity) DS_OVERRIDE DS_FINAL
    {
      localMatrixAssembler_.assembleLocal(t_space_, a_space_, entity, matrix_, this->matrices(), this->indices());
    }

  private:
    const TestSpaceType& t_space_;
    const AnsatzSpaceType& a_space_;
    std::unique_ptr< const ApplyOn::WhichEntity< GridViewType > > where_;
    const LocalVolumeMatrixAssembler& localMatrixAssembler_;
    MatrixType& matrix_;
  }; // class LocalVolumeMatrixAssemblerWrapper

  template< class LocalVolumeVectorAssembler, class VectorType >
  class LocalVolumeVectorAssemblerWrapper
    : public BaseType::Codim0Object
    , TmpStorageProvider::Vectors< RangeFieldType >
  {
    typedef TmpStorageProvider::Vectors< RangeFieldType > TmpVectorsProvider;
  public:
    LocalVolumeVectorAssemblerWrapper(const TestSpaceType& space,
                                      const ApplyOn::WhichEntity< GridViewType >* where,
                                      const LocalVolumeVectorAssembler& localAssembler,
                                      VectorType& vector)
      : TmpVectorsProvider(localAssembler.numTmpObjectsRequired(), space.mapper().maxNumDofs())
      , space_(space)
      , where_(where)
      , localVectorAssembler_(localAssembler)
      , vector_(vector)
    {}

    virtual ~LocalVolumeVectorAssemblerWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const EntityType& entity) const DS_OVERRIDE DS_FINAL
    {
      return where_->apply_on(gv, entity);
    }

    virtual void apply_local(const EntityType& entity) DS_OVERRIDE DS_FINAL
    {
      localVectorAssembler_.assembleLocal(space_, entity, vector_, this->vectors(), this->indices());
    }

  private:
    const TestSpaceType& space_;
    std::unique_ptr< const ApplyOn::WhichEntity< GridViewType > > where_;
    const LocalVolumeVectorAssembler& localVectorAssembler_;
    VectorType& vector_;
  }; // class LocalVolumeVectorAssemblerWrapper

  template< class LocalFaceVectorAssembler, class VectorType >
  class LocalFaceVectorAssemblerWrapper
    : public BaseType::Codim1Object
    , TmpStorageProvider::Vectors< RangeFieldType >
  {
    typedef TmpStorageProvider::Vectors< RangeFieldType > TmpVectorsProvider;
  public:
    LocalFaceVectorAssemblerWrapper(const TestSpaceType& space,
                                    const ApplyOn::WhichIntersection< GridViewType >* where,
                                    const LocalFaceVectorAssembler& localAssembler,
                                    VectorType& vector)
      : TmpVectorsProvider(localAssembler.numTmpObjectsRequired(), space.mapper().maxNumDofs())
      , space_(space)
      , where_(where)
      , localVectorAssembler_(localAssembler)
      , vector_(vector)
    {}

    virtual ~LocalFaceVectorAssemblerWrapper() {}

    virtual bool apply_on(const GridViewType& gv, const IntersectionType& intersection) const DS_OVERRIDE DS_FINAL
    {
      return where_->apply_on(gv, intersection);
    }

    virtual void apply_local(const IntersectionType& intersection) DS_OVERRIDE DS_FINAL
    {
      localVectorAssembler_.assembleLocal(space_, intersection, vector_, this->vectors(), this->indices());
    }

  private:
    const TestSpaceType& space_;
    std::unique_ptr< const ApplyOn::WhichIntersection< GridViewType > > where_;
    const LocalFaceVectorAssembler& localVectorAssembler_;
    VectorType& vector_;
  }; // class LocalFaceVectorAssemblerWrapper

public:
  template< class ConstraintsType, class M >
  void add(ConstraintsType& constraints,
           Dune::Stuff::LA::MatrixInterface< M >& matrix,
           const ApplyOn::WhichEntity< GridViewType >* where = new ApplyOn::AllEntities< GridViewType >())
  {
    typedef typename M::derived_type MatrixType;
    MatrixType& matrix_imp = static_cast< MatrixType& >(matrix);
    assert(matrix_imp.rows() == test_space_.mapper().size());
    assert(matrix_imp.cols() == ansatz_space_.mapper().size());
    typedef LocalMatrixConstraintsWrapper< ConstraintsType, MatrixType > WrapperType;
    this->codim0_functors_.emplace_back(new WrapperType(test_space_, ansatz_space_, where, constraints, matrix_imp));
  }  // ... add(...)

  template< class ConstraintsType, class V >
  void add(ConstraintsType& constraints,
           Dune::Stuff::LA::VectorInterface< V >& vector,
           const ApplyOn::WhichEntity< GridViewType >* where = new ApplyOn::AllEntities< GridViewType >())
  {
    typedef typename V::derived_type VectorType;
    VectorType& vector_imp = static_cast< VectorType& >(vector);
    assert(vector_imp.size() == test_space_.mapper().size());
    typedef LocalVectorConstraintsWrapper< ConstraintsType, VectorType > WrapperType;
    this->codim0_functors_.emplace_back(new WrapperType(test_space_, where, constraints, vector_imp));
  }  // ... add(...)

  template< class L, class M >
  void add(const LocalAssembler::Codim0Matrix< L >& local_assembler,
           Dune::Stuff::LA::MatrixInterface< M >& matrix,
           const ApplyOn::WhichEntity< GridViewType >* where = new ApplyOn::AllEntities< GridViewType >())
  {
    typedef typename M::derived_type MatrixType;
    MatrixType& matrix_imp = static_cast< MatrixType& >(matrix);
    assert(matrix_imp.rows() == test_space_.mapper().size());
    assert(matrix_imp.cols() == ansatz_space_.mapper().size());
    typedef LocalVolumeMatrixAssemblerWrapper< LocalAssembler::Codim0Matrix< L >, MatrixType > WrapperType;
    this->codim0_functors_.emplace_back(new WrapperType(test_space_, ansatz_space_, where, local_assembler, matrix_imp));
  }  // ... add(...)

  template< class L, class V >
  void add(const LocalAssembler::Codim0Vector< L >& local_assembler,
           Dune::Stuff::LA::VectorInterface< V >& vector,
           const ApplyOn::WhichEntity< GridViewType >* where = new ApplyOn::AllEntities< GridViewType >())
  {
    typedef typename V::derived_type VectorType;
    VectorType& vector_imp = static_cast< VectorType& >(vector);
    assert(vector_imp.size() == test_space_.mapper().size());
    typedef LocalVolumeVectorAssemblerWrapper< LocalAssembler::Codim0Vector< L >, VectorType > WrapperType;
    this->codim0_functors_.emplace_back(new WrapperType(test_space_, where, local_assembler, vector_imp));
  }  // ... add(...)

  template< class L, class V >
  void add(const LocalAssembler::Codim1Vector< L >& local_assembler,
           Dune::Stuff::LA::VectorInterface< V >& vector,
           const ApplyOn::WhichIntersection< GridViewType >* where = new ApplyOn::AllIntersections< GridViewType >())
  {
    typedef typename V::derived_type VectorType;
    VectorType& vector_imp = static_cast< VectorType& >(vector);
    assert(vector_imp.size() == test_space_.mapper().size());
    typedef LocalFaceVectorAssemblerWrapper< LocalAssembler::Codim1Vector< L >, VectorType > WrapperType;
    this->codim1_functors_.emplace_back(new WrapperType(test_space_, where, local_assembler, vector_imp));
  }  // ... add(...)

  void assemble(const bool clear_stack = true)
  {
    this->walk(clear_stack);
  }

private:
  const TestSpaceType& test_space_;
  const AnsatzSpaceType& ansatz_space_;
}; // class SystemAssembler

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_ASSEMBLER_SYSTEM_HH
<|endoftext|>


<commit_before>#include "pb_sls.h"
#include "smt_literal.h"
#include "ast_pp.h"
#include "uint_set.h"

namespace smt {
    struct pb_sls::imp {

        struct clause {
            literal_vector    m_lits;
            scoped_mpz_vector m_weights;
            scoped_mpz        m_k;
            scoped_mpz        m_value;
            bool              m_eq;
            clause(unsynch_mpz_manager& m):
                m_weights(m),
                m_k(m),
                m_value(m),
                m_eq(true)
            {}
            clause(clause const& cls):
                m_lits(cls.m_lits),
                m_weights(cls.m_weights.m()),
                m_k(cls.m_k),
                m_value(cls.m_value),
                m_eq(cls.m_eq) {
                for (unsigned i = 0; i < cls.m_weights.size(); ++i) {
                    m_weights.push_back(cls.m_weights[i]);
                }
            }
        };

        struct stats {
            stats() { reset(); }
            void reset() { memset(this, 0, sizeof(*this)); }
        };

        ast_manager&     m;
        pb_util          pb;
        unsynch_mpz_manager mgr;
        volatile bool    m_cancel;
        vector<clause>   m_clauses;      // clauses to be satisfied
        vector<clause>   m_soft;         // soft constraints
        vector<rational> m_weights;      // weights of soft constraints
        rational         m_penalty;      // current penalty of soft constraints
        vector<unsigned_vector>  m_hard_occ, m_soft_occ;  // variable occurrence
        svector<bool>    m_assignment;   // current assignment.
        obj_map<expr, unsigned> m_expr2var; // map expressions to Boolean variables.
        ptr_vector<expr> m_var2expr;        // reverse map
        uint_set         m_hard_false;      // list of hard clauses that are false.
        uint_set         m_soft_false;      // list of soft clauses that are false.
        unsigned         m_max_flips;
        imp(ast_manager& m):
            m(m),
            pb(m),
            m_cancel(false)
        {
            m_max_flips = 100;
        }

        ~imp() {
        }

        unsigned max_flips() {
            return m_max_flips;
        }

        void add(expr* f) {
            clause cls(mgr);
            if (compile_clause(f, cls)) {
                m_clauses.push_back(cls);
            }
        }
        void add(expr* f, rational const& w) {
            clause cls(mgr);
            if (compile_clause(f, cls)) {
                m_clauses.push_back(cls);
            }
        }

        void init_value(expr* f, bool b) {
            literal lit = mk_literal(f);
            SASSERT(!lit.sign());
            m_assignment[lit.var()] = b;
        }

        lbool operator()() {
            init();
            for (unsigned i = 0; i < max_flips(); ++i) {
                flip();
                if (m_cancel) {
                    return l_undef;
                }
            }
            return l_undef;
        }

        bool get_value(expr* f) {
            unsigned var;
            if (m_expr2var.find(f, var)) {
                return m_assignment[var];
            }
            UNREACHABLE();
            return true;
        }
        bool get_value(literal l) {
            return l.sign() ^ m_assignment[l.var()];
        }
        void set_cancel(bool f) {
            m_cancel = f;
        }
        void collect_statistics(statistics& st) const {
        }
        void get_model(model_ref& mdl) {
        }
        void updt_params(params_ref& p) {
        }

        bool eval(clause& cls) {
            unsigned sz = cls.m_lits.size();
            cls.m_value.reset();
            for (unsigned i = 0; i < sz; ++i) {
                if (get_value(cls.m_lits[i])) {
                    cls.m_value += cls.m_weights[i];
                }
            }
            if (cls.m_eq) {
                return cls.m_value == cls.m_k;
            }
            else {
                return cls.m_value >= cls.m_k;
            }
        }

        void init_occ(vector<clause> const& clauses, vector<unsigned_vector> & occ) {
            for (unsigned i = 0; i < clauses.size(); ++i) {
                clause const& cls = clauses[i];
                for (unsigned j = 0; j < cls.m_lits.size(); ++j) {
                    literal lit = cls.m_lits[j];
                    occ[lit.var()].push_back(i);
                }
            }
        }

        void init() {
            // initialize the occurs vectors.
            init_occ(m_clauses, m_hard_occ);
            init_occ(m_soft, m_soft_occ);
            // add clauses that are false.
            for (unsigned i = 0; i < m_clauses.size(); ++i) {
                if (!eval(m_clauses[i])) {
                    m_hard_false.insert(i);
                }
            }
            m_penalty.reset();
            for (unsigned i = 0; i < m_soft.size(); ++i) {
                if (!eval(m_soft[i])) {
                    m_soft_false.insert(i);
                    m_penalty += m_weights[i];
                }
            }
        }

        void flip() {
            if (m_hard_false.empty()) {
                flip_soft();
            }
            else {
                flip_hard();
            }
        }

        void flip_hard() {
            SASSERT(!m_hard_false.empty());
            clause const& cls = pick_hard_clause();
            int break_count;
            int min_bc = INT_MAX;
            unsigned min_bc_index = 0;
            for (unsigned i = 0; i < cls.m_lits.size(); ++i) {
                literal lit = cls.m_lits[i];
                break_count = flip(lit);
                if (break_count <= 0) {
                    return;
                }
                if (break_count < min_bc) {
                    min_bc = break_count;
                    min_bc_index = i;
                }
                VERIFY(-break_count == flip(~lit));
            }
            // just do a greedy move:
            flip(cls.m_lits[min_bc_index]);
        }

        void flip_soft() {
            NOT_IMPLEMENTED_YET();
        }

        // crude selection strategy.
        clause const& pick_hard_clause() {
            SASSERT(!m_hard_false.empty());
            uint_set::iterator it = m_hard_false.begin();
            uint_set::iterator end = m_hard_false.end();
            SASSERT(it != end);
            return m_clauses[*it];
        }

        clause const& pick_soft_clause() {
            SASSERT(!m_soft_false.empty());
            uint_set::iterator it = m_soft_false.begin();
            uint_set::iterator end = m_soft_false.end();
            SASSERT(it != end);
            unsigned index = *it;
            rational penalty = m_weights[index];
            ++it;
            for (; it != end; ++it) {
                if (m_weights[*it] > penalty) {
                    index = *it;
                    penalty = m_weights[*it];
                }
            }
            return m_soft[index];
        }

        int flip(literal l) {
            SASSERT(get_value(l));
            m_assignment[l.var()] = !m_assignment[l.var()];
            int break_count = 0;
            {
                unsigned_vector const& occ = m_hard_occ[l.var()];
                for (unsigned i = 0; i < occ.size(); ++i) {
                    unsigned j = occ[i];
                    if (eval(m_clauses[j])) {
                        if (m_hard_false.contains(j)) {
                            break_count--;
                            m_hard_false.remove(j);
                        }
                    }
                    else {
                        if (!m_hard_false.contains(j)) {
                            break_count++;
                            m_hard_false.insert(j);
                        }
                    }
                }
            }
            {
                unsigned_vector const& occ = m_soft_occ[l.var()];
                for (unsigned i = 0; i < occ.size(); ++i) {
                    unsigned j = occ[i];
                    if (eval(m_soft[j])) {
                        if (m_soft_false.contains(j)) {
                            m_penalty -= m_weights[j];
                            m_soft_false.remove(j);
                        }
                    }
                    else {
                        if (!m_soft_false.contains(j)) {
                            m_penalty += m_weights[j];
                            m_soft_false.insert(j);
                        }
                    }
                }
            }

            SASSERT(get_value(~l));
            return break_count;
        }

        literal mk_literal(expr* f) {
            literal result;
            bool sign = false;
            while (m.is_not(f, f)) {
                sign = !sign;
            }
            if (m.is_true(f)) {
                result = true_literal;
            }
            else if (m.is_false(f)) {
                result = false_literal;
            }
            else {
                unsigned var;
                if (!m_expr2var.find(f, var)) {
                    var = m_hard_occ.size();
                    SASSERT(m_expr2var.size() == var);
                    m_hard_occ.push_back(unsigned_vector());
                    m_soft_occ.push_back(unsigned_vector());
                    m_assignment.push_back(false);
                    m_expr2var.insert(f, var);
                    m_var2expr.push_back(f);
                }
                result = literal(var);
            }
            if (sign) {
                result.neg();
            }
            return result;
        }

        bool compile_clause(expr* _f, clause& cls) {
            if (!is_app(_f)) return false;
            app* f = to_app(_f);
            unsigned sz = f->get_num_args();
            expr* const* args = f->get_args();
            literal lit;
            rational coeff;
            if (pb.is_ge(f) || pb.is_eq(f)) {
                for (unsigned i = 0; i < sz; ++i) {
                    coeff = pb.get_coeff(f, i);
                    SASSERT(coeff.is_int());
                    lit = mk_literal(args[i]);
                    if (lit == null_literal) return false;
                    SASSERT(lit != false_literal && lit != true_literal);
                    cls.m_lits.push_back(lit);
                    cls.m_weights.push_back(coeff.to_mpq().numerator());
                    if (get_value(lit)) {
                        cls.m_value += coeff.to_mpq().numerator();
                    }
                }
                cls.m_eq = pb.is_eq(f);
                coeff = pb.get_k(f);
                SASSERT(coeff.is_int());
                cls.m_k = coeff.to_mpq().numerator();
            }
            else if (m.is_or(f)) {
                for (unsigned i = 0; i < sz; ++i) {
                    lit = mk_literal(args[i]);
                    if (lit == null_literal) return false;
                    SASSERT(lit != false_literal && lit != true_literal);
                    cls.m_lits.push_back(lit);
                    cls.m_weights.push_back(mpz(1));
                    if (get_value(lit)) {
                        cls.m_value += mpz(1);
                    }
                }
                cls.m_eq = false;
                cls.m_k = mpz(1);
            }
            else {
                lit = mk_literal(f);
                if (lit == null_literal) return false;
                SASSERT(lit != false_literal && lit != true_literal);
                cls.m_lits.push_back(lit);
                cls.m_weights.push_back(mpz(1));
                cls.m_eq = true;
                cls.m_k = mpz(1);
            }
            return true;
        }

    };

    pb_sls::pb_sls(ast_manager& m) {
        m_imp = alloc(imp, m);
    }
    pb_sls::~pb_sls() {
        dealloc(m_imp);
    }
    void pb_sls::add(expr* f) {
        m_imp->add(f);
    }
    void pb_sls::add(expr* f, rational const& w) {
        m_imp->add(f, w);
    }
    void pb_sls::init_value(expr* f, bool b) {
        m_imp->init_value(f, b);
    }
    lbool pb_sls::operator()() {
        return (*m_imp)();
    }
    bool pb_sls::get_value(expr* f) {
        return m_imp->get_value(f);
    }
    void pb_sls::set_cancel(bool f) {
        m_imp->set_cancel(f);
    }
    void pb_sls::collect_statistics(statistics& st) const {
        m_imp->collect_statistics(st);
    }
    void pb_sls::get_model(model_ref& mdl) {
        m_imp->get_model(mdl);
    }
    void pb_sls::updt_params(params_ref& p) {
        m_imp->updt_params(p);
    }

}
<commit_msg>working on pb sls<commit_after>/*++
Copyright (c) 2014 Microsoft Corporation

Module Name:

    pb_sls.cpp

Abstract:

    SLS for PB optimization.

Author:

    Nikolaj Bjorner (nbjorner) 2014-03-18

Notes:

--*/
#include "pb_sls.h"
#include "smt_literal.h"
#include "ast_pp.h"
#include "uint_set.h"

namespace smt {
    struct pb_sls::imp {

        struct clause {
            literal_vector    m_lits;
            scoped_mpz_vector m_weights;
            scoped_mpz        m_k;
            scoped_mpz        m_value;
            bool              m_eq;
            clause(unsynch_mpz_manager& m):
                m_weights(m),
                m_k(m),
                m_value(m),
                m_eq(true)
            {}
            clause(clause const& cls):
                m_lits(cls.m_lits),
                m_weights(cls.m_weights.m()),
                m_k(cls.m_k),
                m_value(cls.m_value),
                m_eq(cls.m_eq) {
                for (unsigned i = 0; i < cls.m_weights.size(); ++i) {
                    m_weights.push_back(cls.m_weights[i]);
                }
            }
        };

        struct stats {
            stats() { reset(); }
            void reset() { memset(this, 0, sizeof(*this)); }
        };

        ast_manager&     m;
        pb_util          pb;
        unsynch_mpz_manager mgr;
        volatile bool    m_cancel;
        vector<clause>   m_clauses;      // clauses to be satisfied
        vector<clause>   m_soft;         // soft constraints
        vector<rational> m_weights;      // weights of soft constraints
        rational         m_penalty;      // current penalty of soft constraints
        vector<unsigned_vector>  m_hard_occ, m_soft_occ;  // variable occurrence
        svector<bool>    m_assignment;   // current assignment.
        obj_map<expr, unsigned> m_expr2var; // map expressions to Boolean variables.
        ptr_vector<expr> m_var2expr;        // reverse map
        uint_set         m_hard_false;      // list of hard clauses that are false.
        uint_set         m_soft_false;      // list of soft clauses that are false.
        unsigned         m_max_flips;
        imp(ast_manager& m):
            m(m),
            pb(m),
            m_cancel(false)
        {
            m_max_flips = 100;
        }

        ~imp() {
        }

        unsigned max_flips() {
            return m_max_flips;
        }

        void add(expr* f) {
            clause cls(mgr);
            if (compile_clause(f, cls)) {
                m_clauses.push_back(cls);
            }
        }
        void add(expr* f, rational const& w) {
            clause cls(mgr);
            if (compile_clause(f, cls)) {
                m_clauses.push_back(cls);
                m_weights.push_back(w);
            }
        }

        void init_value(expr* f, bool b) {
            literal lit = mk_literal(f);
            SASSERT(!lit.sign());
            m_assignment[lit.var()] = b;
        }

        lbool operator()() {
            init();
            for (unsigned i = 0; i < max_flips(); ++i) {
                flip();
                if (m_cancel) {
                    return l_undef;
                }
            }
            return l_undef;
        }

        bool get_value(expr* f) {
            unsigned var;
            if (m_expr2var.find(f, var)) {
                return m_assignment[var];
            }
            UNREACHABLE();
            return true;
        }
        bool get_value(literal l) {
            return l.sign() ^ m_assignment[l.var()];
        }
        void set_cancel(bool f) {
            m_cancel = f;
        }
        void collect_statistics(statistics& st) const {
        }
        void get_model(model_ref& mdl) {
            NOT_IMPLEMENTED_YET();
        }
        void updt_params(params_ref& p) {
        }

        bool eval(clause& cls) {
            unsigned sz = cls.m_lits.size();
            cls.m_value.reset();
            for (unsigned i = 0; i < sz; ++i) {
                if (get_value(cls.m_lits[i])) {
                    cls.m_value += cls.m_weights[i];
                }
            }
            if (cls.m_eq) {
                return cls.m_value == cls.m_k;
            }
            else {
                return cls.m_value >= cls.m_k;
            }
        }

        void init_occ(vector<clause> const& clauses, vector<unsigned_vector> & occ) {
            for (unsigned i = 0; i < clauses.size(); ++i) {
                clause const& cls = clauses[i];
                for (unsigned j = 0; j < cls.m_lits.size(); ++j) {
                    literal lit = cls.m_lits[j];
                    occ[lit.var()].push_back(i);
                }
            }
        }

        void init() {
            // initialize the occurs vectors.
            init_occ(m_clauses, m_hard_occ);
            init_occ(m_soft, m_soft_occ);
            // add clauses that are false.
            for (unsigned i = 0; i < m_clauses.size(); ++i) {
                if (!eval(m_clauses[i])) {
                    m_hard_false.insert(i);
                }
            }
            m_penalty.reset();
            for (unsigned i = 0; i < m_soft.size(); ++i) {
                if (!eval(m_soft[i])) {
                    m_soft_false.insert(i);
                    m_penalty += m_weights[i];
                }
            }
        }

        void flip() {
            if (m_hard_false.empty()) {
                flip_soft();
            }
            else {
                flip_hard();
            }
        }

        void flip_hard() {
            SASSERT(!m_hard_false.empty());
            clause const& cls = pick_hard_clause();
            int break_count;
            int min_bc = INT_MAX;
            unsigned min_bc_index = 0;
            for (unsigned i = 0; i < cls.m_lits.size(); ++i) {
                literal lit = cls.m_lits[i];
                break_count = flip(lit);
                if (break_count <= 0) {
                    return;
                }
                if (break_count < min_bc) {
                    min_bc = break_count;
                    min_bc_index = i;
                }
                VERIFY(-break_count == flip(~lit));
            }
            // just do a greedy move:
            flip(cls.m_lits[min_bc_index]);
        }

        void flip_soft() {
            clause const& cls = pick_soft_clause();
            int break_count;
            int min_bc = INT_MAX;
            unsigned min_bc_index = 0;
            rational penalty = m_penalty;
            rational min_penalty = penalty;
            for (unsigned i = 0; i < cls.m_lits.size(); ++i) {
                literal lit = cls.m_lits[i];
                break_count = flip(lit);
                SASSERT(break_count >= 0);
                if (break_count == 0 && penalty > m_penalty) {
                    // TODO: save into best so far if this qualifies.
                    return;
                }
                if ((break_count < min_bc) ||
                    (break_count == min_bc && m_penalty < min_penalty)) {
                    min_bc = break_count;
                    min_bc_index = i;
                    min_penality = m_penalty;
                }
                VERIFY(-break_count == flip(~lit));
            }
            // just do a greedy move:
            flip(cls.m_lits[min_bc_index]);
        }

        //
        // TODO: alternate version: loop over soft clauses and see if there is a flip that
        // reduces the penalty while preserving the hard constraints.
        //

        // crude selection strategy.
        clause const& pick_hard_clause() {
            SASSERT(!m_hard_false.empty());
            uint_set::iterator it = m_hard_false.begin();
            uint_set::iterator end = m_hard_false.end();
            SASSERT(it != end);
            return m_clauses[*it];
        }

        clause const& pick_soft_clause() {
            SASSERT(!m_soft_false.empty());
            uint_set::iterator it = m_soft_false.begin();
            uint_set::iterator end = m_soft_false.end();
            SASSERT(it != end);
            unsigned index = *it;
            rational penalty = m_weights[index];
            ++it;
            for (; it != end; ++it) {
                if (m_weights[*it] > penalty) {
                    index = *it;
                    penalty = m_weights[*it];
                }
            }
            return m_soft[index];
        }

        int flip(literal l) {
            SASSERT(get_value(l));
            m_assignment[l.var()] = !m_assignment[l.var()];
            int break_count = 0;
            {
                unsigned_vector const& occ = m_hard_occ[l.var()];
                for (unsigned i = 0; i < occ.size(); ++i) {
                    unsigned j = occ[i];
                    if (eval(m_clauses[j])) {
                        if (m_hard_false.contains(j)) {
                            break_count--;
                            m_hard_false.remove(j);
                        }
                    }
                    else {
                        if (!m_hard_false.contains(j)) {
                            break_count++;
                            m_hard_false.insert(j);
                        }
                    }
                }
            }
            {
                unsigned_vector const& occ = m_soft_occ[l.var()];
                for (unsigned i = 0; i < occ.size(); ++i) {
                    unsigned j = occ[i];
                    if (eval(m_soft[j])) {
                        if (m_soft_false.contains(j)) {
                            m_penalty -= m_weights[j];
                            m_soft_false.remove(j);
                        }
                    }
                    else {
                        if (!m_soft_false.contains(j)) {
                            m_penalty += m_weights[j];
                            m_soft_false.insert(j);
                        }
                    }
                }
            }

            SASSERT(get_value(~l));
            return break_count;
        }

        literal mk_literal(expr* f) {
            literal result;
            bool sign = false;
            while (m.is_not(f, f)) {
                sign = !sign;
            }
            if (m.is_true(f)) {
                result = true_literal;
            }
            else if (m.is_false(f)) {
                result = false_literal;
            }
            else {
                unsigned var;
                if (!m_expr2var.find(f, var)) {
                    var = m_hard_occ.size();
                    SASSERT(m_expr2var.size() == var);
                    m_hard_occ.push_back(unsigned_vector());
                    m_soft_occ.push_back(unsigned_vector());
                    m_assignment.push_back(false);
                    m_expr2var.insert(f, var);
                    m_var2expr.push_back(f);
                }
                result = literal(var);
            }
            if (sign) {
                result.neg();
            }
            return result;
        }

        bool compile_clause(expr* _f, clause& cls) {
            if (!is_app(_f)) return false;
            app* f = to_app(_f);
            unsigned sz = f->get_num_args();
            expr* const* args = f->get_args();
            literal lit;
            rational coeff;
            if (pb.is_ge(f) || pb.is_eq(f)) {
                for (unsigned i = 0; i < sz; ++i) {
                    coeff = pb.get_coeff(f, i);
                    SASSERT(coeff.is_int());
                    lit = mk_literal(args[i]);
                    if (lit == null_literal) return false;
                    SASSERT(lit != false_literal && lit != true_literal);
                    cls.m_lits.push_back(lit);
                    cls.m_weights.push_back(coeff.to_mpq().numerator());
                    if (get_value(lit)) {
                        cls.m_value += coeff.to_mpq().numerator();
                    }
                }
                cls.m_eq = pb.is_eq(f);
                coeff = pb.get_k(f);
                SASSERT(coeff.is_int());
                cls.m_k = coeff.to_mpq().numerator();
            }
            else if (m.is_or(f)) {
                for (unsigned i = 0; i < sz; ++i) {
                    lit = mk_literal(args[i]);
                    if (lit == null_literal) return false;
                    SASSERT(lit != false_literal && lit != true_literal);
                    cls.m_lits.push_back(lit);
                    cls.m_weights.push_back(mpz(1));
                    if (get_value(lit)) {
                        cls.m_value += mpz(1);
                    }
                }
                cls.m_eq = false;
                cls.m_k = mpz(1);
            }
            else {
                lit = mk_literal(f);
                if (lit == null_literal) return false;
                SASSERT(lit != false_literal && lit != true_literal);
                cls.m_lits.push_back(lit);
                cls.m_weights.push_back(mpz(1));
                cls.m_eq = true;
                cls.m_k = mpz(1);
            }
            return true;
        }

    };

    pb_sls::pb_sls(ast_manager& m) {
        m_imp = alloc(imp, m);
    }
    pb_sls::~pb_sls() {
        dealloc(m_imp);
    }
    void pb_sls::add(expr* f) {
        m_imp->add(f);
    }
    void pb_sls::add(expr* f, rational const& w) {
        m_imp->add(f, w);
    }
    void pb_sls::init_value(expr* f, bool b) {
        m_imp->init_value(f, b);
    }
    lbool pb_sls::operator()() {
        return (*m_imp)();
    }
    bool pb_sls::get_value(expr* f) {
        return m_imp->get_value(f);
    }
    void pb_sls::set_cancel(bool f) {
        m_imp->set_cancel(f);
    }
    void pb_sls::collect_statistics(statistics& st) const {
        m_imp->collect_statistics(st);
    }
    void pb_sls::get_model(model_ref& mdl) {
        m_imp->get_model(mdl);
    }
    void pb_sls::updt_params(params_ref& p) {
        m_imp->updt_params(p);
    }

}
<|endoftext|>


<commit_before><commit_msg>cc: Workaround resourceless draw crash<commit_after><|endoftext|>


<commit_before>#include <iostream>
#include "sockets/SocketW.h"
#include <string>
#include <vector>
#include <cstdlib>
#include <cstdio>
#include "buffer.h"
#include "user.h"
#include "string.h"

#define BUFLEN 1000000

int get_empty( user ** list, int amount ) {
  for (int i = 0; i < amount; i++ ){
    if (!list[i]->is_connected){return i;}
  }
  return -1;
}

int main( int argc, char * argv[] ) {
  if (argc < 4) {
    std::cout << "usage: " << argv[0] << " buffers_count total_buffersize max_clients" << std::endl;
    return 1;
  }
  int buffers = atoi(argv[1]);
  int total_buffersize = atoi(argv[2]);
  int connections = atoi(argv[3]);
  int size_per_buffer = total_buffersize/buffers;
  std::cout << "Size per buffer: " << size_per_buffer << std::endl;
  buffer ** ringbuf = (buffer**) calloc (buffers,sizeof(buffer*));
  for (int i = 0; i < buffers; i ++ ) {
    ringbuf[i] = new buffer;
    ringbuf[i]->data = (char*) malloc(size_per_buffer);
  }
  std::cout << "Successfully allocated " << total_buffersize << " bytes total buffer." << std::endl;
  user ** connectionList = (user**) calloc (connections,sizeof(user*));
  for (int i = 0; i < connections; i++) { connectionList[i] = new user; }
  char header[13];//FLV header is always 13 bytes
  int ret = 0;
  int frame_bodylength = 0;
  int current_buffer = 0;
  int open_connection = -1;
  int lastproper = 0;//last properly finished buffer number
  unsigned int loopcount = 0;
  SWUnixSocket listener;
  SWBaseSocket * incoming = 0;
  SWBaseSocket::SWBaseError BError;
  //read FLV header - 13 bytes
  ret = fread(&header,1,13,stdin);
  //TODO: check ret?

  listener.bind("/tmp/socketfile");
  listener.listen();
  listener.set_timeout(0,50000);

  //TODO: not while true, but while running - set running to false when kill signal is received!
  while(true) {
    loopcount ++;
    //invalidate the current buffer
    ringbuf[current_buffer]->size = 0;
    ringbuf[current_buffer]->number = -1;
    //read FLV frame header - 11 bytes
    ret = fread(ringbuf[current_buffer]->data,1,11,stdin);
    //TODO: Check ret?
    //if video frame? (id 9) check for incoming connections
    if (ringbuf[current_buffer]->data[0] == 9) {
      incoming = listener.accept(&BError);
      if (incoming){
        open_connection = get_empty(connectionList,connections);
        if (open_connection != -1) {
          connectionList[open_connection]->connect(incoming);
          //send the FLV header
          std::cout << "Client " << open_connection << " connected." << std::endl;
          connectionList[open_connection]->MyBuffer = lastproper;
          connectionList[open_connection]->MyBuffer_num = ringbuf[lastproper]->number;
          //TODO: Do this more nicely?
          if (connectionList[open_connection]->Conn->send(&header[0],13,NULL) != 13){
            connectionList[open_connection]->disconnect();
            std::cout << "Client " << open_connection << " failed to receive the header!" << std::endl;
          }
          std::cout << "Client " << open_connection << " received header!" << std::endl;
        }else{
          std::cout << "New client not connected: no more connections!" << std::endl;
        }
      }
    }
    //calculate body length of frame
    frame_bodylength = 4;
    frame_bodylength += ringbuf[current_buffer]->data[3];
    frame_bodylength += (ringbuf[current_buffer]->data[2] << 8);
    frame_bodylength += (ringbuf[current_buffer]->data[1] << 16);
    //read the rest of the frame
    ret = fread(&ringbuf[current_buffer]->data[11],1,frame_bodylength,stdin);
    //TODO: Check ret?
    ringbuf[current_buffer]->size = frame_bodylength + 11;
    ringbuf[current_buffer]->number = loopcount;
    //send all connections what they need, if and when they need it
    for (int i = 0; i < connections; i++) {connectionList[i]->Send(ringbuf, buffers);}
    //keep track of buffers
    lastproper = current_buffer;
    current_buffer++;
    current_buffer %= buffers;
  }

  // disconnect listener
  listener.disconnect();
  //TODO: cleanup
  return 0;
}
<commit_msg>Loop support<commit_after>#include <iostream>
#include "sockets/SocketW.h"
#include <string>
#include <vector>
#include <cstdlib>
#include <cstdio>
#include "buffer.h"
#include "user.h"
#include "string.h"

#define BUFLEN 1000000

int get_empty( user ** list, int amount ) {
  for (int i = 0; i < amount; i++ ){
    if (!list[i]->is_connected){return i;}
  }
  return -1;
}

int main( int argc, char * argv[] ) {
  if (argc < 4) {
    std::cout << "usage: " << argv[0] << " buffers_count total_buffersize max_clients" << std::endl;
    return 1;
  }
  int buffers = atoi(argv[1]);
  int total_buffersize = atoi(argv[2]);
  int connections = atoi(argv[3]);
  int size_per_buffer = total_buffersize/buffers;
  std::cout << "Size per buffer: " << size_per_buffer << std::endl;
  buffer ** ringbuf = (buffer**) calloc (buffers,sizeof(buffer*));
  for (int i = 0; i < buffers; i ++ ) {
    ringbuf[i] = new buffer;
    ringbuf[i]->data = (char*) malloc(size_per_buffer);
  }
  std::cout << "Successfully allocated " << total_buffersize << " bytes total buffer." << std::endl;
  user ** connectionList = (user**) calloc (connections,sizeof(user*));
  for (int i = 0; i < connections; i++) { connectionList[i] = new user; }
  char header[13];//FLV header is always 13 bytes
  int ret = 0;
  int frame_bodylength = 0;
  int current_buffer = 0;
  int open_connection = -1;
  int lastproper = 0;//last properly finished buffer number
  unsigned int loopcount = 0;
  SWUnixSocket listener;
  SWBaseSocket * incoming = 0;
  SWBaseSocket::SWBaseError BError;
  //read FLV header - 13 bytes
  ret = fread(&header,1,13,stdin);
  //TODO: check ret?

  listener.bind("/tmp/socketfile");
  listener.listen();
  listener.set_timeout(0,50000);

  //TODO: not while true, but while running - set running to false when kill signal is received!
  while(true) {
    loopcount ++;
    //invalidate the current buffer
    ringbuf[current_buffer]->size = 0;
    ringbuf[current_buffer]->number = -1;
    if (std::cin.peek() == 'F') {
      //new FLV file, read the file head again.
      ret = fread(&header,1,13,stdin);
    } else {
      //read FLV frame header - 11 bytes
      ret = fread(ringbuf[current_buffer]->data,1,11,stdin);
      //TODO: Check ret?
      //if video frame? (id 9) check for incoming connections
      if (ringbuf[current_buffer]->data[0] == 9) {
        incoming = listener.accept(&BError);
        if (incoming){
          open_connection = get_empty(connectionList,connections);
          if (open_connection != -1) {
            connectionList[open_connection]->connect(incoming);
            //send the FLV header
            std::cout << "Client " << open_connection << " connected." << std::endl;
            connectionList[open_connection]->MyBuffer = lastproper;
            connectionList[open_connection]->MyBuffer_num = ringbuf[lastproper]->number;
            //TODO: Do this more nicely?
            if (connectionList[open_connection]->Conn->send(&header[0],13,NULL) != 13){
              connectionList[open_connection]->disconnect();
              std::cout << "Client " << open_connection << " failed to receive the header!" << std::endl;
            }
            std::cout << "Client " << open_connection << " received header!" << std::endl;
          }else{
            std::cout << "New client not connected: no more connections!" << std::endl;
          }
        }
      }
      //calculate body length of frame
      frame_bodylength = 4;
      frame_bodylength += ringbuf[current_buffer]->data[3];
      frame_bodylength += (ringbuf[current_buffer]->data[2] << 8);
      frame_bodylength += (ringbuf[current_buffer]->data[1] << 16);
      //read the rest of the frame
      ret = fread(&ringbuf[current_buffer]->data[11],1,frame_bodylength,stdin);
      //TODO: Check ret?
      ringbuf[current_buffer]->size = frame_bodylength + 11;
      ringbuf[current_buffer]->number = loopcount;
      //send all connections what they need, if and when they need it
      for (int i = 0; i < connections; i++) {connectionList[i]->Send(ringbuf, buffers);}
      //keep track of buffers
      lastproper = current_buffer;
      current_buffer++;
      current_buffer %= buffers;
    }
  }

  // disconnect listener
  listener.disconnect();
  //TODO: cleanup
  return 0;
}
<|endoftext|>


<commit_before>/*
 * This file is part of the Camera Streaming Daemon
 *
 * Copyright (C) 2017  Intel Corporation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <assert.h>
#include <fcntl.h>
#include <gst/app/gstappsrc.h>
#include <librealsense/rs.h>
#include <sstream>
#include <sys/ioctl.h>
#include <unistd.h>

#include "log.h"
#include "samples/stream_realsense.h"

#define WIDTH (640)
#define HEIGHT (480)
#define SIZE (WIDTH * HEIGHT * 3)
#define ONE_METER (999)

struct rgb {
    uint8_t r;
    uint8_t g;
    uint8_t b;
};

struct Context {
    rs_device *dev;
    rs_context *rs_ctx;
    struct rgb rgb_data[];
};

static void rainbow_scale(double value, uint8_t rgb[])
{
    rgb[0] = rgb[1] = rgb[2] = 0;

    if (value <= 0.0)
        return;

    if (value < 0.25) { // RED to YELLOW
        rgb[0] = 255;
        rgb[1] = (uint8_t)255 * (value / 0.25);
    } else if (value < 0.5) { // YELLOW to GREEN
        rgb[0] = (uint8_t)255 * (1 - ((value - 0.25) / 0.25));
        rgb[1] = 255;
    } else if (value < 0.75) { // GREEN to CYAN
        rgb[1] = 255;
        rgb[2] = (uint8_t)255 * (value - 0.5 / 0.25);
    } else if (value < 1.0) { // CYAN to BLUE
        rgb[1] = (uint8_t)255 * (1 - ((value - 0.75) / 0.25));
        rgb[2] = 255;
    } else { // BLUE
        rgb[2] = 255;
    }
}

static void cb_need_data(GstAppSrc *appsrc, guint unused_size, gpointer user_data)
{
    GstFlowReturn ret;
    Context *ctx = (Context *)user_data;

    rs_wait_for_frames(ctx->dev, NULL);
    uint16_t *depth = (uint16_t *)rs_get_frame_data(ctx->dev, RS_STREAM_DEPTH, NULL);
    if (!depth) {
        log_error("No depth data. Not building frame");
        return;
    }

    GstBuffer *buffer
        = gst_buffer_new_wrapped_full((GstMemoryFlags)0, ctx->rgb_data, SIZE, 0, SIZE, NULL, NULL);
    assert(buffer);
    for (int i = 0, end = WIDTH * HEIGHT; i < end; ++i) {
        uint8_t rainbow[3];
        rainbow_scale((double)depth[i] * 0.001, rainbow);

        ctx->rgb_data[i].r = rainbow[0];
        ctx->rgb_data[i].g = rainbow[1];
        ctx->rgb_data[i].b = rainbow[2];
    }

    g_signal_emit_by_name(appsrc, "push-buffer", buffer, &ret);
}

static void cb_enough_data(GstAppSrc *src, gpointer user_data)
{
}

static gboolean cb_seek_data(GstAppSrc *src, guint64 offset, gpointer user_data)
{
    return TRUE;
}

StreamRealSense::StreamRealSense()
    : Stream()
{
}

const std::string StreamRealSense::get_path() const
{
    return "/RealSense";
}

const std::string StreamRealSense::get_name() const
{
    return "RealSense Sample Stream";
}

const std::vector<Stream::PixelFormat> &StreamRealSense::get_formats() const
{
    static std::vector<Stream::PixelFormat> formats;
    return formats;
}

GstElement *
StreamRealSense::create_gstreamer_pipeline(std::map<std::string, std::string> &params) const
{
    Context *ctx = (Context *)malloc(sizeof(Context) + SIZE);
    assert(ctx);

    /* librealsense */
    rs_error *e = 0;
    ctx->rs_ctx = rs_create_context(RS_API_VERSION, &e);
    if (e) {
        log_error("rs_error was raised when calling %s(%s): %s", rs_get_failed_function(e),
                  rs_get_failed_args(e), rs_get_error_message(e));
        log_error("Current librealsense api version %d", rs_get_api_version(NULL));
        log_error("Compiled for librealsense api version %d", RS_API_VERSION);
        return nullptr;
    }
    ctx->dev = rs_get_device(ctx->rs_ctx, 0, NULL);
    if (!ctx->dev) {
        log_error("Unable to access realsense device");
        return nullptr;
    }

    /* Configure all streams to run at VGA resolution at 60 frames per second */
    rs_enable_stream(ctx->dev, RS_STREAM_DEPTH, WIDTH, HEIGHT, RS_FORMAT_Z16, 60, NULL);
    rs_start_device(ctx->dev, NULL);

    /* gstreamer */
    GError *error = nullptr;
    GstElement *pipeline;

    pipeline = gst_parse_launch("appsrc name=mysource ! videoconvert ! "
                                "video/x-raw,width=640,height=480,format=NV12 ! vaapih264enc ! "
                                "rtph264pay name=pay0",
                                &error);
    if (!pipeline) {
        log_error("Error processing pipeline for RealSense stream device: %s\n",
                  error ? error->message : "unknown error");
        g_clear_error(&error);

        return nullptr;
    }

    GstElement *appsrc = gst_bin_get_by_name(GST_BIN(pipeline), "mysource");

    /* setup */
    gst_app_src_set_caps(GST_APP_SRC(appsrc),
                         gst_caps_new_simple("video/x-raw", "format", G_TYPE_STRING, "RGB", "width",
                                             G_TYPE_INT, WIDTH, "height", G_TYPE_INT, HEIGHT,
                                             NULL));

    /* setup appsrc */
    g_object_set(G_OBJECT(appsrc), "is-live", TRUE, "format", GST_FORMAT_TIME, NULL);

    /* connect signals */
    GstAppSrcCallbacks cbs;
    cbs.need_data = cb_need_data;
    cbs.enough_data = cb_enough_data;
    cbs.seek_data = cb_seek_data;
    gst_app_src_set_callbacks(GST_APP_SRC_CAST(appsrc), &cbs, ctx, NULL);

    g_object_set_data(G_OBJECT(pipeline), "context", ctx);

    return pipeline;
}

void StreamRealSense::finalize_gstreamer_pipeline(GstElement *pipeline)
{
    Context *ctx = (Context *)g_object_get_data(G_OBJECT(pipeline), "context");

    if (!ctx) {
        log_error("Media not created by stream_realsense is being cleared with stream_realsense");
        return;
    }
    rs_delete_context(ctx->rs_ctx, NULL);
    free(ctx);
}
<commit_msg>realsense: Turn on auto exposure and always on IR<commit_after>/*
 * This file is part of the Camera Streaming Daemon
 *
 * Copyright (C) 2017  Intel Corporation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <assert.h>
#include <fcntl.h>
#include <gst/app/gstappsrc.h>
#include <librealsense/rs.h>
#include <sstream>
#include <sys/ioctl.h>
#include <unistd.h>

#include "log.h"
#include "samples/stream_realsense.h"

#define WIDTH (640)
#define HEIGHT (480)
#define SIZE (WIDTH * HEIGHT * 3)
#define ONE_METER (999)

struct rgb {
    uint8_t r;
    uint8_t g;
    uint8_t b;
};

struct Context {
    rs_device *dev;
    rs_context *rs_ctx;
    struct rgb rgb_data[];
};

static void rainbow_scale(double value, uint8_t rgb[])
{
    rgb[0] = rgb[1] = rgb[2] = 0;

    if (value <= 0.0)
        return;

    if (value < 0.25) { // RED to YELLOW
        rgb[0] = 255;
        rgb[1] = (uint8_t)255 * (value / 0.25);
    } else if (value < 0.5) { // YELLOW to GREEN
        rgb[0] = (uint8_t)255 * (1 - ((value - 0.25) / 0.25));
        rgb[1] = 255;
    } else if (value < 0.75) { // GREEN to CYAN
        rgb[1] = 255;
        rgb[2] = (uint8_t)255 * (value - 0.5 / 0.25);
    } else if (value < 1.0) { // CYAN to BLUE
        rgb[1] = (uint8_t)255 * (1 - ((value - 0.75) / 0.25));
        rgb[2] = 255;
    } else { // BLUE
        rgb[2] = 255;
    }
}

static void cb_need_data(GstAppSrc *appsrc, guint unused_size, gpointer user_data)
{
    GstFlowReturn ret;
    Context *ctx = (Context *)user_data;

    rs_wait_for_frames(ctx->dev, NULL);
    uint16_t *depth = (uint16_t *)rs_get_frame_data(ctx->dev, RS_STREAM_DEPTH, NULL);
    if (!depth) {
        log_error("No depth data. Not building frame");
        return;
    }

    GstBuffer *buffer
        = gst_buffer_new_wrapped_full((GstMemoryFlags)0, ctx->rgb_data, SIZE, 0, SIZE, NULL, NULL);
    assert(buffer);
    for (int i = 0, end = WIDTH * HEIGHT; i < end; ++i) {
        uint8_t rainbow[3];
        rainbow_scale((double)depth[i] * 0.001, rainbow);

        ctx->rgb_data[i].r = rainbow[0];
        ctx->rgb_data[i].g = rainbow[1];
        ctx->rgb_data[i].b = rainbow[2];
    }

    g_signal_emit_by_name(appsrc, "push-buffer", buffer, &ret);
}

static void cb_enough_data(GstAppSrc *src, gpointer user_data)
{
}

static gboolean cb_seek_data(GstAppSrc *src, guint64 offset, gpointer user_data)
{
    return TRUE;
}

StreamRealSense::StreamRealSense()
    : Stream()
{
}

const std::string StreamRealSense::get_path() const
{
    return "/RealSense";
}

const std::string StreamRealSense::get_name() const
{
    return "RealSense Sample Stream";
}

const std::vector<Stream::PixelFormat> &StreamRealSense::get_formats() const
{
    static std::vector<Stream::PixelFormat> formats;
    return formats;
}

GstElement *
StreamRealSense::create_gstreamer_pipeline(std::map<std::string, std::string> &params) const
{
    Context *ctx = (Context *)malloc(sizeof(Context) + SIZE);
    assert(ctx);

    /* librealsense */
    rs_error *e = 0;
    ctx->rs_ctx = rs_create_context(RS_API_VERSION, &e);
    if (e) {
        log_error("rs_error was raised when calling %s(%s): %s", rs_get_failed_function(e),
                  rs_get_failed_args(e), rs_get_error_message(e));
        log_error("Current librealsense api version %d", rs_get_api_version(NULL));
        log_error("Compiled for librealsense api version %d", RS_API_VERSION);
        return nullptr;
    }
    ctx->dev = rs_get_device(ctx->rs_ctx, 0, NULL);
    if (!ctx->dev) {
        log_error("Unable to access realsense device");
        return nullptr;
    }

    /* Configure all streams to run at VGA resolution at 60 frames per second */
    rs_enable_stream(ctx->dev, RS_STREAM_DEPTH, WIDTH, HEIGHT, RS_FORMAT_Z16, 60, NULL);
    rs_start_device(ctx->dev, NULL);

    if (rs_device_supports_option(ctx->dev, RS_OPTION_R200_EMITTER_ENABLED, NULL))
        rs_set_device_option(ctx->dev, RS_OPTION_R200_EMITTER_ENABLED, 1, NULL);
    if (rs_device_supports_option(ctx->dev, RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED, NULL))
        rs_set_device_option(ctx->dev, RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED, 1, NULL);

    /* gstreamer */
    GError *error = nullptr;
    GstElement *pipeline;

    pipeline = gst_parse_launch("appsrc name=mysource ! videoconvert ! "
                                "video/x-raw,width=640,height=480,format=NV12 ! vaapih264enc ! "
                                "rtph264pay name=pay0",
                                &error);
    if (!pipeline) {
        log_error("Error processing pipeline for RealSense stream device: %s\n",
                  error ? error->message : "unknown error");
        g_clear_error(&error);

        return nullptr;
    }

    GstElement *appsrc = gst_bin_get_by_name(GST_BIN(pipeline), "mysource");

    /* setup */
    gst_app_src_set_caps(GST_APP_SRC(appsrc),
                         gst_caps_new_simple("video/x-raw", "format", G_TYPE_STRING, "RGB", "width",
                                             G_TYPE_INT, WIDTH, "height", G_TYPE_INT, HEIGHT,
                                             NULL));

    /* setup appsrc */
    g_object_set(G_OBJECT(appsrc), "is-live", TRUE, "format", GST_FORMAT_TIME, NULL);

    /* connect signals */
    GstAppSrcCallbacks cbs;
    cbs.need_data = cb_need_data;
    cbs.enough_data = cb_enough_data;
    cbs.seek_data = cb_seek_data;
    gst_app_src_set_callbacks(GST_APP_SRC_CAST(appsrc), &cbs, ctx, NULL);

    g_object_set_data(G_OBJECT(pipeline), "context", ctx);

    return pipeline;
}

void StreamRealSense::finalize_gstreamer_pipeline(GstElement *pipeline)
{
    Context *ctx = (Context *)g_object_get_data(G_OBJECT(pipeline), "context");

    if (!ctx) {
        log_error("Media not created by stream_realsense is being cleared with stream_realsense");
        return;
    }
    rs_delete_context(ctx->rs_ctx, NULL);
    free(ctx);
}
<|endoftext|>


<commit_before>#include <babylon/samples/mesh/rotation_and_scaling_scene.h>

#include <babylon/cameras/arc_rotate_camera.h>
#include <babylon/lights/hemispheric_light.h>
#include <babylon/mesh/mesh.h>

namespace BABYLON {
namespace Samples {

RotationAndScalingScene::RotationAndScalingScene(ICanvas* iCanvas)
    : IRenderableScene(iCanvas)
{
}

RotationAndScalingScene::~RotationAndScalingScene()
{
}

const char* RotationAndScalingScene::getName()
{
  return "Rotation and Scaling Scene";
}

void RotationAndScalingScene::initializeScene(ICanvas* canvas, Scene* scene)
{
  // Create a camera
  auto camera = ArcRotateCamera::New("Camera", Math::PI, Math::PI / 8.f, 150.f,
                                     Vector3::Zero(), scene);

  // Attach the camera to the canvas
  camera->attachControl(canvas, true);

  // Create a basic light, aiming 0,1,0 - meaning, to the sky
  HemisphericLight::New("Hemi", Vector3(0, 1, 0), scene);

  // Creation of boxes
  auto box1 = Mesh::CreateBox("Box1", 6.f, scene);
  auto box2 = Mesh::CreateBox("Box2", 6.f, scene);
  auto box3 = Mesh::CreateBox("Box3", 6.f, scene);
  auto box4 = Mesh::CreateBox("Box4", 6.f, scene);
  auto box5 = Mesh::CreateBox("Box5", 6.f, scene);
  auto box6 = Mesh::CreateBox("Box6", 6.f, scene);
  auto box7 = Mesh::CreateBox("Box7", 6.f, scene);

  // Moving boxes on the x axis
  box1->position().x = -20.f;
  box2->position().x = -10.f;
  box3->position().x = 0.f;
  box4->position().x = 15.f;
  box5->position().x = 30.f;
  box6->position().x = 45.f;

  // Rotate box around the x axis
  box1->rotation().x = Math::PI / 6.f;

  // Rotate box around the y axis
  box2->rotation().y = Math::PI / 3.f;

  // Scaling on the x axis
  box4->scaling().x = 2.f;

  // Scaling on the y axis
  box5->scaling().y = 2.f;

  // Scaling on the z axis
  box6->scaling().z = 2.f;

  // Moving box7 relatively to box1
  box7->setParent(box1);
  box7->position().z = -10.f;
}

} // end of namespace Samples
} // end of namespace BABYLON
<commit_msg>Changed light color in rotate and scaling example<commit_after>#include <babylon/samples/mesh/rotation_and_scaling_scene.h>

#include <babylon/cameras/arc_rotate_camera.h>
#include <babylon/lights/hemispheric_light.h>
#include <babylon/mesh/mesh.h>

namespace BABYLON {
namespace Samples {

RotationAndScalingScene::RotationAndScalingScene(ICanvas* iCanvas)
    : IRenderableScene(iCanvas)
{
}

RotationAndScalingScene::~RotationAndScalingScene()
{
}

const char* RotationAndScalingScene::getName()
{
  return "Rotation and Scaling Scene";
}

void RotationAndScalingScene::initializeScene(ICanvas* canvas, Scene* scene)
{
  // Create a camera
  auto camera = ArcRotateCamera::New("Camera", Math::PI, Math::PI / 8.f, 150.f,
                                     Vector3::Zero(), scene);

  // Attach the camera to the canvas
  camera->attachControl(canvas, true);

  // Create a basic light, aiming 0,1,0 - meaning, to the sky
  auto light      = HemisphericLight::New("Hemi", Vector3(0, 1, 0), scene);
  light->diffuse  = Color3::FromInt(0xf68712);
  light->specular = Color3::FromInt(0xf1471d);

  // Creation of boxes
  auto box1 = Mesh::CreateBox("Box1", 6.f, scene);
  auto box2 = Mesh::CreateBox("Box2", 6.f, scene);
  auto box3 = Mesh::CreateBox("Box3", 6.f, scene);
  auto box4 = Mesh::CreateBox("Box4", 6.f, scene);
  auto box5 = Mesh::CreateBox("Box5", 6.f, scene);
  auto box6 = Mesh::CreateBox("Box6", 6.f, scene);
  auto box7 = Mesh::CreateBox("Box7", 6.f, scene);

  // Moving boxes on the x axis
  box1->position().x = -20.f;
  box2->position().x = -10.f;
  box3->position().x = 0.f;
  box4->position().x = 15.f;
  box5->position().x = 30.f;
  box6->position().x = 45.f;

  // Rotate box around the x axis
  box1->rotation().x = Math::PI / 6.f;

  // Rotate box around the y axis
  box2->rotation().y = Math::PI / 3.f;

  // Scaling on the x axis
  box4->scaling().x = 2.f;

  // Scaling on the y axis
  box5->scaling().y = 2.f;

  // Scaling on the z axis
  box6->scaling().z = 2.f;

  // Moving box7 relatively to box1
  box7->setParent(box1);
  box7->position().z = -10.f;
}

} // end of namespace Samples
} // end of namespace BABYLON
<|endoftext|>


<commit_before>#include "BrainStdAfx.h"
#include "HAL.h"
#include "Comms.h"

#include <sys/time.h>
#include <sys/resource.h>

#include <boost/program_options.hpp>
#include <thread>
#include <iostream>

#ifdef RASPBERRY_PI
extern auto shutdown_bcm() -> bool;
#endif

size_t s_test = 0;
bool s_exit = false;
boost::asio::io_service s_async_io_service;

namespace boost
{
	void throw_exception(std::exception const & e)
	{
        QLOGE("boost::exception {}", e.what());
		throw e;
    }
}

// Define the function to be called when ctrl-c (SIGINT) signal is sent to process
void signal_handler(int signum)
{
    if (s_exit)
    {
        QLOGI("Forcing an exit due to signal {}", signum);
        abort();
    }
    s_exit = true;
    QLOGI("Exitting due to signal {}", signum);
}

void out_of_memory_handler()
{
    QLOGE("Out of memory");
    std::abort();
}

int main(int argc, char const* argv[])
{
    signal(SIGINT, signal_handler); // Trap basic signals (exit cleanly)
    signal(SIGKILL, signal_handler);
    signal(SIGUSR1, signal_handler);
    signal(SIGQUIT, signal_handler);
//    signal(SIGABRT, signal_handler);
    signal(SIGTERM, signal_handler);

    //set the new_handler
    std::set_new_handler(out_of_memory_handler);

    q::logging::add_logger(q::logging::Logger_uptr(new q::logging::Console_Logger()));
    q::logging::set_decorations(q::logging::Decorations(q::logging::Decoration::TIMESTAMP, q::logging::Decoration::LEVEL, q::logging::Decoration::TOPIC));

    QLOG_TOPIC("silk");

//    q::util::Rand rnd;
//    while (true)
//    {
//        math::vec3f target(rnd.get_float() * 40.0, rnd.get_float() * 40.0, rnd.get_float() * 10.0);
//        test_mm(target, 0.01);
//        test_mm(target, 0.1);
//        test_mm(target, 0.5);
//    }

    namespace po = boost::program_options;

	po::options_description desc("Options");
	desc.add_options()
		("help", "produce help message")
        ("blind", "no camera")
        ("test", po::value<size_t>(), "test");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help"))
	{
        std::cout << desc << "\n";
		return 1;
	}

    s_test = vm.count("test") ? vm["test"].as<size_t>() : size_t(0);
    //bool blind = vm.count("blind") != 0;

    QLOGI("Creating io_service thread");

    boost::shared_ptr<boost::asio::io_service::work> async_work(new boost::asio::io_service::work(s_async_io_service));
    auto async_thread = std::thread([]() { s_async_io_service.run(); });

#if defined RASPBERRY_PI
    {
        int policy = SCHED_FIFO;
        struct sched_param param;
        param.sched_priority = sched_get_priority_max(policy);
        if (pthread_setschedparam(pthread_self(), policy, &param) != 0)
        {
            perror("main sched_setscheduler");
            exit(EXIT_FAILURE);
        }
        policy = SCHED_IDLE;
        param.sched_priority = sched_get_priority_min(policy);
        if (pthread_setschedparam(async_thread.native_handle(), policy, &param) != 0)
        {
            perror("async_thread sched_setscheduler");
            exit(EXIT_FAILURE);
        }
    }
#endif

    try
    {
        silk::HAL hal;
        silk::Comms comms(hal);

        if (!hal.init(comms))
        {
            QLOGE("Hardware failure! Aborting");
            goto exit;
        }

#if defined RASPBERRY_PI
        if (!comms.start_rfmon("mon0", 5))
        {
            QLOGE("Cannot start communication channel! Aborting");
            goto exit;
        }
#else
        if (!comms.start_udp(8000, 8001))
        {
            QLOGE("Cannot start communication channel! Aborting");
            goto exit;
        }
#endif

//        while (!s_exit)
//        {
//            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
//            QLOGI("Waiting for comms to connect...");
//            if (comms.is_connected())
//            {
//                break;
//            }
//        }

        QLOGI("All systems up. Ready to fly...");

        {
            constexpr std::chrono::milliseconds PERIOD(5);
            auto last = q::Clock::now();
            while (!s_exit)
            {
                auto start = q::Clock::now();
                if (start - last >= PERIOD)
                {
                    last = start;

                    comms.process();
                    hal.process();
                }

                //don't sleep too much if we're late
                if (q::Clock::now() - start < PERIOD)
                {
                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
                }
                else
                {
                    std::this_thread::yield();
                }
            }
        }

exit:
        QLOGI("Stopping everything");

        //stop threads
        async_work.reset();
        s_async_io_service.stop();
        if (async_thread.joinable())
        {
            std::this_thread::yield();
            async_thread.join();
        }
        hal.shutdown();

#ifdef RASPBERRY_PI
        shutdown_bcm();
#endif
    }
    catch (std::exception const& e)
    {
        QLOGE("exception: {}", e.what());
        abort();
    }

    QLOGI("Closing");
}

<commit_msg>Removed sleeping to allow dependent nodes in the wrong order to propagate faster the stream data<commit_after>#include "BrainStdAfx.h"
#include "HAL.h"
#include "Comms.h"

#include <sys/time.h>
#include <sys/resource.h>

#include <boost/program_options.hpp>
#include <thread>
#include <iostream>

#ifdef RASPBERRY_PI
extern auto shutdown_bcm() -> bool;
#endif

size_t s_test = 0;
bool s_exit = false;
boost::asio::io_service s_async_io_service;

namespace boost
{
	void throw_exception(std::exception const & e)
	{
        QLOGE("boost::exception {}", e.what());
		throw e;
    }
}

// Define the function to be called when ctrl-c (SIGINT) signal is sent to process
void signal_handler(int signum)
{
    if (s_exit)
    {
        QLOGI("Forcing an exit due to signal {}", signum);
        abort();
    }
    s_exit = true;
    QLOGI("Exitting due to signal {}", signum);
}

void out_of_memory_handler()
{
    QLOGE("Out of memory");
    std::abort();
}

int main(int argc, char const* argv[])
{
    signal(SIGINT, signal_handler); // Trap basic signals (exit cleanly)
    signal(SIGKILL, signal_handler);
    signal(SIGUSR1, signal_handler);
    signal(SIGQUIT, signal_handler);
//    signal(SIGABRT, signal_handler);
    signal(SIGTERM, signal_handler);

    //set the new_handler
    std::set_new_handler(out_of_memory_handler);

    q::logging::add_logger(q::logging::Logger_uptr(new q::logging::Console_Logger()));
    q::logging::set_decorations(q::logging::Decorations(q::logging::Decoration::TIMESTAMP, q::logging::Decoration::LEVEL, q::logging::Decoration::TOPIC));

    QLOG_TOPIC("silk");

//    q::util::Rand rnd;
//    while (true)
//    {
//        math::vec3f target(rnd.get_float() * 40.0, rnd.get_float() * 40.0, rnd.get_float() * 10.0);
//        test_mm(target, 0.01);
//        test_mm(target, 0.1);
//        test_mm(target, 0.5);
//    }

    namespace po = boost::program_options;

	po::options_description desc("Options");
	desc.add_options()
		("help", "produce help message")
        ("blind", "no camera")
        ("test", po::value<size_t>(), "test");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help"))
	{
        std::cout << desc << "\n";
		return 1;
	}

    s_test = vm.count("test") ? vm["test"].as<size_t>() : size_t(0);
    //bool blind = vm.count("blind") != 0;

    QLOGI("Creating io_service thread");

    boost::shared_ptr<boost::asio::io_service::work> async_work(new boost::asio::io_service::work(s_async_io_service));
    auto async_thread = std::thread([]() { s_async_io_service.run(); });

#if defined RASPBERRY_PI
    {
        int policy = SCHED_FIFO;
        struct sched_param param;
        param.sched_priority = sched_get_priority_max(policy);
        if (pthread_setschedparam(pthread_self(), policy, &param) != 0)
        {
            perror("main sched_setscheduler");
            exit(EXIT_FAILURE);
        }
        policy = SCHED_IDLE;
        param.sched_priority = sched_get_priority_min(policy);
        if (pthread_setschedparam(async_thread.native_handle(), policy, &param) != 0)
        {
            perror("async_thread sched_setscheduler");
            exit(EXIT_FAILURE);
        }
    }
#endif

    try
    {
        silk::HAL hal;
        silk::Comms comms(hal);

        if (!hal.init(comms))
        {
            QLOGE("Hardware failure! Aborting");
            goto exit;
        }

#if defined RASPBERRY_PI
        if (!comms.start_rfmon("mon0", 5))
        {
            QLOGE("Cannot start communication channel! Aborting");
            goto exit;
        }
#else
        if (!comms.start_udp(8000, 8001))
        {
            QLOGE("Cannot start communication channel! Aborting");
            goto exit;
        }
#endif

//        while (!s_exit)
//        {
//            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
//            QLOGI("Waiting for comms to connect...");
//            if (comms.is_connected())
//            {
//                break;
//            }
//        }

        QLOGI("All systems up. Ready to fly...");

        {
            constexpr std::chrono::microseconds PERIOD(5000);
            auto last = q::Clock::now();
            while (!s_exit)
            {
                auto start = q::Clock::now();
                auto dt = start - last;
                if (dt > std::chrono::milliseconds(10))
                {
                    QLOGW("Process Latency of {}!!!!!", dt);
                }
                //if (dt >= PERIOD)
                {
                    last = start;

                    //QLOGI("---- LOOP -----");
                    comms.process();
                    hal.process();
                }

                //No sleeping here!!! process as fast as possible as the nodes are not always in the ideal order
                // and out of order nodes will be processes next 'frame'. So the quicker the frames, the smaller the lag between nodes

                //don't sleep too much if we're late
//                if (q::Clock::now() - start < PERIOD)
//                {
//                    //std::this_thread::sleep_for(std::chrono::milliseconds(1));
//                    for (volatile int i = 0; i < 10000; i++)
//                    {
//                        ;
//                    }
//                }
//                else
//                {
////                    std::this_thread::yield();
//                }
            }
        }

exit:
        QLOGI("Stopping everything");

        //stop threads
        async_work.reset();
        s_async_io_service.stop();
        if (async_thread.joinable())
        {
            std::this_thread::yield();
            async_thread.join();
        }
        hal.shutdown();

#ifdef RASPBERRY_PI
        shutdown_bcm();
#endif
    }
    catch (std::exception const& e)
    {
        QLOGE("exception: {}", e.what());
        abort();
    }

    QLOGI("Closing");
}

<|endoftext|>