<commit_before>/*=========================================================================

Program:   ORFEO Toolbox
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
See OTBCopyright.txt for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#include "otbImageToSIFTKeyPointSetFilter.h"
#include "otbImage.h"
#include "otbImageFileReader.h"
#include "otbImageFileWriter.h"
#include "itkPointSet.h"
#include "itkVariableLengthVector.h"
#include "otbRationalQuotientResampleImageFilter.h"
#include "itkRGBPixel.h"
#include "itkImageRegionIterator.h"

#include <iostream>
#include <fstream>

int otbImageToSIFTKeyPointSetFilter(int argc, char * argv[])
{
  const char * infname = argv[1];
  const char * outfname = argv[2];
  const char * outputImageFilename = argv[3];

  const unsigned int octaves = atoi(argv[4]);
  const unsigned int scales = atoi(argv[5]);

  typedef float RealType;
  const unsigned int Dimension =2;

  typedef otb::Image<RealType,Dimension> ImageType;
  typedef itk::VariableLengthVector<RealType> RealVectorType;
  typedef otb::ImageFileReader<ImageType> ReaderType;
  typedef itk::PointSet<RealVectorType,Dimension> PointSetType;
  typedef otb::ImageToSIFTKeyPointSetFilter<ImageType,PointSetType> ImageToSIFTKeyPointSetFilterType;

  // PointSet iterator types
  typedef PointSetType::PointsContainer PointsContainerType;
  typedef PointsContainerType::Iterator PointsIteratorType;
  typedef PointSetType::PointDataContainer PointDataContainerType;
  typedef PointDataContainerType::Iterator PointDataIteratorType;

  // Instantiating object
  ReaderType::Pointer reader = ReaderType::New();
  ImageToSIFTKeyPointSetFilterType::Pointer filter = ImageToSIFTKeyPointSetFilterType::New();

  reader->SetFileName(infname);
  filter->SetInput(0,reader->GetOutput());
  filter->SetOctavesNumber(octaves);
  filter->SetScalesNumber(scales);
  filter->Update();

  std::ofstream outfile(outfname);
  outfile<<"Number of SIFT key: "<<filter->GetOutput()->GetNumberOfPoints()<<std::endl;
  ImageType::OffsetType t = {{ 0, 1}};
  ImageType::OffsetType b = {{ 0,-1}};
  ImageType::OffsetType l = {{ 1, 0}};
  ImageType::OffsetType r = {{-1, 0}};

  typedef otb::Image<unsigned char,2> UCharImageType;

  typedef itk::RGBPixel<unsigned char> RGBPixelType;
  typedef otb::Image<RGBPixelType, 2> OutputImageType;

  typedef otb::ImageFileWriter<OutputImageType> WriterType;

  OutputImageType::Pointer outputImage = OutputImageType::New();
  outputImage->SetRegions(reader->GetOutput()->GetLargestPossibleRegion());
  outputImage->Allocate();

  OutputImageType::SizeType lRadius;
  lRadius.Fill(1);

  itk::ImageRegionIterator<OutputImageType> iterOutput(outputImage,
						       outputImage->GetRequestedRegion());

  for (iterOutput.GoToBegin(); !iterOutput.IsAtEnd(); ++iterOutput)
    {
      ImageType::IndexType index = iterOutput.GetIndex();
      ImageType::PixelType grayPix = reader->GetOutput()->GetPixel(index);
      OutputImageType::PixelType rgbPixel;
      rgbPixel.SetRed( static_cast<unsigned char>(grayPix) );
      rgbPixel.SetGreen( static_cast<unsigned char>(grayPix) );
      rgbPixel.SetBlue( static_cast<unsigned char>(grayPix) );

      iterOutput.Set(rgbPixel);
    }

  PointsIteratorType pIt = filter->GetOutput()->GetPoints()->Begin();
  PointDataIteratorType pdIt = filter->GetOutput()->GetPointData()->Begin();
  while( pIt!=filter->GetOutput()->GetPoints()->End() &&
	 pdIt!=filter->GetOutput()->GetPointData()->End() )
    {
      ImageType::IndexType index;
      reader->GetOutput()->TransformPhysicalPointToIndex(pIt.Value(),index);
      outfile<<"Point: "<<pIt.Value()<<", Index: "<<index \
	     << " Octave: " << pdIt.Value()[0] \
	     << " Scale: " <<  pdIt.Value()[1] << std::endl;

      OutputImageType::PixelType keyPixel;
      keyPixel.SetRed(0);
      keyPixel.SetGreen(255);
      keyPixel.SetBlue(0);

      outputImage->SetPixel(index,keyPixel);
      outputImage->SetPixel(index+t,keyPixel);
      outputImage->SetPixel(index+b,keyPixel);
      outputImage->SetPixel(index+l,keyPixel);
      outputImage->SetPixel(index+r,keyPixel);
      ++pIt;
      ++pdIt;
    }
  outfile.close();

  WriterType::Pointer writer = WriterType::New();
  writer->SetInput(outputImage);
  writer->SetFileName(outputImageFilename);
  writer->Update();

  return EXIT_SUCCESS;
}
<commit_msg>*Correction m image index bound<commit_after>/*=========================================================================

Program:   ORFEO Toolbox
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
See OTBCopyright.txt for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#include "otbImageToSIFTKeyPointSetFilter.h"
#include "otbImage.h"
#include "otbImageFileReader.h"
#include "otbImageFileWriter.h"
#include "itkPointSet.h"
#include "itkVariableLengthVector.h"
#include "otbRationalQuotientResampleImageFilter.h"
#include "itkRGBPixel.h"
#include "itkImageRegionIterator.h"

#include <iostream>
#include <fstream>

int otbImageToSIFTKeyPointSetFilter(int argc, char * argv[])
{
  const char * infname = argv[1];
  const char * outfname = argv[2];
  const char * outputImageFilename = argv[3];

  const unsigned int octaves = atoi(argv[4]);
  const unsigned int scales = atoi(argv[5]);

  typedef float RealType;
  const unsigned int Dimension =2;

  typedef otb::Image<RealType,Dimension> ImageType;
  typedef itk::VariableLengthVector<RealType> RealVectorType;
  typedef otb::ImageFileReader<ImageType> ReaderType;
  typedef itk::PointSet<RealVectorType,Dimension> PointSetType;
  typedef otb::ImageToSIFTKeyPointSetFilter<ImageType,PointSetType> ImageToSIFTKeyPointSetFilterType;

  // PointSet iterator types
  typedef PointSetType::PointsContainer PointsContainerType;
  typedef PointsContainerType::Iterator PointsIteratorType;
  typedef PointSetType::PointDataContainer PointDataContainerType;
  typedef PointDataContainerType::Iterator PointDataIteratorType;

  // Instantiating object
  ReaderType::Pointer reader = ReaderType::New();
  ImageToSIFTKeyPointSetFilterType::Pointer filter = ImageToSIFTKeyPointSetFilterType::New();

  reader->SetFileName(infname);
  filter->SetInput(0,reader->GetOutput());
  filter->SetOctavesNumber(octaves);
  filter->SetScalesNumber(scales);
  filter->Update();

  ImageType::OffsetType t = {{ 0, 1}};
  ImageType::OffsetType b = {{ 0,-1}};
  ImageType::OffsetType l = {{ 1, 0}};
  ImageType::OffsetType r = {{-1, 0}};

  typedef otb::Image<unsigned char,2> UCharImageType;

  typedef itk::RGBPixel<unsigned char> RGBPixelType;
  typedef otb::Image<RGBPixelType, 2> OutputImageType;

  typedef otb::ImageFileWriter<OutputImageType> WriterType;

  OutputImageType::Pointer outputImage = OutputImageType::New();
  outputImage->SetRegions(reader->GetOutput()->GetLargestPossibleRegion());
  outputImage->Allocate();

  OutputImageType::SizeType lRadius;
  lRadius.Fill(1);

  itk::ImageRegionIterator<OutputImageType> iterOutput(outputImage,
						       outputImage->GetRequestedRegion());

  for (iterOutput.GoToBegin(); !iterOutput.IsAtEnd(); ++iterOutput)
    {
      ImageType::IndexType index = iterOutput.GetIndex();
      ImageType::PixelType grayPix = reader->GetOutput()->GetPixel(index);
      OutputImageType::PixelType rgbPixel;
      rgbPixel.SetRed( static_cast<unsigned char>(grayPix) );
      rgbPixel.SetGreen( static_cast<unsigned char>(grayPix) );
      rgbPixel.SetBlue( static_cast<unsigned char>(grayPix) );

      iterOutput.Set(rgbPixel);
    }

  PointsIteratorType pIt = filter->GetOutput()->GetPoints()->Begin();
  ImageType::SpacingType spacing = reader->GetOutput()->GetSpacing();
  ImageType::PointType origin = reader->GetOutput()->GetOrigin();
  OutputImageType::SizeType size = outputImage->GetLargestPossibleRegion().GetSize();

  while( pIt!=filter->GetOutput()->GetPoints()->End() )
    {
      ImageType::IndexType index;

      index[0] = (unsigned int)
	(vcl_floor
	 ((double)((pIt.Value()[0]-origin[0])/spacing[0]+0.5)));

      index[1] = (unsigned int)
	(vcl_floor
	 ((double)((pIt.Value()[1]-origin[1])/spacing[1]+0.5)));

      OutputImageType::PixelType keyPixel;
      keyPixel.SetRed(0);
      keyPixel.SetGreen(255);
      keyPixel.SetBlue(0);

      outputImage->SetPixel(index,keyPixel);
      if (static_cast<unsigned int>(index[1]) < static_cast<unsigned int>(size[1]) )
	outputImage->SetPixel(index+t,keyPixel);
      if (index[1] > 0)
	outputImage->SetPixel(index+b,keyPixel);
      if (static_cast<unsigned int>(index[0]) < static_cast<unsigned int>(size[0]) )
	outputImage->SetPixel(index+l,keyPixel);
      if (index[0] > 0)
	outputImage->SetPixel(index+r,keyPixel);
      ++pIt;
    }

  std::ofstream outfile(outfname);
  outfile << filter;
  outfile.close();

  WriterType::Pointer writer = WriterType::New();
  writer->SetInput(outputImage);
  writer->SetFileName(outputImageFilename);
  writer->Update();

  return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>// MOOSE includes
#include "LaplaceProjectionTransfer.h"
#include "FEProblem.h"
#include "AddVariableAction.h"
#include "MooseMesh.h"
#include "AuxiliarySystem.h"
#include "MooseVariable.h"
// libMesh includes
#include "libmesh/quadrature_gauss.h"
#include "libmesh/dof_map.h"
#include "libmesh/mesh_function.h"
#include "libmesh/mesh_tools.h"
#include "libmesh/string_to_enum.h"
#include "libmesh/parallel_algebra.h"
#include "libmesh/sparse_matrix.h"

void
assemble_laplace_proj(EquationSystems & es, const std::string & system_name)
{
  LaplaceProjectionTransfer * transfer = es.parameters.get<LaplaceProjectionTransfer *>("transfer");
  transfer->assembleLaplaceProjection(es, system_name);
}

registerMooseObject("THMApp", LaplaceProjectionTransfer);

template <>
InputParameters
validParams<LaplaceProjectionTransfer>()
{
  InputParameters params = validParams<Transfer>();
  params.addRequiredParam<AuxVariableName>(
      "variable", "The auxiliary variable to store the transferred values in.");
  params.addRequiredParam<VariableName>("source_variable", "The variable to transfer from.");
  params.addParam<bool>("fixed_meshes",
                        false,
                        "Set to true when the meshes are not changing (ie, "
                        "no movement or adaptivity).  This will cache some "
                        "information to speed up the transfer.");

  return params;
}

LaplaceProjectionTransfer::LaplaceProjectionTransfer(const InputParameters & parameters)
  : Transfer(parameters),
    _to_var_name(getParam<AuxVariableName>("variable")),
    _from_var_name(getParam<VariableName>("source_variable")),
    _fixed_meshes(getParam<bool>("fixed_meshes"))
{
}

void
LaplaceProjectionTransfer::initialSetup()
{
  EquationSystems & es = _fe_problem.es();

  // Add the projection system.
  MooseVariable & from_var = _fe_problem.getStandardVariable(0, _from_var_name);
  FEType fe_type = _fe_problem.getStandardVariable(0, _to_var_name).feType();
  LinearImplicitSystem & proj_sys =
      es.add_system<LinearImplicitSystem>("laplace-proj-sys-" + name());
  _proj_var_num = proj_sys.add_variable("var", fe_type, &from_var.activeSubdomains());
  proj_sys.attach_assemble_function(assemble_laplace_proj);
  _proj_sys = &proj_sys;

  // Prevent the projection system from being written to checkpoint
  // files.  In the event of a recover or restart, we'll read the checkpoint
  // before this initialSetup method is called.  As a result, we'll find
  // systems in the checkpoint (the projection systems) that we don't know
  // what to do with, and there will be a crash.  We could fix this by making
  // the systems in the constructor, except we don't know how many sub apps
  // there are at the time of construction.  So instead, we'll just nuke the
  // projection system and rebuild it from scratch every recover/restart.
  proj_sys.hide_output() = true;

  // Reinitialize EquationSystems since we added a system.
  es.reinit();
}

void
LaplaceProjectionTransfer::assembleLaplaceProjection(EquationSystems & es,
                                                     const std::string & system_name)
{
  // Get the system and mesh from the input arguments.
  LinearImplicitSystem & system = es.get_system<LinearImplicitSystem>(system_name);
  MeshBase & to_mesh = es.get_mesh();

  // Setup system vectors and matrices.
  FEType fe_type = system.variable_type(0);
  std::unique_ptr<FEBase> fe(FEBase::build(to_mesh.mesh_dimension(), fe_type));
  QGauss qrule(to_mesh.mesh_dimension(), fe_type.default_quadrature_order());
  fe->attach_quadrature_rule(&qrule);
  const DofMap & dof_map = system.get_dof_map();
  DenseMatrix<Number> Ke;
  DenseVector<Number> Fe;
  unsigned int nqp = qrule.n_points();
  std::vector<dof_id_type> dof_indices;
  const std::vector<Real> & JxW = fe->get_JxW();
  const std::vector<std::vector<Real>> & phi = fe->get_phi();
  const std::vector<std::vector<RealGradient>> & dphi = fe->get_dphi();

  std::unique_ptr<FEBase> fe_face(FEBase::build(to_mesh.mesh_dimension(), fe_type));
  QGauss qrule_face(to_mesh.mesh_dimension() - 1, fe_type.default_quadrature_order());
  fe_face->attach_quadrature_rule(&qrule_face);
  unsigned int from_var_num = _fe_problem
                                  .getVariable(0,
                                               _from_var_name,
                                               Moose::VarKindType::VAR_ANY,
                                               Moose::VarFieldType::VAR_FIELD_STANDARD)
                                  .number();
  const DofMap & from_dof_map = _fe_problem.getAuxiliarySystem().dofMap();
  std::vector<dof_id_type> from_dof_indices;
  // FIXME: remove the need of calling getAuxiliarySystem()
  const NumericVector<Real> & from_current_solution =
      *_fe_problem.getAuxiliarySystem().currentSolution();
  std::vector<RealGradient> from_var_grad(nqp);

  const MeshBase::const_element_iterator end_el = to_mesh.active_local_elements_end();
  for (MeshBase::const_element_iterator el = to_mesh.active_local_elements_begin(); el != end_el;
       ++el)
  {
    const Elem * elem = *el;

    from_dof_map.dof_indices(elem, from_dof_indices, from_var_num);
    if (from_dof_indices.size() > 0)
    {
      fe->reinit(elem);
      // compute values of the source FE variable
      for (unsigned int i = 0; i < nqp; ++i)
        from_var_grad[i] = 0;

      for (unsigned int i = 0; i < phi.size(); i++)
      {
        unsigned int idx = from_dof_indices[i];
        Real soln_local = from_current_solution(idx);

        for (unsigned int qp = 0; qp < nqp; qp++)
          from_var_grad[qp].add_scaled(dphi[i][qp], soln_local);
      }

      dof_map.dof_indices(elem, dof_indices);

      Ke.resize(dof_indices.size(), dof_indices.size());
      Fe.resize(dof_indices.size());

      for (unsigned int qp = 0; qp < qrule.n_points(); qp++)
      {
        // Now compute the element matrix and RHS contributions.
        for (unsigned int i = 0; i < phi.size(); i++)
        {
          // RHS
          Fe(i) -= JxW[qp] * (from_var_grad[qp] * dphi[i][qp]);

          for (unsigned int j = 0; j < phi.size(); j++)
          {
            // The matrix contribution
            Ke(i, j) += JxW[qp] * (phi[i][qp] * phi[j][qp]);
          }
        }
      }

      // side
      for (unsigned int side = 0; side < elem->n_sides(); side++)
      {
        if (elem->neighbor_ptr(side) == libmesh_nullptr)
        {
          const std::vector<std::vector<Real>> & phi_face = fe_face->get_phi();
          const std::vector<std::vector<RealGradient>> & dphi_face = fe_face->get_dphi();
          const std::vector<Real> & JxW_face = fe_face->get_JxW();
          const std::vector<Point> & normals = fe_face->get_normals();

          fe_face->reinit(elem, side);

          unsigned int nqp_face = qrule_face.n_points();

          // compute values of the source FE variable
          for (unsigned int i = 0; i < nqp_face; ++i)
            from_var_grad[i] = 0;

          for (unsigned int i = 0; i < phi.size(); i++)
          {
            unsigned int idx = from_dof_indices[i];
            Real soln_local = from_current_solution(idx);

            for (unsigned int qp = 0; qp < nqp_face; qp++)
              from_var_grad[qp].add_scaled(dphi_face[i][qp], soln_local);
          }

          for (unsigned int qp = 0; qp < nqp_face; qp++)
          {
            for (unsigned int i = 0; i < phi_face.size(); i++)
              Fe(i) += JxW_face[qp] * from_var_grad[qp] * normals[qp] * phi_face[i][qp];
          }
        }
      } // sides

      dof_map.constrain_element_matrix_and_vector(Ke, Fe, dof_indices);

      system.matrix->add_matrix(Ke, dof_indices);
      system.rhs->add_vector(Fe, dof_indices);
    }
  }
}

void
LaplaceProjectionTransfer::execute()
{
  projectSolution();
}

void
LaplaceProjectionTransfer::projectSolution()
{
  FEProblemBase & to_problem = _fe_problem;
  EquationSystems & proj_es = to_problem.es();
  LinearImplicitSystem & ls = *_proj_sys;
  // activate the current transfer
  proj_es.parameters.set<LaplaceProjectionTransfer *>("transfer") = this;

  // TODO: specify solver params in an input file
  // solver tolerance
  Real tol = proj_es.parameters.get<Real>("linear solver tolerance");
  proj_es.parameters.set<Real>("linear solver tolerance") = 1e-10; // set our tolerance
  // solve it
  ls.solve();
  proj_es.parameters.set<Real>("linear solver tolerance") = tol; // restore the original tolerance

  // copy projected solution into target es
  MeshBase & to_mesh = proj_es.get_mesh();

  MooseVariable & to_var = to_problem.getStandardVariable(0, _to_var_name);
  System & to_sys = to_var.sys().system();
  NumericVector<Number> * to_solution = to_sys.solution.get();

  {
    MeshBase::const_node_iterator it = to_mesh.local_nodes_begin();
    const MeshBase::const_node_iterator end_it = to_mesh.local_nodes_end();
    for (; it != end_it; ++it)
    {
      const Node * node = *it;
      for (unsigned int comp = 0; comp < node->n_comp(to_sys.number(), to_var.number()); comp++)
      {
        const dof_id_type proj_index = node->dof_number(ls.number(), _proj_var_num, comp);
        const dof_id_type to_index = node->dof_number(to_sys.number(), to_var.number(), comp);
        to_solution->set(to_index, (*ls.solution)(proj_index));
      }
    }
  }

  to_solution->close();
  to_sys.update();
}
<commit_msg>Fixing coupling functor in Laplace projection transfer<commit_after>// MOOSE includes
#include "LaplaceProjectionTransfer.h"
#include "FEProblem.h"
#include "AddVariableAction.h"
#include "MooseMesh.h"
#include "AuxiliarySystem.h"
#include "MooseVariable.h"
// libMesh includes
#include "libmesh/quadrature_gauss.h"
#include "libmesh/dof_map.h"
#include "libmesh/mesh_function.h"
#include "libmesh/mesh_tools.h"
#include "libmesh/string_to_enum.h"
#include "libmesh/parallel_algebra.h"
#include "libmesh/sparse_matrix.h"
#include "libmesh/default_coupling.h"

void
assemble_laplace_proj(EquationSystems & es, const std::string & system_name)
{
  LaplaceProjectionTransfer * transfer = es.parameters.get<LaplaceProjectionTransfer *>("transfer");
  transfer->assembleLaplaceProjection(es, system_name);
}

registerMooseObject("THMApp", LaplaceProjectionTransfer);

template <>
InputParameters
validParams<LaplaceProjectionTransfer>()
{
  InputParameters params = validParams<Transfer>();
  params.addRequiredParam<AuxVariableName>(
      "variable", "The auxiliary variable to store the transferred values in.");
  params.addRequiredParam<VariableName>("source_variable", "The variable to transfer from.");
  params.addParam<bool>("fixed_meshes",
                        false,
                        "Set to true when the meshes are not changing (ie, "
                        "no movement or adaptivity).  This will cache some "
                        "information to speed up the transfer.");

  return params;
}

LaplaceProjectionTransfer::LaplaceProjectionTransfer(const InputParameters & parameters)
  : Transfer(parameters),
    _to_var_name(getParam<AuxVariableName>("variable")),
    _from_var_name(getParam<VariableName>("source_variable")),
    _fixed_meshes(getParam<bool>("fixed_meshes"))
{
}

void
LaplaceProjectionTransfer::initialSetup()
{
  EquationSystems & es = _fe_problem.es();

  // Add the projection system.
  MooseVariable & from_var = _fe_problem.getStandardVariable(0, _from_var_name);
  FEType fe_type = _fe_problem.getStandardVariable(0, _to_var_name).feType();
  LinearImplicitSystem & proj_sys =
      es.add_system<LinearImplicitSystem>("laplace-proj-sys-" + name());
  proj_sys.get_dof_map().add_coupling_functor(
      proj_sys.get_dof_map().default_coupling(),
      false); // The false keeps it from getting added to the mesh
  _proj_var_num = proj_sys.add_variable("var", fe_type, &from_var.activeSubdomains());
  proj_sys.attach_assemble_function(assemble_laplace_proj);
  _proj_sys = &proj_sys;

  // Prevent the projection system from being written to checkpoint
  // files.  In the event of a recover or restart, we'll read the checkpoint
  // before this initialSetup method is called.  As a result, we'll find
  // systems in the checkpoint (the projection systems) that we don't know
  // what to do with, and there will be a crash.  We could fix this by making
  // the systems in the constructor, except we don't know how many sub apps
  // there are at the time of construction.  So instead, we'll just nuke the
  // projection system and rebuild it from scratch every recover/restart.
  proj_sys.hide_output() = true;

  // Reinitialize EquationSystems since we added a system.
  es.reinit();
}

void
LaplaceProjectionTransfer::assembleLaplaceProjection(EquationSystems & es,
                                                     const std::string & system_name)
{
  // Get the system and mesh from the input arguments.
  LinearImplicitSystem & system = es.get_system<LinearImplicitSystem>(system_name);
  MeshBase & to_mesh = es.get_mesh();

  // Setup system vectors and matrices.
  FEType fe_type = system.variable_type(0);
  std::unique_ptr<FEBase> fe(FEBase::build(to_mesh.mesh_dimension(), fe_type));
  QGauss qrule(to_mesh.mesh_dimension(), fe_type.default_quadrature_order());
  fe->attach_quadrature_rule(&qrule);
  const DofMap & dof_map = system.get_dof_map();
  DenseMatrix<Number> Ke;
  DenseVector<Number> Fe;
  unsigned int nqp = qrule.n_points();
  std::vector<dof_id_type> dof_indices;
  const std::vector<Real> & JxW = fe->get_JxW();
  const std::vector<std::vector<Real>> & phi = fe->get_phi();
  const std::vector<std::vector<RealGradient>> & dphi = fe->get_dphi();

  std::unique_ptr<FEBase> fe_face(FEBase::build(to_mesh.mesh_dimension(), fe_type));
  QGauss qrule_face(to_mesh.mesh_dimension() - 1, fe_type.default_quadrature_order());
  fe_face->attach_quadrature_rule(&qrule_face);
  unsigned int from_var_num = _fe_problem
                                  .getVariable(0,
                                               _from_var_name,
                                               Moose::VarKindType::VAR_ANY,
                                               Moose::VarFieldType::VAR_FIELD_STANDARD)
                                  .number();
  const DofMap & from_dof_map = _fe_problem.getAuxiliarySystem().dofMap();
  std::vector<dof_id_type> from_dof_indices;
  // FIXME: remove the need of calling getAuxiliarySystem()
  const NumericVector<Real> & from_current_solution =
      *_fe_problem.getAuxiliarySystem().currentSolution();
  std::vector<RealGradient> from_var_grad(nqp);

  const MeshBase::const_element_iterator end_el = to_mesh.active_local_elements_end();
  for (MeshBase::const_element_iterator el = to_mesh.active_local_elements_begin(); el != end_el;
       ++el)
  {
    const Elem * elem = *el;

    from_dof_map.dof_indices(elem, from_dof_indices, from_var_num);
    if (from_dof_indices.size() > 0)
    {
      fe->reinit(elem);
      // compute values of the source FE variable
      for (unsigned int i = 0; i < nqp; ++i)
        from_var_grad[i] = 0;

      for (unsigned int i = 0; i < phi.size(); i++)
      {
        unsigned int idx = from_dof_indices[i];
        Real soln_local = from_current_solution(idx);

        for (unsigned int qp = 0; qp < nqp; qp++)
          from_var_grad[qp].add_scaled(dphi[i][qp], soln_local);
      }

      dof_map.dof_indices(elem, dof_indices);

      Ke.resize(dof_indices.size(), dof_indices.size());
      Fe.resize(dof_indices.size());

      for (unsigned int qp = 0; qp < qrule.n_points(); qp++)
      {
        // Now compute the element matrix and RHS contributions.
        for (unsigned int i = 0; i < phi.size(); i++)
        {
          // RHS
          Fe(i) -= JxW[qp] * (from_var_grad[qp] * dphi[i][qp]);

          for (unsigned int j = 0; j < phi.size(); j++)
          {
            // The matrix contribution
            Ke(i, j) += JxW[qp] * (phi[i][qp] * phi[j][qp]);
          }
        }
      }

      // side
      for (unsigned int side = 0; side < elem->n_sides(); side++)
      {
        if (elem->neighbor_ptr(side) == libmesh_nullptr)
        {
          const std::vector<std::vector<Real>> & phi_face = fe_face->get_phi();
          const std::vector<std::vector<RealGradient>> & dphi_face = fe_face->get_dphi();
          const std::vector<Real> & JxW_face = fe_face->get_JxW();
          const std::vector<Point> & normals = fe_face->get_normals();

          fe_face->reinit(elem, side);

          unsigned int nqp_face = qrule_face.n_points();

          // compute values of the source FE variable
          for (unsigned int i = 0; i < nqp_face; ++i)
            from_var_grad[i] = 0;

          for (unsigned int i = 0; i < phi.size(); i++)
          {
            unsigned int idx = from_dof_indices[i];
            Real soln_local = from_current_solution(idx);

            for (unsigned int qp = 0; qp < nqp_face; qp++)
              from_var_grad[qp].add_scaled(dphi_face[i][qp], soln_local);
          }

          for (unsigned int qp = 0; qp < nqp_face; qp++)
          {
            for (unsigned int i = 0; i < phi_face.size(); i++)
              Fe(i) += JxW_face[qp] * from_var_grad[qp] * normals[qp] * phi_face[i][qp];
          }
        }
      } // sides

      dof_map.constrain_element_matrix_and_vector(Ke, Fe, dof_indices);

      system.matrix->add_matrix(Ke, dof_indices);
      system.rhs->add_vector(Fe, dof_indices);
    }
  }
}

void
LaplaceProjectionTransfer::execute()
{
  projectSolution();
}

void
LaplaceProjectionTransfer::projectSolution()
{
  FEProblemBase & to_problem = _fe_problem;
  EquationSystems & proj_es = to_problem.es();
  LinearImplicitSystem & ls = *_proj_sys;
  // activate the current transfer
  proj_es.parameters.set<LaplaceProjectionTransfer *>("transfer") = this;

  // TODO: specify solver params in an input file
  // solver tolerance
  Real tol = proj_es.parameters.get<Real>("linear solver tolerance");
  proj_es.parameters.set<Real>("linear solver tolerance") = 1e-10; // set our tolerance
  // solve it
  ls.solve();
  proj_es.parameters.set<Real>("linear solver tolerance") = tol; // restore the original tolerance

  // copy projected solution into target es
  MeshBase & to_mesh = proj_es.get_mesh();

  MooseVariable & to_var = to_problem.getStandardVariable(0, _to_var_name);
  System & to_sys = to_var.sys().system();
  NumericVector<Number> * to_solution = to_sys.solution.get();

  {
    MeshBase::const_node_iterator it = to_mesh.local_nodes_begin();
    const MeshBase::const_node_iterator end_it = to_mesh.local_nodes_end();
    for (; it != end_it; ++it)
    {
      const Node * node = *it;
      for (unsigned int comp = 0; comp < node->n_comp(to_sys.number(), to_var.number()); comp++)
      {
        const dof_id_type proj_index = node->dof_number(ls.number(), _proj_var_num, comp);
        const dof_id_type to_index = node->dof_number(to_sys.number(), to_var.number(), comp);
        to_solution->set(to_index, (*ls.solution)(proj_index));
      }
    }
  }

  to_solution->close();
  to_sys.update();
}
<|endoftext|>


<commit_before>#pragma once

#include <sstream>

#include "rapidcheck/Show.hpp"

#include "ImplicitParam.hpp"
#include "RandomEngine.hpp"

namespace rc {
namespace detail {

//! Represents the structure of value generation where large complex values are
//! generated from small simple values. This also means that large values often
//! can be shrunk by shrinking the small values individually.
class RoseNode
{
public:
    //! Constructs a new root \c RoseNode.
    RoseNode() : RoseNode(nullptr) {}

    //! Returns an atom. If one has already been generated, it's reused. If not,
    //! a new one is generated.
    RandomEngine::Atom atom()
    {
        if (!m_hasAtom) {
            ImplicitParam<param::RandomEngine> randomEngine;
            m_atom = randomEngine->nextAtom();
            m_hasAtom = true;
        }

        return m_atom;
    }

    //! Outputs a string representation of this node and all its children.
    void print(std::ostream &os)
    {
        for (int i = 0; i < depth(); i++)
            os << "  ";
        os << "- " << description() << std::endl;

        for (auto &child : m_children)
            child.print(os);
    }

    //! Generates a value in this node using the given generator.
    template<typename Gen>
    typename Gen::GeneratedType generate(const Gen &generator)
    {
        typedef typename Gen::GeneratedType T;
        ImplicitParam<ShrunkNode> shrunkNode;

        m_lastGenerator = UntypedGeneratorUP(new Gen(generator));

        if (shrunkNode.hasBinding() && (*shrunkNode == nullptr)) {
            if (!m_shrinkIterator) {
                T value(regenerate<T>());
                // Did children shrink before us?
                if (*shrunkNode != nullptr)
                    return value;

                m_shrinkIterator = generator.shrink(value);
                // We need a fallback accepted generator if shrinking fails
                if (!m_acceptedGenerator)
                    m_acceptedGenerator = UntypedGeneratorUP(new Gen(generator));
            }

            if (m_shrinkIterator->hasNext()) {
                auto typedIterator =
                    dynamic_cast<ShrinkIterator<T> *>(m_shrinkIterator.get());
                assert(typedIterator != nullptr);
                m_shrunkGenerator = UntypedGeneratorUP(
                    new Constant<T>(typedIterator->next()));
                *shrunkNode = this;
            } else {
                // Shrinking exhausted
                m_shrunkGenerator = nullptr;
            }
        }

        return regenerate<T>();
    }

    //! Picks a value using the given generator in the context of the current
    //! node.
    template<typename Gen>
    typename Gen::GeneratedType pick(const Gen &generator)
    {
        ImplicitParam<NextChildIndex> nextChildIndex;
        if (*nextChildIndex >= m_children.size())
            m_children.push_back(RoseNode(this));
        (*nextChildIndex)++;
        return m_children[*nextChildIndex - 1].generate(generator);
    }

    //! Returns a list of \c ValueDescriptions from the immediate children of
    //! this node.
    std::vector<std::string> example()
    {
        std::vector<std::string> values;
        values.reserve(m_children.size());
        for (auto &child : m_children)
            values.push_back(child.stringValue());
        return values;
    }

    //! Returns a string representation of the value of this node or an empty
    //! if one hasn't been decided.
    std::string stringValue()
    {
        ImplicitParam<CurrentNode> currentNode;
        currentNode.let(this);
        ImplicitParam<NextChildIndex> nextChildIndex;
        nextChildIndex.let(0);

        UntypedGenerator *generator = activeGenerator();
        if (generator != nullptr)
            return generator->generateString();
        else
            return std::string();
    }

    //! Tries to find an immediate shrink that yields \c false for the given
    //! generator.
    //!
    //! @return  A tuple where the first value tells whether the shrinking was
    //!          successful and the second how many shrinks were tried,
    //!          regardless of success.
    template<typename Gen>
    std::tuple<bool, int> shrink(const Gen &generator)
    {
        ImplicitParam<ShrunkNode> shrunkNode;
        int numTries = 0;
        bool result = true;
        while (result) {
            numTries++;
            shrunkNode.let(nullptr);
            result = generate(generator);
            if (*shrunkNode == nullptr)
                return std::make_tuple(false, numTries);
        }

        (*shrunkNode)->acceptShrink();
        return std::make_tuple(true, numTries);
    }

    //! Move constructor.
    RoseNode(RoseNode &&other)
        : m_parent(other.m_parent)
        , m_children(std::move(other.m_children))
        , m_hasAtom(other.m_hasAtom)
        , m_atom(other.m_atom)
        , m_lastGenerator(std::move(other.m_lastGenerator))
        , m_acceptedGenerator(std::move(other.m_acceptedGenerator))
        , m_shrunkGenerator(std::move(other.m_shrunkGenerator))
        , m_shrinkIterator(std::move(other.m_shrinkIterator))
    {
        adoptChildren();
    }

    //! Move assignment
    RoseNode &operator=(RoseNode &&rhs)
    {
        m_parent = rhs.m_parent;
        m_children = std::move(rhs.m_children);
        m_hasAtom = rhs.m_hasAtom;
        m_atom = rhs.m_atom;
        m_lastGenerator = std::move(rhs.m_lastGenerator);
        m_acceptedGenerator = std::move(rhs.m_acceptedGenerator);
        m_shrunkGenerator = std::move(rhs.m_shrunkGenerator);
        m_shrinkIterator = std::move(rhs.m_shrinkIterator);
        adoptChildren();
        return *this;
    }

    void printExample()
    {
        for (const auto &desc : example())
            std::cout << desc << std::endl;
    }

    //! Returns a reference to the current node.
    static RoseNode &current()
    { return **ImplicitParam<CurrentNode>(); }

private:
    RC_DISABLE_COPY(RoseNode)

    // Implicit parameters, see ImplicitParam
    struct CurrentNode { typedef RoseNode *ValueType; };
    struct NextChildIndex { typedef size_t ValueType; };
    struct ShrunkNode { typedef RoseNode *ValueType; };

    //! Constructs a new \c RoseNode with the given parent or \c 0 if it should
    //! have no parent, i.e. is root.
    explicit RoseNode(RoseNode *parent) : m_parent(parent) {}

    //! Returns the depth of this node.
    int depth() const
    {
        if (m_parent == nullptr)
            return 0;

        return m_parent->depth() + 1;
    }

    //! Sets the parent of all children to this node.
    void adoptChildren()
    {
        for (auto &child : m_children)
            child.m_parent = this;
    }

    //! Returns a description of this node.
    std::string description() const
    {
        return generatorName();
    }

    //! Returns the index of this node among its sibilings. Returns \c -1 if
    //! node is root.
    std::ptrdiff_t index() const
    {
        if (m_parent == nullptr)
            return -1;

        auto &siblings = m_parent->m_children;
        auto it = std::find_if(
            siblings.begin(),
            siblings.end(),
            [this](const RoseNode &node){ return &node == this; });

        return it - siblings.begin();
    }

    //! Returns a string describing the path to this node from the root node.
    std::string path()
    {
        if (m_parent == nullptr)
            return "/ " + description();
        else
            return m_parent->path() + " / " + description();
    }

    //! Returns the active generator.
    UntypedGenerator *activeGenerator() const
    {
        if (m_shrunkGenerator)
            return m_shrunkGenerator.get();
        else if (m_acceptedGenerator)
            return m_acceptedGenerator.get();
        else if (m_lastGenerator)
            return m_lastGenerator.get();
        else
            return nullptr;
    }

    //! Returns the name of the active generator
    std::string generatorName() const
    {
        auto gen = activeGenerator();
        if (gen == nullptr)
            return std::string();
        else
            return demangle(typeid(*gen).name());
    }

    //! Returns the active generator cast to a generator of the given type or
    //! \c default if there is none or if there is a type mismatch.
    template<typename T>
    T regenerate()
    {
        ImplicitParam<CurrentNode> currentNode;
        currentNode.let(this);
        ImplicitParam<NextChildIndex> nextChildIndex;
        nextChildIndex.let(0);
        return (*dynamic_cast<Generator<T> *>(activeGenerator()))();
    }

    //! Accepts the current shrink value
    void acceptShrink()
    {
        if (!m_shrunkGenerator)
            return;
        m_acceptedGenerator = std::move(m_shrunkGenerator);
        m_shrinkIterator = nullptr;
    }

    typedef std::vector<RoseNode> Children;
    RoseNode *m_parent;
    Children m_children;
    bool m_hasAtom = false;
    RandomEngine::Atom m_atom;
    UntypedGeneratorUP m_lastGenerator;
    UntypedGeneratorUP m_acceptedGenerator;
    UntypedGeneratorUP m_shrunkGenerator;
    UntypedShrinkIteratorUP m_shrinkIterator;
};

} // namespace detail
} // namespace rc
<commit_msg>Some refactoring in RoseNode<commit_after>#pragma once

#include <sstream>

#include "rapidcheck/Show.hpp"

#include "ImplicitParam.hpp"
#include "RandomEngine.hpp"

namespace rc {
namespace detail {

//! Represents the structure of value generation where large complex values are
//! generated from small simple values. This also means that large values often
//! can be shrunk by shrinking the small values individually.
class RoseNode
{
public:
    //! Constructs a new root \c RoseNode.
    RoseNode() : RoseNode(nullptr) {}

    //! Returns an atom. If one has already been generated, it's reused. If not,
    //! a new one is generated.
    RandomEngine::Atom atom()
    {
        if (!m_hasAtom) {
            ImplicitParam<param::RandomEngine> randomEngine;
            m_atom = randomEngine->nextAtom();
            m_hasAtom = true;
        }

        return m_atom;
    }

    //! Outputs a string representation of this node and all its children.
    void print(std::ostream &os)
    {
        for (int i = 0; i < depth(); i++)
            os << "  ";
        os << "- " << description() << std::endl;

        for (auto &child : m_children)
            child.print(os);
    }

    //! Generates a value in this node using the given generator.
    template<typename Gen>
    typename Gen::GeneratedType generate(const Gen &generator)
    {
        typedef typename Gen::GeneratedType T;
        ImplicitParam<ShrunkNode> shrunkNode;

        if (!isFrozen())
            m_originalGenerator = UntypedGeneratorUP(new Gen(generator));

        if (shrunkNode.hasBinding() && (*shrunkNode == nullptr)) {
            if (!m_shrinkIterator) {
                T value(regenerate<T>());
                // Did children shrink before us?
                if (*shrunkNode != nullptr)
                    return value;

                m_shrinkIterator = generator.shrink(value);
                // We need a fallback accepted generator if shrinking fails
                if (!m_acceptedGenerator)
                    m_acceptedGenerator = UntypedGeneratorUP(new Gen(generator));
            }

            if (m_shrinkIterator->hasNext()) {
                auto typedIterator =
                    dynamic_cast<ShrinkIterator<T> *>(m_shrinkIterator.get());
                assert(typedIterator != nullptr);
                m_currentGenerator = UntypedGeneratorUP(
                    new Constant<T>(typedIterator->next()));
                *shrunkNode = this;
            } else {
                // Shrinking exhausted
                m_currentGenerator = nullptr;
            }
        }

        return regenerate<T>();
    }

    //! Picks a value using the given generator in the context of the current
    //! node.
    template<typename Gen>
    typename Gen::GeneratedType pick(const Gen &generator)
    {
        ImplicitParam<NextChildIndex> nextChildIndex;
        if (*nextChildIndex >= m_children.size())
            m_children.push_back(RoseNode(this));
        (*nextChildIndex)++;
        return m_children[*nextChildIndex - 1].generate(generator);
    }

    //! Returns a list of \c ValueDescriptions from the immediate children of
    //! this node.
    std::vector<std::string> example()
    {
        std::vector<std::string> values;
        values.reserve(m_children.size());
        for (auto &child : m_children)
            values.push_back(child.stringValue());
        return values;
    }

    //! Returns a string representation of the value of this node or an empty
    //! if one hasn't been decided.
    std::string stringValue()
    {
        ImplicitParam<CurrentNode> currentNode;
        currentNode.let(this);
        ImplicitParam<NextChildIndex> nextChildIndex;
        nextChildIndex.let(0);

        UntypedGenerator *generator = activeGenerator();
        if (generator != nullptr)
            return generator->generateString();
        else
            return std::string();
    }

    //! Tries to find an immediate shrink that yields \c false for the given
    //! generator.
    //!
    //! @return  A tuple where the first value tells whether the shrinking was
    //!          successful and the second how many shrinks were tried,
    //!          regardless of success.
    template<typename Gen>
    std::tuple<bool, int> shrink(const Gen &generator)
    {
        ImplicitParam<ShrunkNode> shrunkNode;
        int numTries = 0;
        bool result = true;
        while (result) {
            numTries++;
            shrunkNode.let(nullptr);
            result = generate(generator);
            if (*shrunkNode == nullptr)
                return std::make_tuple(false, numTries);
        }

        (*shrunkNode)->acceptShrink();
        return std::make_tuple(true, numTries);
    }

    //! Returns true if this node is frozen. If a node is frozen, the generator
    //! passed to \c generate will only be used to infer the type, the actual
    //! value will come from shrinking or similar.
    bool isFrozen() const
    {
        return bool(m_acceptedGenerator);
    }

    //! Move constructor.
    RoseNode(RoseNode &&other)
        : m_parent(other.m_parent)
        , m_children(std::move(other.m_children))
        , m_hasAtom(other.m_hasAtom)
        , m_atom(other.m_atom)
        , m_originalGenerator(std::move(other.m_originalGenerator))
        , m_acceptedGenerator(std::move(other.m_acceptedGenerator))
        , m_currentGenerator(std::move(other.m_currentGenerator))
        , m_shrinkIterator(std::move(other.m_shrinkIterator))
    {
        adoptChildren();
    }

    //! Move assignment
    RoseNode &operator=(RoseNode &&rhs)
    {
        m_parent = rhs.m_parent;
        m_children = std::move(rhs.m_children);
        m_hasAtom = rhs.m_hasAtom;
        m_atom = rhs.m_atom;
        m_originalGenerator = std::move(rhs.m_originalGenerator);
        m_acceptedGenerator = std::move(rhs.m_acceptedGenerator);
        m_currentGenerator = std::move(rhs.m_currentGenerator);
        m_shrinkIterator = std::move(rhs.m_shrinkIterator);
        adoptChildren();
        return *this;
    }

    void printExample()
    {
        for (const auto &desc : example())
            std::cout << desc << std::endl;
    }

    //! Returns a reference to the current node.
    static RoseNode &current()
    { return **ImplicitParam<CurrentNode>(); }

private:
    RC_DISABLE_COPY(RoseNode)

    // Implicit parameters, see ImplicitParam
    struct CurrentNode { typedef RoseNode *ValueType; };
    struct NextChildIndex { typedef size_t ValueType; };
    struct ShrunkNode { typedef RoseNode *ValueType; };

    //! Constructs a new \c RoseNode with the given parent or \c 0 if it should
    //! have no parent, i.e. is root.
    explicit RoseNode(RoseNode *parent) : m_parent(parent) {}

    //! Returns the depth of this node.
    int depth() const
    {
        if (m_parent == nullptr)
            return 0;

        return m_parent->depth() + 1;
    }

    //! Sets the parent of all children to this node.
    void adoptChildren()
    {
        for (auto &child : m_children)
            child.m_parent = this;
    }

    //! Returns a description of this node.
    std::string description() const
    {
        return generatorName();
    }

    //! Returns the index of this node among its sibilings. Returns \c -1 if
    //! node is root.
    std::ptrdiff_t index() const
    {
        if (m_parent == nullptr)
            return -1;

        auto &siblings = m_parent->m_children;
        auto it = std::find_if(
            siblings.begin(),
            siblings.end(),
            [this](const RoseNode &node){ return &node == this; });

        return it - siblings.begin();
    }

    //! Returns a string describing the path to this node from the root node.
    std::string path()
    {
        if (m_parent == nullptr)
            return "/ " + description();
        else
            return m_parent->path() + " / " + description();
    }

    //! Returns the active generator.
    UntypedGenerator *activeGenerator() const
    {
        if (m_currentGenerator)
            return m_currentGenerator.get();
        else if (m_acceptedGenerator)
            return m_acceptedGenerator.get();
        else if (m_originalGenerator)
            return m_originalGenerator.get();
        else
            return nullptr;
    }

    //! Returns the name of the active generator
    std::string generatorName() const
    {
        auto gen = activeGenerator();
        if (gen == nullptr)
            return std::string();
        else
            return demangle(typeid(*gen).name());
    }

    //! Returns the active generator cast to a generator of the given type or
    //! \c default if there is none or if there is a type mismatch.
    template<typename T>
    T regenerate()
    {
        ImplicitParam<CurrentNode> currentNode;
        currentNode.let(this);
        ImplicitParam<NextChildIndex> nextChildIndex;
        nextChildIndex.let(0);
        return (*dynamic_cast<Generator<T> *>(activeGenerator()))();
    }

    //! Accepts the current shrink value
    void acceptShrink()
    {
        if (!m_currentGenerator)
            return;
        m_acceptedGenerator = std::move(m_currentGenerator);
        m_shrinkIterator = nullptr;
    }

    typedef std::vector<RoseNode> Children;
    RoseNode *m_parent;
    Children m_children;
    bool m_hasAtom = false;
    RandomEngine::Atom m_atom;
    UntypedGeneratorUP m_originalGenerator;
    UntypedGeneratorUP m_acceptedGenerator;
    UntypedGeneratorUP m_currentGenerator;
    UntypedShrinkIteratorUP m_shrinkIterator;
};

} // namespace detail
} // namespace rc
<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- *
 *                     OpenSim:  exampleHopperDevice.cpp                      *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2016 Stanford University and the Authors                *
 * Author(s): Chris Dembia, Shrinidhi K. Lakshmikanth, Ajay Seth,             *
 *            Thomas Uchida                                                   *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

/* This example demonstrates some of the new features of the OpenSim 4.0 API.
The Component architecture allows us to join sub-assemblies to form larger
Models, with information flowing between Components via Inputs, Outputs, and
Connectors. For more information, please refer to the Component documentation.

This interactive example consists of three steps:
  Step 1. Build and simulate a single-legged hopping mechanism.
  Step 2. Build an assistive device and test it on a simple testbed.
  Step 3. Connect the device to the hopper to increase hop height.

To start working through this example, go to run() at the bottom of this file.
From there, you will be directed to specific files and methods in this project
that need to be completed. Now, hop to it! */

#include <OpenSim/OpenSim.h>
#include "defineDeviceAndController_answers.h"
#include "helperMethods.h"

static const double SIGNAL_GEN_CONSTANT{ 0.33 };
static const double REPORTING_INTERVAL{ 0.2 };

static const std::string testbedAttachment1{"ground"};
static const std::string testbedAttachment2{"load"};

//TODO: Provide the name of the coordinate corresponding to the hopper's height.
//      Hint: the hopper's pelvis is attached to ground with a vertical slider
//      joint; see buildHopperModel.cpp and showAllOutputs() in helperMethods.h.
// [Step 1, Task A]
//static const std::string hopperHeightCoord{"/Dennis/?????"}; //fill this in
#pragma region Step1_TaskA_solution

static const std::string hopperHeightCoord{"/Dennis/slider/yCoord"};

#pragma endregion

//TODO: Provide the full path names of the PhysicalOffsetFrames defined on the
//      hopper for attaching the assistive device. See buildHopperModel.cpp and
//      showSubcomponentInfo() in helperMethods.h.
// [Step 3, Task A]
//static const std::string thighAttachment{"/Dennis/?????"}; //fill this in
//static const std::string shankAttachment{"/Dennis/?????"}; //fill this in
#pragma region Step3_TaskA_solution

static const std::string thighAttachment{"/Dennis/thigh/deviceAttachmentPoint"};
static const std::string shankAttachment{"/Dennis/shank/deviceAttachmentPoint"};

#pragma endregion

//TODO: To assist hopping, we will activate the knee device whenever the vastus
//      muscle is active. For convenience, we define a string "vastus" which
//      is the path to the vastus muscle.
// [Step 3, Task B]
//static const std::string vastus{"/Dennis/?????"}; //fill this in
#pragma region Step3_TaskB_solution

static const std::string vastus{"/Dennis/vastus"};

#pragma endregion

namespace OpenSim {

// Forward declarations for methods used below.
Model buildHopper();    //defined in buildHopperModel.cpp
Model buildTestbed();   //defined in helperMethods.h
Device* buildDevice();  //defined in buildDevice.cpp

//------------------------------------------------------------------------------
// Attach the device to any two PhysicalFrames in a model.
// [Step 2, Task D]
//------------------------------------------------------------------------------
void connectDeviceToModel(OpenSim::Device& device, OpenSim::Model& model,
    const std::string& modelFrameAname, const std::string& modelFrameBname)
{
    //TODO: Get writable references to the "anchor" joints in the device.
    #pragma region Step2_TaskD_solution

    auto& anchorA = device.updComponent<WeldJoint>("anchorA");
    auto& anchorB = device.updComponent<WeldJoint>("anchorB");

    #pragma endregion

    //TODO: Recall that the child frame of each anchor (WeldJoint) was attached
    //      to the corresponding cuff. We will now attach the parent frames of
    //      the anchors to modelFrameA and modelFrameB. First get references to
    //      the two specified PhysicalFrames in model (i.e., modelFrameAname and
    //      modelFrameBname), then connect them to the parent frames of each
    //      anchor. (2 lines of code for each anchor.)
    #pragma region Step2_TaskD_solution

    const auto& frameA = model.getComponent<PhysicalFrame>(modelFrameAname);
    anchorA.updConnector("parent_frame").connect(frameA);
    const auto& frameB = model.getComponent<PhysicalFrame>(modelFrameBname);
    anchorB.updConnector("parent_frame").connect(frameB);

    #pragma endregion

    // Add the device to the model. We need to add the device using
    // addModelComponent() rather than addComponent() because of a bug in
    // Model::initSystem().
    model.addModelComponent(&device);

    // Configure the device to wrap over the patella (if one exists; there is no
    // patella in the testbed).
    if (model.hasComponent<WrapCylinder>("thigh/patella")) {
        auto& cable = model.updComponent<PathActuator>("device/cableAtoB");
        auto& frame = model.updComponent<PhysicalFrame>("thigh");
        auto& wrapObject = frame.upd_WrapObjectSet().get("patella");
        cable.updGeometryPath().addPathWrap(wrapObject);
    }
}

//------------------------------------------------------------------------------
// Add a ConsoleReporter to the hopper model to display variables of interest.
// [Step 1, Task B]
//------------------------------------------------------------------------------
void addConsoleReporterToHopper(Model& hopper)
{
    //TODO: Create a new ConsoleReporter. Set its name and reporting interval.
    #pragma region Step1_TaskB_solution

    auto reporter = new ConsoleReporter();
    reporter->setName("hopper_results");
    reporter->set_report_time_interval(REPORTING_INTERVAL);

    #pragma endregion

    //TODO: Connect outputs from the hopper to the reporter's inputs. Try
    //      reporting the hopper's height, the vastus muscle's activation, the
    //      knee angle, and any other variables of interest.
    #pragma region Step1_TaskB_solution

    reporter->updInput("inputs").connect(
        hopper.getComponent(hopperHeightCoord).getOutput("value"), "height");

    #pragma endregion
    #pragma region Step1_TaskB_solution

    reporter->updInput("inputs").connect(
        hopper.getComponent("/Dennis/vastus").getOutput("activation"));

    #pragma endregion
    #pragma region Step1_TaskB_solution

    reporter->updInput("inputs").connect(
        hopper.getComponent("/Dennis/knee/kneeFlexion").getOutput("value"), "knee_angle");

    #pragma endregion

    //TODO: Add the reporter to the model.
    #pragma region Step1_TaskB_solution

    hopper.addComponent(reporter);

    #pragma endregion
}

//------------------------------------------------------------------------------
// Add a SignalGenerator to a device (the SignalGenerator class is defined in
// helperMethods.h).
// [Step 2, Task E]
//------------------------------------------------------------------------------
void addSignalGeneratorToDevice(Device& device)
{
    //TODO: Create a new SignalGenerator and set its name.
    #pragma region Step2_TaskE_solution

    auto signalGen = new SignalGenerator();
    signalGen->setName("signalGen");

    #pragma endregion

    // Try changing the constant value and/or the function (e.g., try a
    // LinearFunction).
    signalGen->set_function(Constant(SIGNAL_GEN_CONSTANT));
    device.addComponent(signalGen);

    //TODO: Connect the signal generator's output signal to the controller's
    //      activation input.
    #pragma region Step2_TaskE_solution

    device.updComponent("controller").updInput("activation")
        .connect(signalGen->getOutput("signal"));

    #pragma endregion
}

//------------------------------------------------------------------------------
// Add a ConsoleReporter to a model for displaying outputs from a device.
//------------------------------------------------------------------------------
void addDeviceConsoleReporterToModel(Model& model, Device& device,
    const std::vector<std::string>& deviceOutputs,
    const std::vector<std::string>& deviceControllerOutputs)
{
    // Create a new ConsoleReporter. Set its name and reporting interval.
    auto reporter = new ConsoleReporter();
    reporter->setName(model.getName() + "_" + device.getName() + "_results");
    reporter->set_report_time_interval(REPORTING_INTERVAL);

    // Loop through the desired device outputs and add them to the reporter.
    for (auto thisOutputName : deviceOutputs)
        reporter->updInput("inputs").connect(device.getOutput(thisOutputName));

    for (auto thisOutputName : deviceControllerOutputs)
        reporter->updInput("inputs").
            connect(device.getComponent("controller").getOutput(thisOutputName));

    // Add the reporter to the model.
    model.addComponent(reporter);
}

} // namespace OpenSim

void run();

int main()
{
    try {
        run();
    }
    catch (const std::exception& ex) {
        std::cout << "Hopper Example Failed to run due to the following Exception: "
            << ex.what() << std::endl;
        return 1;
    }

    return 0;
}

//------------------------------------------------------------------------------
// START HERE! Toggle "if (false)" to "if (true)" to enable/disable each step in
// the exercise. The project should execute without making any changes (you
// should see the unassisted hopper hop slightly).
//------------------------------------------------------------------------------
void run()
{
    using namespace OpenSim;

    //==========================================================================
    // Step 1. Build and simulate a single-legged hopping mechanism.
    //==========================================================================
    if (true)
    {
        // Build the hopper.
        auto hopper = buildHopper();
        // Update the hopper model's internal data members, which includes
        // identifying its subcomponents from its properties.
        hopper.finalizeFromProperties();

        // Show all Components in the model.
        showSubcomponentInfo(hopper);
        // Show only the Joints in the model.
        showSubcomponentInfo<Joint>(hopper);
        // Show the outputs generated by the thigh body.
        showAllOutputs(hopper.getComponent("/Dennis/thigh"), false);

        // Step 1, Task A
        // ==============
        // Determine the name of the coordinate corresponding to the hopper's
        // height. The hopperHeightCoord string (at the top of this file) must
        // be filled in.

        // Step 1, Task B
        // ==============
        // Report the hopper's height and vastus muscle activation during the
        // simulation. The addConsoleReporterToHopper() method (in this file)
        // needs to be filled in.
        addConsoleReporterToHopper(hopper);

        // Create the system, initialize the state, and simulate.
        SimTK::State& sHop = hopper.initSystem();
        simulate(hopper, sHop);
    }

    //==========================================================================
    // Step 2. Build an assistive device and test it on a simple testbed.
    //==========================================================================
    if (true)
    {
        // Build the testbed and device.
        auto testbed = buildTestbed();
        testbed.finalizeFromProperties();

        // Step 2, Task A
        // ==============
        // Go to defineDeviceAndController.h and complete the "TODO"s in the
        // Device class.

        // Step 2, Task B
        // ==============
        // Go to defineDeviceAndController.h and complete the "TODO"s in the
        // PropMyoController class.

        // Step 2, Task C
        // ==============
        // Go to buildDeviceModel.cpp and complete the "TODO"s in buildDevice().
        auto device = buildDevice();
        device->finalizeFromProperties();

        // Show all Components in the device and testbed.
        showSubcomponentInfo(*device);
        showSubcomponentInfo(testbed);
        // Show the outputs generated by the device.
        showAllOutputs(*device, false);

        // Step 2, Task D
        // ==============
        // Connect the device to the testbed. The connectDeviceToModel() method
        // (in this file) needs to be filled in.
        connectDeviceToModel(*device, testbed, testbedAttachment1,
                             testbedAttachment2);

        // Step 2, Task E
        // ==============
        // Use a SignalGenerator to create a control signal for testing the
        // device. The addSignalGeneratorToDevice() method (in this file) needs
        // to be filled in.
        addSignalGeneratorToDevice(*device);

        // List the device outputs we wish to display during the simulation.
        std::vector<std::string> deviceOutputs{ "length", "tension", "power" };
        std::vector<std::string> deviceControllerOutputs{ "myo_control" };

        // Add a ConsoleReporter to report deviceOutputs.
        addDeviceConsoleReporterToModel(testbed, *device, deviceOutputs,
            deviceControllerOutputs);

        // Create the system, initialize the state, and simulate.
        SimTK::State& sDev = testbed.initSystem();
        simulate(testbed, sDev);
    }

    //==========================================================================
    // Step 3. Connect the device to the hopper to increase hop height.
    //==========================================================================
    if (true)
    {
        // Build the hopper and device.
        auto assistedHopper = buildHopper();
        assistedHopper.finalizeFromProperties();
        auto kneeDevice = buildDevice();
        kneeDevice->finalizeFromProperties();

        // Step 3, Task A
        // ==============
        // Connect the device to the hopper. The thighAttachment and
        // shankAttachment strings (at the top of this file) must be filled in.
        connectDeviceToModel(*kneeDevice, assistedHopper, thighAttachment,
                             shankAttachment);

        // Step 3, Task B
        // ==============
        // Use the vastus muscle's activation as the control signal for the
        // device. The vastus string (at the top of this file) must
        // be filled in.
        kneeDevice->updComponent("controller").updInput("activation")
            .connect(assistedHopper.getComponent(vastus).getOutput("activation"));

        // List the device outputs we wish to display during the simulation.
        std::vector<std::string> kneeDeviceOutputs{ "tension", "height" };
        std::vector<std::string> deviceControllerOutputs{ "myo_control" };

        // Add a ConsoleReporter to report deviceOutputs.
        addDeviceConsoleReporterToModel(assistedHopper, *kneeDevice,
                                        kneeDeviceOutputs, deviceControllerOutputs);

        // Create the system, initialize the state, and simulate.
        SimTK::State& sHD = assistedHopper.initSystem();
        simulate(assistedHopper, sHD);
    }

    return 0;
};
<commit_msg>removed return value from example's void run() method.<commit_after>/* -------------------------------------------------------------------------- *
 *                     OpenSim:  exampleHopperDevice.cpp                      *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2016 Stanford University and the Authors                *
 * Author(s): Chris Dembia, Shrinidhi K. Lakshmikanth, Ajay Seth,             *
 *            Thomas Uchida                                                   *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

/* This example demonstrates some of the new features of the OpenSim 4.0 API.
The Component architecture allows us to join sub-assemblies to form larger
Models, with information flowing between Components via Inputs, Outputs, and
Connectors. For more information, please refer to the Component documentation.

This interactive example consists of three steps:
  Step 1. Build and simulate a single-legged hopping mechanism.
  Step 2. Build an assistive device and test it on a simple testbed.
  Step 3. Connect the device to the hopper to increase hop height.

To start working through this example, go to run() at the bottom of this file.
From there, you will be directed to specific files and methods in this project
that need to be completed. Now, hop to it! */

#include <OpenSim/OpenSim.h>
#include "defineDeviceAndController_answers.h"
#include "helperMethods.h"

static const double SIGNAL_GEN_CONSTANT{ 0.33 };
static const double REPORTING_INTERVAL{ 0.2 };

static const std::string testbedAttachment1{"ground"};
static const std::string testbedAttachment2{"load"};

//TODO: Provide the name of the coordinate corresponding to the hopper's height.
//      Hint: the hopper's pelvis is attached to ground with a vertical slider
//      joint; see buildHopperModel.cpp and showAllOutputs() in helperMethods.h.
// [Step 1, Task A]
//static const std::string hopperHeightCoord{"/Dennis/?????"}; //fill this in
#pragma region Step1_TaskA_solution

static const std::string hopperHeightCoord{"/Dennis/slider/yCoord"};

#pragma endregion

//TODO: Provide the full path names of the PhysicalOffsetFrames defined on the
//      hopper for attaching the assistive device. See buildHopperModel.cpp and
//      showSubcomponentInfo() in helperMethods.h.
// [Step 3, Task A]
//static const std::string thighAttachment{"/Dennis/?????"}; //fill this in
//static const std::string shankAttachment{"/Dennis/?????"}; //fill this in
#pragma region Step3_TaskA_solution

static const std::string thighAttachment{"/Dennis/thigh/deviceAttachmentPoint"};
static const std::string shankAttachment{"/Dennis/shank/deviceAttachmentPoint"};

#pragma endregion

//TODO: To assist hopping, we will activate the knee device whenever the vastus
//      muscle is active. For convenience, we define a string "vastus" which
//      is the path to the vastus muscle.
// [Step 3, Task B]
//static const std::string vastus{"/Dennis/?????"}; //fill this in
#pragma region Step3_TaskB_solution

static const std::string vastus{"/Dennis/vastus"};

#pragma endregion

namespace OpenSim {

// Forward declarations for methods used below.
Model buildHopper();    //defined in buildHopperModel.cpp
Model buildTestbed();   //defined in helperMethods.h
Device* buildDevice();  //defined in buildDevice.cpp

//------------------------------------------------------------------------------
// Attach the device to any two PhysicalFrames in a model.
// [Step 2, Task D]
//------------------------------------------------------------------------------
void connectDeviceToModel(OpenSim::Device& device, OpenSim::Model& model,
    const std::string& modelFrameAname, const std::string& modelFrameBname)
{
    //TODO: Get writable references to the "anchor" joints in the device.
    #pragma region Step2_TaskD_solution

    auto& anchorA = device.updComponent<WeldJoint>("anchorA");
    auto& anchorB = device.updComponent<WeldJoint>("anchorB");

    #pragma endregion

    //TODO: Recall that the child frame of each anchor (WeldJoint) was attached
    //      to the corresponding cuff. We will now attach the parent frames of
    //      the anchors to modelFrameA and modelFrameB. First get references to
    //      the two specified PhysicalFrames in model (i.e., modelFrameAname and
    //      modelFrameBname), then connect them to the parent frames of each
    //      anchor. (2 lines of code for each anchor.)
    #pragma region Step2_TaskD_solution

    const auto& frameA = model.getComponent<PhysicalFrame>(modelFrameAname);
    anchorA.updConnector("parent_frame").connect(frameA);
    const auto& frameB = model.getComponent<PhysicalFrame>(modelFrameBname);
    anchorB.updConnector("parent_frame").connect(frameB);

    #pragma endregion

    // Add the device to the model. We need to add the device using
    // addModelComponent() rather than addComponent() because of a bug in
    // Model::initSystem().
    model.addModelComponent(&device);

    // Configure the device to wrap over the patella (if one exists; there is no
    // patella in the testbed).
    if (model.hasComponent<WrapCylinder>("thigh/patella")) {
        auto& cable = model.updComponent<PathActuator>("device/cableAtoB");
        auto& frame = model.updComponent<PhysicalFrame>("thigh");
        auto& wrapObject = frame.upd_WrapObjectSet().get("patella");
        cable.updGeometryPath().addPathWrap(wrapObject);
    }
}

//------------------------------------------------------------------------------
// Add a ConsoleReporter to the hopper model to display variables of interest.
// [Step 1, Task B]
//------------------------------------------------------------------------------
void addConsoleReporterToHopper(Model& hopper)
{
    //TODO: Create a new ConsoleReporter. Set its name and reporting interval.
    #pragma region Step1_TaskB_solution

    auto reporter = new ConsoleReporter();
    reporter->setName("hopper_results");
    reporter->set_report_time_interval(REPORTING_INTERVAL);

    #pragma endregion

    //TODO: Connect outputs from the hopper to the reporter's inputs. Try
    //      reporting the hopper's height, the vastus muscle's activation, the
    //      knee angle, and any other variables of interest.
    #pragma region Step1_TaskB_solution

    reporter->updInput("inputs").connect(
        hopper.getComponent(hopperHeightCoord).getOutput("value"), "height");

    #pragma endregion
    #pragma region Step1_TaskB_solution

    reporter->updInput("inputs").connect(
        hopper.getComponent("/Dennis/vastus").getOutput("activation"));

    #pragma endregion
    #pragma region Step1_TaskB_solution

    reporter->updInput("inputs").connect(
        hopper.getComponent("/Dennis/knee/kneeFlexion").getOutput("value"), "knee_angle");

    #pragma endregion

    //TODO: Add the reporter to the model.
    #pragma region Step1_TaskB_solution

    hopper.addComponent(reporter);

    #pragma endregion
}

//------------------------------------------------------------------------------
// Add a SignalGenerator to a device (the SignalGenerator class is defined in
// helperMethods.h).
// [Step 2, Task E]
//------------------------------------------------------------------------------
void addSignalGeneratorToDevice(Device& device)
{
    //TODO: Create a new SignalGenerator and set its name.
    #pragma region Step2_TaskE_solution

    auto signalGen = new SignalGenerator();
    signalGen->setName("signalGen");

    #pragma endregion

    // Try changing the constant value and/or the function (e.g., try a
    // LinearFunction).
    signalGen->set_function(Constant(SIGNAL_GEN_CONSTANT));
    device.addComponent(signalGen);

    //TODO: Connect the signal generator's output signal to the controller's
    //      activation input.
    #pragma region Step2_TaskE_solution

    device.updComponent("controller").updInput("activation")
        .connect(signalGen->getOutput("signal"));

    #pragma endregion
}

//------------------------------------------------------------------------------
// Add a ConsoleReporter to a model for displaying outputs from a device.
//------------------------------------------------------------------------------
void addDeviceConsoleReporterToModel(Model& model, Device& device,
    const std::vector<std::string>& deviceOutputs,
    const std::vector<std::string>& deviceControllerOutputs)
{
    // Create a new ConsoleReporter. Set its name and reporting interval.
    auto reporter = new ConsoleReporter();
    reporter->setName(model.getName() + "_" + device.getName() + "_results");
    reporter->set_report_time_interval(REPORTING_INTERVAL);

    // Loop through the desired device outputs and add them to the reporter.
    for (auto thisOutputName : deviceOutputs)
        reporter->updInput("inputs").connect(device.getOutput(thisOutputName));

    for (auto thisOutputName : deviceControllerOutputs)
        reporter->updInput("inputs").
            connect(device.getComponent("controller").getOutput(thisOutputName));

    // Add the reporter to the model.
    model.addComponent(reporter);
}

} // namespace OpenSim

void run();

int main()
{
    try {
        run();
    }
    catch (const std::exception& ex) {
        std::cout << "Hopper Example Failed to run due to the following Exception: "
            << ex.what() << std::endl;
        return 1;
    }

    return 0;
}

//------------------------------------------------------------------------------
// START HERE! Toggle "if (false)" to "if (true)" to enable/disable each step in
// the exercise. The project should execute without making any changes (you
// should see the unassisted hopper hop slightly).
//------------------------------------------------------------------------------
void run()
{
    using namespace OpenSim;

    //==========================================================================
    // Step 1. Build and simulate a single-legged hopping mechanism.
    //==========================================================================
    if (true)
    {
        // Build the hopper.
        auto hopper = buildHopper();
        // Update the hopper model's internal data members, which includes
        // identifying its subcomponents from its properties.
        hopper.finalizeFromProperties();

        // Show all Components in the model.
        showSubcomponentInfo(hopper);
        // Show only the Joints in the model.
        showSubcomponentInfo<Joint>(hopper);
        // Show the outputs generated by the thigh body.
        showAllOutputs(hopper.getComponent("/Dennis/thigh"), false);

        // Step 1, Task A
        // ==============
        // Determine the name of the coordinate corresponding to the hopper's
        // height. The hopperHeightCoord string (at the top of this file) must
        // be filled in.

        // Step 1, Task B
        // ==============
        // Report the hopper's height and vastus muscle activation during the
        // simulation. The addConsoleReporterToHopper() method (in this file)
        // needs to be filled in.
        addConsoleReporterToHopper(hopper);

        // Create the system, initialize the state, and simulate.
        SimTK::State& sHop = hopper.initSystem();
        simulate(hopper, sHop);
    }

    //==========================================================================
    // Step 2. Build an assistive device and test it on a simple testbed.
    //==========================================================================
    if (true)
    {
        // Build the testbed and device.
        auto testbed = buildTestbed();
        testbed.finalizeFromProperties();

        // Step 2, Task A
        // ==============
        // Go to defineDeviceAndController.h and complete the "TODO"s in the
        // Device class.

        // Step 2, Task B
        // ==============
        // Go to defineDeviceAndController.h and complete the "TODO"s in the
        // PropMyoController class.

        // Step 2, Task C
        // ==============
        // Go to buildDeviceModel.cpp and complete the "TODO"s in buildDevice().
        auto device = buildDevice();
        device->finalizeFromProperties();

        // Show all Components in the device and testbed.
        showSubcomponentInfo(*device);
        showSubcomponentInfo(testbed);
        // Show the outputs generated by the device.
        showAllOutputs(*device, false);

        // Step 2, Task D
        // ==============
        // Connect the device to the testbed. The connectDeviceToModel() method
        // (in this file) needs to be filled in.
        connectDeviceToModel(*device, testbed, testbedAttachment1,
                             testbedAttachment2);

        // Step 2, Task E
        // ==============
        // Use a SignalGenerator to create a control signal for testing the
        // device. The addSignalGeneratorToDevice() method (in this file) needs
        // to be filled in.
        addSignalGeneratorToDevice(*device);

        // List the device outputs we wish to display during the simulation.
        std::vector<std::string> deviceOutputs{ "length", "tension", "power" };
        std::vector<std::string> deviceControllerOutputs{ "myo_control" };

        // Add a ConsoleReporter to report deviceOutputs.
        addDeviceConsoleReporterToModel(testbed, *device, deviceOutputs,
            deviceControllerOutputs);

        // Create the system, initialize the state, and simulate.
        SimTK::State& sDev = testbed.initSystem();
        simulate(testbed, sDev);
    }

    //==========================================================================
    // Step 3. Connect the device to the hopper to increase hop height.
    //==========================================================================
    if (true)
    {
        // Build the hopper and device.
        auto assistedHopper = buildHopper();
        assistedHopper.finalizeFromProperties();
        auto kneeDevice = buildDevice();
        kneeDevice->finalizeFromProperties();

        // Step 3, Task A
        // ==============
        // Connect the device to the hopper. The thighAttachment and
        // shankAttachment strings (at the top of this file) must be filled in.
        connectDeviceToModel(*kneeDevice, assistedHopper, thighAttachment,
                             shankAttachment);

        // Step 3, Task B
        // ==============
        // Use the vastus muscle's activation as the control signal for the
        // device. The vastus string (at the top of this file) must
        // be filled in.
        kneeDevice->updComponent("controller").updInput("activation")
            .connect(assistedHopper.getComponent(vastus).getOutput("activation"));

        // List the device outputs we wish to display during the simulation.
        std::vector<std::string> kneeDeviceOutputs{ "tension", "height" };
        std::vector<std::string> deviceControllerOutputs{ "myo_control" };

        // Add a ConsoleReporter to report deviceOutputs.
        addDeviceConsoleReporterToModel(assistedHopper, *kneeDevice,
                                        kneeDeviceOutputs, deviceControllerOutputs);

        // Create the system, initialize the state, and simulate.
        SimTK::State& sHD = assistedHopper.initSystem();
        simulate(assistedHopper, sHD);
    }
};
<|endoftext|>


<commit_before>/*
 * NMEA library. Encodes raw AIS data from FIFO and sends through UART
 * Author: Adrian Studer
 * License: CC BY-NC-SA Creative Commons Attribution-NonCommercial-ShareAlike
 * 			http://creativecommons.org/licenses/by-nc-sa/4.0/
 * 			Please contact the author if you want to use this work in a commercial product
 */

#include "Arduino.h"
#include "fifo.h"
#include "nmea.h"

void nmea_push_char(char c);
uint8_t nmea_push_packet(uint8_t packet_size);

#define NMEA_MAX_AIS_PAYLOAD 42		// number of AIS bytes per NMEA sentence, to keep total NMEA sentence always below 82 characters
#define NMEA_AIS_BITS (NMEA_MAX_AIS_PAYLOAD * 8)
#define NMEA_AIS_BITS_ENCODED ((NMEA_AIS_BITS + 5) / 6)

const char nmea_lead[] = "!AIVDM,";             // static start of NMEA sentence
char nmea_buffer[8+NMEA_AIS_BITS_ENCODED+5+1];  // buffer for dynamic part of NMEA sentence
                                                // fragment and channel info, AIS payload, stuff-bit and crc, 0-termination
uint8_t nmea_buffer_index;                      // current buffer position

#define NMEA_LEAD_CRC 'A' ^ 'I' ^ 'V' ^ 'D' ^ 'M' ^ ',' // CRC for static start of sentence
uint8_t nmea_crc; // calculated CRC

uint8_t nmea_message_id = 0; // sequential message id for multi-sentence message

const char nmea_hex[] = {
  '0', '1', '2', '3',		// lookup table for hex conversion of CRC
  '4', '5', '6', '7',
  '8', '9', 'A', 'B',
  'C', 'D', 'E', 'F'
};

// process AIS next packet in FIFO and transmit as NMEA sentence(s) through UART
void nmea_process_packet(void)
{
  uint16_t packet_size = fifo_get_packet();

  if (packet_size == 0 || packet_size < 4)    // check for empty packet
    return;                                 // no (valid) packet available in FIFO, nothing to send

  uint8_t radio_channel = fifo_read_byte() + 'A';	// retrieve radio channel (0=A, 1=B)

  // calculate number of fragments, NMEA allows 82 characters per sentence
  //			-> max 62 6-bit characters payload
  //			-> max 46 AIS bytes (368 bits) per sentence
  packet_size -= 3;							// Ignore channel information and AIS CRC
  uint8_t curr_fragment = 1;
  uint8_t total_fragments = 1;
  uint16_t packet_bits = packet_size * 8;
  while (packet_bits > NMEA_AIS_BITS) {
    packet_bits -= NMEA_AIS_BITS;
    total_fragments++;
  }

  // avoid sending garbage if fragment count does not make sense
  if (total_fragments > 9)
  {
    return;
  }

  // maintain message id if this is a multi-sentence message
  if (total_fragments > 1)
  {
    nmea_message_id++;
    if (nmea_message_id > 9)
      nmea_message_id = 1;		// keep message id < 10
  }

  // create fragments
  while (packet_size > 0) {
    // reset buffer and CRC
    nmea_buffer_index = 0;
    nmea_crc = NMEA_LEAD_CRC;

    // write fragment information, I assume total fragments always < 10
    nmea_push_char(total_fragments + '0');
    nmea_push_char(',');
    nmea_push_char(curr_fragment + '0');
    nmea_push_char(',');
    curr_fragment++;

    // write message id if there are multiple fragments
    if (total_fragments > 1)
    nmea_push_char(nmea_message_id + '0');
    nmea_push_char(',');

    // write channel information
    nmea_push_char(radio_channel);
    nmea_push_char(',');

    // encode and write next NMEA_MAX_AIS_PAYLOAD bytes from AIS packet
    uint8_t fragment_size = packet_size;
    if (fragment_size > NMEA_MAX_AIS_PAYLOAD)
    {
      fragment_size = NMEA_MAX_AIS_PAYLOAD;
    }
    uint8_t stuff_bits = nmea_push_packet(fragment_size);
    packet_size -= fragment_size;

    // write stuff bit
    nmea_push_char(',');
    nmea_push_char(stuff_bits + '0');

    // write CRC
    uint8_t final_crc = nmea_crc;		// copy CRC as push_char will modify it
    nmea_push_char('*');
    nmea_push_char(nmea_hex[final_crc >> 4]);
    nmea_push_char(nmea_hex[final_crc & 0x0f]);

    // terminate message with 0
    nmea_push_char(0);

    // send NMEA sentence over UART
    Serial.print(nmea_lead);
    Serial.println(nmea_buffer);
  }
}

// adds char to buffer and updates CRC
inline void nmea_push_char(char c)
{
  nmea_crc ^= c;
  nmea_buffer[nmea_buffer_index++] = c;
}

// encodes and adds AIS packet to buffer, returns # of stuff bits
uint8_t nmea_push_packet(uint8_t packet_size)
{
  uint8_t raw_byte;
  uint8_t raw_bit;

  uint8_t nmea_byte;
  uint8_t nmea_bit;

  nmea_byte = 0;
  nmea_bit = 6;

  while (packet_size != 0) {
    raw_byte = fifo_read_byte();
    raw_bit = 8;

    while (raw_bit > 0) {
      nmea_byte <<= 1;
      if (raw_byte & 0x80)
      nmea_byte |= 1;
      nmea_bit--;

      if (nmea_bit == 0) {
        if (nmea_byte > 39)
          nmea_byte += 8;
        nmea_byte += 48;
        nmea_push_char(nmea_byte);
        nmea_byte = 0;
        nmea_bit = 6;
      }

      raw_byte <<= 1;
      raw_bit--;
    }

    packet_size--;
  }

  // stuff unfinished NMEA character
  uint8_t stuff_bits = 0;
  if (nmea_bit != 6)
  {
    // stuff with 0 bits as needed
    while (nmea_bit != 0) {
      nmea_byte <<= 1;
      nmea_bit--;
      stuff_bits++;
    }

    // .. and convert and store last byte
    if (nmea_byte > 39)
      nmea_byte += 8;
    nmea_byte += 48;
    nmea_push_char(nmea_byte);
  }

  return stuff_bits;
}
<commit_msg>Remove inline<commit_after>/*
 * NMEA library. Encodes raw AIS data from FIFO and sends through UART
 * Author: Adrian Studer
 * License: CC BY-NC-SA Creative Commons Attribution-NonCommercial-ShareAlike
 * 			http://creativecommons.org/licenses/by-nc-sa/4.0/
 * 			Please contact the author if you want to use this work in a commercial product
 */

#include "Arduino.h"
#include "fifo.h"
#include "nmea.h"

void nmea_push_char(char c);
uint8_t nmea_push_packet(uint8_t packet_size);

#define NMEA_MAX_AIS_PAYLOAD 42		// number of AIS bytes per NMEA sentence, to keep total NMEA sentence always below 82 characters
#define NMEA_AIS_BITS (NMEA_MAX_AIS_PAYLOAD * 8)
#define NMEA_AIS_BITS_ENCODED ((NMEA_AIS_BITS + 5) / 6)

const char nmea_lead[] = "!AIVDM,";             // static start of NMEA sentence
char nmea_buffer[8+NMEA_AIS_BITS_ENCODED+5+1];  // buffer for dynamic part of NMEA sentence
                                                // fragment and channel info, AIS payload, stuff-bit and crc, 0-termination
uint8_t nmea_buffer_index;                      // current buffer position

#define NMEA_LEAD_CRC 'A' ^ 'I' ^ 'V' ^ 'D' ^ 'M' ^ ',' // CRC for static start of sentence
uint8_t nmea_crc; // calculated CRC

uint8_t nmea_message_id = 0; // sequential message id for multi-sentence message

const char nmea_hex[] = {
  '0', '1', '2', '3',		// lookup table for hex conversion of CRC
  '4', '5', '6', '7',
  '8', '9', 'A', 'B',
  'C', 'D', 'E', 'F'
};

// process AIS next packet in FIFO and transmit as NMEA sentence(s) through UART
void nmea_process_packet(void)
{
  uint16_t packet_size = fifo_get_packet();

  if (packet_size == 0 || packet_size < 4)    // check for empty packet
    return;                                 // no (valid) packet available in FIFO, nothing to send

  uint8_t radio_channel = fifo_read_byte() + 'A';	// retrieve radio channel (0=A, 1=B)

  // calculate number of fragments, NMEA allows 82 characters per sentence
  //			-> max 62 6-bit characters payload
  //			-> max 46 AIS bytes (368 bits) per sentence
  packet_size -= 3;							// Ignore channel information and AIS CRC
  uint8_t curr_fragment = 1;
  uint8_t total_fragments = 1;
  uint16_t packet_bits = packet_size * 8;
  while (packet_bits > NMEA_AIS_BITS) {
    packet_bits -= NMEA_AIS_BITS;
    total_fragments++;
  }

  // avoid sending garbage if fragment count does not make sense
  if (total_fragments > 9)
  {
    return;
  }

  // maintain message id if this is a multi-sentence message
  if (total_fragments > 1)
  {
    nmea_message_id++;
    if (nmea_message_id > 9)
      nmea_message_id = 1;		// keep message id < 10
  }

  // create fragments
  while (packet_size > 0) {
    // reset buffer and CRC
    nmea_buffer_index = 0;
    nmea_crc = NMEA_LEAD_CRC;

    // write fragment information, I assume total fragments always < 10
    nmea_push_char(total_fragments + '0');
    nmea_push_char(',');
    nmea_push_char(curr_fragment + '0');
    nmea_push_char(',');
    curr_fragment++;

    // write message id if there are multiple fragments
    if (total_fragments > 1)
    nmea_push_char(nmea_message_id + '0');
    nmea_push_char(',');

    // write channel information
    nmea_push_char(radio_channel);
    nmea_push_char(',');

    // encode and write next NMEA_MAX_AIS_PAYLOAD bytes from AIS packet
    uint8_t fragment_size = packet_size;
    if (fragment_size > NMEA_MAX_AIS_PAYLOAD)
    {
      fragment_size = NMEA_MAX_AIS_PAYLOAD;
    }
    uint8_t stuff_bits = nmea_push_packet(fragment_size);
    packet_size -= fragment_size;

    // write stuff bit
    nmea_push_char(',');
    nmea_push_char(stuff_bits + '0');

    // write CRC
    uint8_t final_crc = nmea_crc;		// copy CRC as push_char will modify it
    nmea_push_char('*');
    nmea_push_char(nmea_hex[final_crc >> 4]);
    nmea_push_char(nmea_hex[final_crc & 0x0f]);

    // terminate message with 0
    nmea_push_char(0);

    // send NMEA sentence over UART
    Serial.print(nmea_lead);
    Serial.println(nmea_buffer);
  }
}

// adds char to buffer and updates CRC
void nmea_push_char(char c)
{
  nmea_crc ^= c;
  nmea_buffer[nmea_buffer_index++] = c;
}

// encodes and adds AIS packet to buffer, returns # of stuff bits
uint8_t nmea_push_packet(uint8_t packet_size)
{
  uint8_t raw_byte;
  uint8_t raw_bit;

  uint8_t nmea_byte;
  uint8_t nmea_bit;

  nmea_byte = 0;
  nmea_bit = 6;

  while (packet_size != 0) {
    raw_byte = fifo_read_byte();
    raw_bit = 8;

    while (raw_bit > 0) {
      nmea_byte <<= 1;
      if (raw_byte & 0x80)
      nmea_byte |= 1;
      nmea_bit--;

      if (nmea_bit == 0) {
        if (nmea_byte > 39)
          nmea_byte += 8;
        nmea_byte += 48;
        nmea_push_char(nmea_byte);
        nmea_byte = 0;
        nmea_bit = 6;
      }

      raw_byte <<= 1;
      raw_bit--;
    }

    packet_size--;
  }

  // stuff unfinished NMEA character
  uint8_t stuff_bits = 0;
  if (nmea_bit != 6)
  {
    // stuff with 0 bits as needed
    while (nmea_bit != 0) {
      nmea_byte <<= 1;
      nmea_bit--;
      stuff_bits++;
    }

    // .. and convert and store last byte
    if (nmea_byte > 39)
      nmea_byte += 8;
    nmea_byte += 48;
    nmea_push_char(nmea_byte);
  }

  return stuff_bits;
}
<|endoftext|>