<commit_before>/* The libMesh Finite Element Library. */
/* Copyright (C) 2003  Benjamin S. Kirk */

/* This library is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU Lesser General Public */
/* License as published by the Free Software Foundation; either */
/* version 2.1 of the License, or (at your option) any later version. */

/* This library is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU */
/* Lesser General Public License for more details. */

/* You should have received a copy of the GNU Lesser General Public */
/* License along with this library; if not, write to the Free Software */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

 //
Miscellaneous Example 6 - Meshing with LibMesh's TetGen and Triangle Interfaces

 //
 // LibMesh provides interfaces to both Triangle and TetGen for generating
 // Delaunay triangulations and tetrahedralizations in two and three dimensions
 // (respectively).

// Local header files
#include "libmesh/elem.h"
#include "libmesh/face_tri3.h"
#include "libmesh/mesh.h"
#include "libmesh/mesh_generation.h"
#include "libmesh/mesh_tetgen_interface.h"
#include "libmesh/mesh_triangle_holes.h"
#include "libmesh/mesh_triangle_interface.h"
#include "libmesh/node.h"
#include "libmesh/serial_mesh.h"

// Bring in everything from the libMesh namespace
using namespace libMesh;

// Major functions called by main
void triangulate_domain(const Parallel::Communicator& comm);
void tetrahedralize_domain(const Parallel::Communicator& comm);

// Helper routine for tetrahedralize_domain().  Adds the points and elements
// of a convex hull generated by TetGen to the input mesh
void add_cube_convex_hull_to_mesh(MeshBase& mesh, Point lower_limit, Point upper_limit);

// Begin the main program.
int main (int argc, char** argv)
{
  // Initialize libMesh and any dependent libaries, like in example 2.
  LibMeshInit init (argc, argv);

  libmesh_example_assert(2 <= LIBMESH_DIM, "2D support");

  std::cout << "Triangulating an L-shaped domain with holes" << std::endl;

  // 1.) 2D triangulation of L-shaped domain with three holes of different shape
  triangulate_domain(init.comm);

  libmesh_example_assert(3 <= LIBMESH_DIM, "3D support");

  std::cout << "Tetrahedralizing a prismatic domain with a hole" << std::endl;

  // 2.) 3D tetrahedralization of rectangular domain with hole.
  tetrahedralize_domain(init.comm);

  return 0;
}

void triangulate_domain(const Parallel::Communicator& comm)
{
#ifdef LIBMESH_HAVE_TRIANGLE
  // Use typedefs for slightly less typing.
  typedef TriangleInterface::Hole Hole;
  typedef TriangleInterface::PolygonHole PolygonHole;
  typedef TriangleInterface::ArbitraryHole ArbitraryHole;

  // Libmesh mesh that will eventually be created.
  Mesh mesh(2, comm);

  // The points which make up the L-shape:
  mesh.add_point(Point( 0. ,  0.));
  mesh.add_point(Point( 0. , -1.));
  mesh.add_point(Point(-1. , -1.));
  mesh.add_point(Point(-1. ,  1.));
  mesh.add_point(Point( 1. ,  1.));
  mesh.add_point(Point( 1. ,  0.));

  // Declare the TriangleInterface object.  This is where
  // we can set parameters of the triangulation and where the
  // actual triangulate function lives.
  TriangleInterface t(mesh);

  // Customize the variables for the triangulation
  t.desired_area()       = .01;

  // A Planar Straight Line Graph (PSLG) is essentially a list
  // of segments which have to exist in the final triangulation.
  // For an L-shaped domain, Triangle will compute the convex
  // hull of boundary points if we do not specify the PSLG.
  // The PSLG algorithm is also required for triangulating domains
  // containing holes
  t.triangulation_type() = TriangleInterface::PSLG;

  // Turn on/off Laplacian mesh smoothing after generation.
  // By default this is on.
  t.smooth_after_generating() = true;

  // Define holes...

  // hole_1 is a circle (discretized by 50 points)
  PolygonHole hole_1(Point(-0.5,  0.5), // center
		     0.25,              // radius
		     50);               // n. points

  // hole_2 is itself a triangle
  PolygonHole hole_2(Point(0.5, 0.5),   // center
		     0.1,               // radius
		     3);                // n. points

  // hole_3 is an ellipse of 100 points which we define here
  Point ellipse_center(-0.5,  -0.5);
  const unsigned int n_ellipse_points=100;
  std::vector<Point> ellipse_points(n_ellipse_points);
  const Real
    dtheta = 2*libMesh::pi / static_cast<Real>(n_ellipse_points),
    a = .1,
    b = .2;

  for (unsigned int i=0; i<n_ellipse_points; ++i)
    ellipse_points[i]= Point(ellipse_center(0)+a*cos(i*dtheta),
			     ellipse_center(1)+b*sin(i*dtheta));

  ArbitraryHole hole_3(ellipse_center, ellipse_points);

  // Create the vector of Hole*'s ...
  std::vector<Hole*> holes;
  holes.push_back(&hole_1);
  holes.push_back(&hole_2);
  holes.push_back(&hole_3);

  // ... and attach it to the triangulator object
  t.attach_hole_list(&holes);

  // Triangulate!
  t.triangulate();

  // Write the result to file
  mesh.write("delaunay_l_shaped_hole.e");

#endif // LIBMESH_HAVE_TRIANGLE
}

void tetrahedralize_domain(const Parallel::Communicator& comm)
{
#ifdef LIBMESH_HAVE_TETGEN
  // The algorithm is broken up into several steps:
  // 1.) A convex hull is constructed for a rectangular hole.
  // 2.) A convex hull is constructed for the domain exterior.
  // 3.) Neighbor information is updated so TetGen knows there is a convex hull
  // 4.) A vector of hole points is created.
  // 5.) The domain is tetrahedralized, the mesh is written out, etc.

  // The mesh we will eventually generate
  SerialMesh mesh(3,comm);

  // Lower and Upper bounding box limits for a rectangular hole within the unit cube.
  Point hole_lower_limit(0.2, 0.2, 0.4);
  Point hole_upper_limit(0.8, 0.8, 0.6);

  // 1.) Construct a convex hull for the hole
  add_cube_convex_hull_to_mesh(mesh, hole_lower_limit, hole_upper_limit);

  // 2.) Generate elements comprising the outer boundary of the domain.
  add_cube_convex_hull_to_mesh(mesh, Point(0.,0.,0.), Point(1., 1., 1.));

  // 3.) Update neighbor information so that TetGen can verify there is a convex hull.
  mesh.find_neighbors();

  // 4.) Set up vector of hole points
  std::vector<Point> hole(1);
  hole[0] = Point( 0.5*(hole_lower_limit + hole_upper_limit) );

  // 5.) Set parameters and tetrahedralize the domain

  // 0 means "use TetGen default value"
  Real quality_constraint = 2.0;

  // The volume constraint determines the max-allowed tetrahedral
  // volume in the Mesh.  TetGen will split cells which are larger than
  // this size
  Real volume_constraint = 0.001;

  // Construct the Delaunay tetrahedralization
  TetGenMeshInterface t(mesh);
  t.triangulate_conformingDelaunayMesh_carvehole(hole,
						 quality_constraint,
						 volume_constraint);

  // Find neighbors, etc in preparation for writing out the Mesh
  mesh.prepare_for_use();

  // Finally, write out the result
  mesh.write("hole_3D.e");
#endif // LIBMESH_HAVE_TETGEN
}

void add_cube_convex_hull_to_mesh(MeshBase& mesh, Point lower_limit, Point upper_limit)
{
#ifdef LIBMESH_HAVE_TETGEN
  SerialMesh cube_mesh(3);

  unsigned n_elem = 1;

  MeshTools::Generation::build_cube(cube_mesh,
				    n_elem,n_elem,n_elem, // n. elements in each direction
				    lower_limit(0), upper_limit(0),
				    lower_limit(1), upper_limit(1),
				    lower_limit(2), upper_limit(2),
				    HEX8);

  // The pointset_convexhull() algorithm will ignore the Hex8s
  // in the Mesh, and just construct the triangulation
  // of the convex hull.
  TetGenMeshInterface t(cube_mesh);
  t.pointset_convexhull();

  // Now add all nodes from the boundary of the cube_mesh to the input mesh.

  // Map from "node id in cube_mesh" -> "node id in mesh".  Initially inserted
  // with a dummy value, later to be assigned a value by the input mesh.
  std::map<unsigned,unsigned> node_id_map;
  typedef std::map<unsigned,unsigned>::iterator iterator;

  {
    MeshBase::element_iterator it = cube_mesh.elements_begin();
    const MeshBase::element_iterator end = cube_mesh.elements_end();
    for ( ; it != end; ++it)
      {
	Elem* elem = *it;

	for (unsigned s=0; s<elem->n_sides(); ++s)
	  if (elem->neighbor(s) == NULL)
	    {
	      // Add the node IDs of this side to the set
	      AutoPtr<Elem> side = elem->side(s);

	      for (unsigned n=0; n<side->n_nodes(); ++n)
		node_id_map.insert( std::make_pair(side->node(n), /*dummy_value=*/0) );
	    }
      }
  }

  // For each node in the map, insert it into the input mesh and keep
  // track of the ID assigned.
  for (iterator it=node_id_map.begin(); it != node_id_map.end(); ++it)
    {
      // Id of the node in the cube mesh
      unsigned id = (*it).first;

      // Pointer to node in the cube mesh
      Node* old_node = cube_mesh.node_ptr(id);

      // Add geometric point to input mesh
      Node* new_node = mesh.add_point ( *old_node );

      // Track ID value of new_node in map
      (*it).second = new_node->id();
    }

  // With the points added and the map data structure in place, we are
  // ready to add each TRI3 element of the cube_mesh to the input Mesh
  // with proper node assignments
  {
    MeshBase::element_iterator       el     = cube_mesh.elements_begin();
    const MeshBase::element_iterator end_el = cube_mesh.elements_end();

    for (; el != end_el; ++el)
      {
	Elem* old_elem = *el;

	if (old_elem->type() == TRI3)
	  {
	    Elem* new_elem = mesh.add_elem(new Tri3);

	    // Assign nodes in new elements.  Since this is an example,
	    // we'll do it in several steps.
	    for (unsigned i=0; i<old_elem->n_nodes(); ++i)
	      {
		// Locate old node ID in the map
		iterator it = node_id_map.find(old_elem->node(i));

		// Check for not found
		if (it == node_id_map.end())
		  {
		    libMesh::err << "Node id " << old_elem->node(i) << " not found in map!" << std::endl;
		    libmesh_error();
		  }

		// Mapping to node ID in input mesh
		unsigned new_node_id = (*it).second;

		// Node pointer assigned from input mesh
		new_elem->set_node(i) = mesh.node_ptr(new_node_id);
	      }
	  }
      }
  }
#endif // LIBMESH_HAVE_TETGEN
}
<commit_msg>Add communicator for miscellaneous_ex6 SerialMesh<commit_after>/* The libMesh Finite Element Library. */
/* Copyright (C) 2003  Benjamin S. Kirk */

/* This library is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU Lesser General Public */
/* License as published by the Free Software Foundation; either */
/* version 2.1 of the License, or (at your option) any later version. */

/* This library is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU */
/* Lesser General Public License for more details. */

/* You should have received a copy of the GNU Lesser General Public */
/* License along with this library; if not, write to the Free Software */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

 //
Miscellaneous Example 6 - Meshing with LibMesh's TetGen and Triangle Interfaces

 //
 // LibMesh provides interfaces to both Triangle and TetGen for generating
 // Delaunay triangulations and tetrahedralizations in two and three dimensions
 // (respectively).

// Local header files
#include "libmesh/elem.h"
#include "libmesh/face_tri3.h"
#include "libmesh/mesh.h"
#include "libmesh/mesh_generation.h"
#include "libmesh/mesh_tetgen_interface.h"
#include "libmesh/mesh_triangle_holes.h"
#include "libmesh/mesh_triangle_interface.h"
#include "libmesh/node.h"
#include "libmesh/serial_mesh.h"

// Bring in everything from the libMesh namespace
using namespace libMesh;

// Major functions called by main
void triangulate_domain(const Parallel::Communicator& comm);
void tetrahedralize_domain(const Parallel::Communicator& comm);

// Helper routine for tetrahedralize_domain().  Adds the points and elements
// of a convex hull generated by TetGen to the input mesh
void add_cube_convex_hull_to_mesh(MeshBase& mesh, Point lower_limit, Point upper_limit);

// Begin the main program.
int main (int argc, char** argv)
{
  // Initialize libMesh and any dependent libaries, like in example 2.
  LibMeshInit init (argc, argv);

  libmesh_example_assert(2 <= LIBMESH_DIM, "2D support");

  std::cout << "Triangulating an L-shaped domain with holes" << std::endl;

  // 1.) 2D triangulation of L-shaped domain with three holes of different shape
  triangulate_domain(init.comm);

  libmesh_example_assert(3 <= LIBMESH_DIM, "3D support");

  std::cout << "Tetrahedralizing a prismatic domain with a hole" << std::endl;

  // 2.) 3D tetrahedralization of rectangular domain with hole.
  tetrahedralize_domain(init.comm);

  return 0;
}

void triangulate_domain(const Parallel::Communicator& comm)
{
#ifdef LIBMESH_HAVE_TRIANGLE
  // Use typedefs for slightly less typing.
  typedef TriangleInterface::Hole Hole;
  typedef TriangleInterface::PolygonHole PolygonHole;
  typedef TriangleInterface::ArbitraryHole ArbitraryHole;

  // Libmesh mesh that will eventually be created.
  Mesh mesh(2, comm);

  // The points which make up the L-shape:
  mesh.add_point(Point( 0. ,  0.));
  mesh.add_point(Point( 0. , -1.));
  mesh.add_point(Point(-1. , -1.));
  mesh.add_point(Point(-1. ,  1.));
  mesh.add_point(Point( 1. ,  1.));
  mesh.add_point(Point( 1. ,  0.));

  // Declare the TriangleInterface object.  This is where
  // we can set parameters of the triangulation and where the
  // actual triangulate function lives.
  TriangleInterface t(mesh);

  // Customize the variables for the triangulation
  t.desired_area()       = .01;

  // A Planar Straight Line Graph (PSLG) is essentially a list
  // of segments which have to exist in the final triangulation.
  // For an L-shaped domain, Triangle will compute the convex
  // hull of boundary points if we do not specify the PSLG.
  // The PSLG algorithm is also required for triangulating domains
  // containing holes
  t.triangulation_type() = TriangleInterface::PSLG;

  // Turn on/off Laplacian mesh smoothing after generation.
  // By default this is on.
  t.smooth_after_generating() = true;

  // Define holes...

  // hole_1 is a circle (discretized by 50 points)
  PolygonHole hole_1(Point(-0.5,  0.5), // center
		     0.25,              // radius
		     50);               // n. points

  // hole_2 is itself a triangle
  PolygonHole hole_2(Point(0.5, 0.5),   // center
		     0.1,               // radius
		     3);                // n. points

  // hole_3 is an ellipse of 100 points which we define here
  Point ellipse_center(-0.5,  -0.5);
  const unsigned int n_ellipse_points=100;
  std::vector<Point> ellipse_points(n_ellipse_points);
  const Real
    dtheta = 2*libMesh::pi / static_cast<Real>(n_ellipse_points),
    a = .1,
    b = .2;

  for (unsigned int i=0; i<n_ellipse_points; ++i)
    ellipse_points[i]= Point(ellipse_center(0)+a*cos(i*dtheta),
			     ellipse_center(1)+b*sin(i*dtheta));

  ArbitraryHole hole_3(ellipse_center, ellipse_points);

  // Create the vector of Hole*'s ...
  std::vector<Hole*> holes;
  holes.push_back(&hole_1);
  holes.push_back(&hole_2);
  holes.push_back(&hole_3);

  // ... and attach it to the triangulator object
  t.attach_hole_list(&holes);

  // Triangulate!
  t.triangulate();

  // Write the result to file
  mesh.write("delaunay_l_shaped_hole.e");

#endif // LIBMESH_HAVE_TRIANGLE
}

void tetrahedralize_domain(const Parallel::Communicator& comm)
{
#ifdef LIBMESH_HAVE_TETGEN
  // The algorithm is broken up into several steps:
  // 1.) A convex hull is constructed for a rectangular hole.
  // 2.) A convex hull is constructed for the domain exterior.
  // 3.) Neighbor information is updated so TetGen knows there is a convex hull
  // 4.) A vector of hole points is created.
  // 5.) The domain is tetrahedralized, the mesh is written out, etc.

  // The mesh we will eventually generate
  SerialMesh mesh(3,comm);

  // Lower and Upper bounding box limits for a rectangular hole within the unit cube.
  Point hole_lower_limit(0.2, 0.2, 0.4);
  Point hole_upper_limit(0.8, 0.8, 0.6);

  // 1.) Construct a convex hull for the hole
  add_cube_convex_hull_to_mesh(mesh, hole_lower_limit, hole_upper_limit);

  // 2.) Generate elements comprising the outer boundary of the domain.
  add_cube_convex_hull_to_mesh(mesh, Point(0.,0.,0.), Point(1., 1., 1.));

  // 3.) Update neighbor information so that TetGen can verify there is a convex hull.
  mesh.find_neighbors();

  // 4.) Set up vector of hole points
  std::vector<Point> hole(1);
  hole[0] = Point( 0.5*(hole_lower_limit + hole_upper_limit) );

  // 5.) Set parameters and tetrahedralize the domain

  // 0 means "use TetGen default value"
  Real quality_constraint = 2.0;

  // The volume constraint determines the max-allowed tetrahedral
  // volume in the Mesh.  TetGen will split cells which are larger than
  // this size
  Real volume_constraint = 0.001;

  // Construct the Delaunay tetrahedralization
  TetGenMeshInterface t(mesh);
  t.triangulate_conformingDelaunayMesh_carvehole(hole,
						 quality_constraint,
						 volume_constraint);

  // Find neighbors, etc in preparation for writing out the Mesh
  mesh.prepare_for_use();

  // Finally, write out the result
  mesh.write("hole_3D.e");
#endif // LIBMESH_HAVE_TETGEN
}

void add_cube_convex_hull_to_mesh(MeshBase& mesh, Point lower_limit, Point upper_limit)
{
#ifdef LIBMESH_HAVE_TETGEN
  SerialMesh cube_mesh(3,mesh.communicator());

  unsigned n_elem = 1;

  MeshTools::Generation::build_cube(cube_mesh,
				    n_elem,n_elem,n_elem, // n. elements in each direction
				    lower_limit(0), upper_limit(0),
				    lower_limit(1), upper_limit(1),
				    lower_limit(2), upper_limit(2),
				    HEX8);

  // The pointset_convexhull() algorithm will ignore the Hex8s
  // in the Mesh, and just construct the triangulation
  // of the convex hull.
  TetGenMeshInterface t(cube_mesh);
  t.pointset_convexhull();

  // Now add all nodes from the boundary of the cube_mesh to the input mesh.

  // Map from "node id in cube_mesh" -> "node id in mesh".  Initially inserted
  // with a dummy value, later to be assigned a value by the input mesh.
  std::map<unsigned,unsigned> node_id_map;
  typedef std::map<unsigned,unsigned>::iterator iterator;

  {
    MeshBase::element_iterator it = cube_mesh.elements_begin();
    const MeshBase::element_iterator end = cube_mesh.elements_end();
    for ( ; it != end; ++it)
      {
	Elem* elem = *it;

	for (unsigned s=0; s<elem->n_sides(); ++s)
	  if (elem->neighbor(s) == NULL)
	    {
	      // Add the node IDs of this side to the set
	      AutoPtr<Elem> side = elem->side(s);

	      for (unsigned n=0; n<side->n_nodes(); ++n)
		node_id_map.insert( std::make_pair(side->node(n), /*dummy_value=*/0) );
	    }
      }
  }

  // For each node in the map, insert it into the input mesh and keep
  // track of the ID assigned.
  for (iterator it=node_id_map.begin(); it != node_id_map.end(); ++it)
    {
      // Id of the node in the cube mesh
      unsigned id = (*it).first;

      // Pointer to node in the cube mesh
      Node* old_node = cube_mesh.node_ptr(id);

      // Add geometric point to input mesh
      Node* new_node = mesh.add_point ( *old_node );

      // Track ID value of new_node in map
      (*it).second = new_node->id();
    }

  // With the points added and the map data structure in place, we are
  // ready to add each TRI3 element of the cube_mesh to the input Mesh
  // with proper node assignments
  {
    MeshBase::element_iterator       el     = cube_mesh.elements_begin();
    const MeshBase::element_iterator end_el = cube_mesh.elements_end();

    for (; el != end_el; ++el)
      {
	Elem* old_elem = *el;

	if (old_elem->type() == TRI3)
	  {
	    Elem* new_elem = mesh.add_elem(new Tri3);

	    // Assign nodes in new elements.  Since this is an example,
	    // we'll do it in several steps.
	    for (unsigned i=0; i<old_elem->n_nodes(); ++i)
	      {
		// Locate old node ID in the map
		iterator it = node_id_map.find(old_elem->node(i));

		// Check for not found
		if (it == node_id_map.end())
		  {
		    libMesh::err << "Node id " << old_elem->node(i) << " not found in map!" << std::endl;
		    libmesh_error();
		  }

		// Mapping to node ID in input mesh
		unsigned new_node_id = (*it).second;

		// Node pointer assigned from input mesh
		new_elem->set_node(i) = mesh.node_ptr(new_node_id);
	      }
	  }
      }
  }
#endif // LIBMESH_HAVE_TETGEN
}
<|endoftext|>


<commit_before>#include "TRCFileAdapter.h"
#include <fstream>

namespace OpenSim {

const std::string TRCFileAdapter::_markers{"markers"};
const std::string TRCFileAdapter::_delimiterWrite{"\t"};
const std::string TRCFileAdapter::_delimitersRead{" \t"};
const std::string TRCFileAdapter::_frameNumColumnLabel{"Frame#"};
const std::string TRCFileAdapter::_timeColumnLabel{"Time"};
const std::string TRCFileAdapter::_xLabel{"X"};
const std::string TRCFileAdapter::_yLabel{"Y"};
const std::string TRCFileAdapter::_zLabel{"Z"};
const std::string TRCFileAdapter::_numMarkersLabel{"NumMarkers"};
const std::string TRCFileAdapter::_numFramesLabel{"NumFrames"};
const unsigned    TRCFileAdapter::_dataStartsAtLine{6};
const std::vector<std::string> TRCFileAdapter::_metadataKeys{"DataRate",
        "CameraRate", "NumFrames", "NumMarkers", "Units", "OrigDataRate",
        "OrigDataStartFrame", "OrigNumFrames"};

TRCFileAdapter*
TRCFileAdapter::clone() const {
    return new TRCFileAdapter{*this};
}

TimeSeriesTableVec3
TRCFileAdapter::read(const std::string& fileName) const {
    auto abs_table = extendRead(fileName).at(_markers);
    return static_cast<TimeSeriesTableVec3&>(*abs_table);
}

void
TRCFileAdapter::write(const TimeSeriesTableVec3& table,
                      const std::string& fileName) const {
    InputTables tables{};
    tables.emplace(_markers, &table);
    extendWrite(tables, fileName);
}

TRCFileAdapter::OutputTables
TRCFileAdapter::extendRead(const std::string& fileName) const {
    OPENSIM_THROW_IF(fileName.empty(),
                     EmptyFileName);

    std::ifstream in_stream{fileName};
    OPENSIM_THROW_IF(!in_stream.good(),
                     FileDoesNotExist,
                     fileName);

    auto table = std::make_shared<TimeSeriesTableVec3>();

    // Callable to get the next line in form of vector of tokens.
    auto nextLine = [&] {
        return getNextLine(in_stream, _delimitersRead);
    };

    // First line of the stream is considered the header.
    std::string header{};
    std::getline(in_stream, header);
    auto header_tokens = tokenize(header, _delimitersRead);
    OPENSIM_THROW_IF(header_tokens.empty(),
                     FileIsEmpty,
                     fileName);
    OPENSIM_THROW_IF(header_tokens.at(0) != "PathFileType",
                     MissingHeader);
    table->updTableMetaData().setValueForKey("header", header);

    // Read the line containing metadata keys.
    auto keys = nextLine();
    OPENSIM_THROW_IF(keys.size() != _metadataKeys.size(),
                     IncorrectNumMetaDataKeys,
                     fileName,
                     _metadataKeys.size(),
                     keys.size());

    for(size_t i = 0; i < keys.size(); ++i)
        OPENSIM_THROW_IF(keys[i] != _metadataKeys[i],
                         UnexpectedMetaDataKey,
                         fileName,
                         _metadataKeys[i],
                         keys[i]);

    // Read the line containing metadata values.
    auto values = nextLine();
    OPENSIM_THROW_IF(keys.size() != values.size(),
                     MetaDataLengthMismatch,
                     fileName,
                     keys.size(),
                     values.size());

    // Fill up the metadata container.
    for(std::size_t i = 0; i < keys.size(); ++i)
        table->updTableMetaData().setValueForKey(keys[i], values[i]);

    auto num_markers_expected =
        std::stoul(table->
                   getTableMetaData().
                   getValueForKey(_numMarkersLabel).
                   template getValue<std::string>());

    // Read the line containing column labels and fill up the column labels
    // container.
    auto column_labels = nextLine();
    OPENSIM_THROW_IF(column_labels.size() != num_markers_expected + 2,
                     IncorrectNumColumnLabels,
                     fileName,
                     num_markers_expected + 2,
                     column_labels.size());

    // Column 0 should be the frame number. Check and get rid of it as it is
    // not used. The whole column is discarded as the data is read in.
    OPENSIM_THROW_IF(column_labels[0] != _frameNumColumnLabel,
                     UnexpectedColumnLabel,
                     fileName,
                     _frameNumColumnLabel,
                     column_labels[0]);
    column_labels.erase(column_labels.begin());

    // Column 0 (originally column 1 before removing frame number) should
    // now be the time column. Check and get rid of it. The data in this
    // column is maintained separately from rest of the data.
    OPENSIM_THROW_IF(column_labels[0] != _timeColumnLabel,
                     UnexpectedColumnLabel,
                     fileName,
                     _timeColumnLabel,
                     column_labels[0]);
    column_labels.erase(column_labels.begin());

    // Read in the next line of column labels containing (Xdd, Ydd, Zdd)
    // tuples where dd is a 1 or 2 digit subscript. For example --
    // X1, Y1, Z1, X2, Y2, Z2, ... so on.
    // Check and ignore these labels.
    auto xyz_labels_found = nextLine();
    for(int i = 1; i <= num_markers_expected; ++i) {
        size_t j = 0;
        for(auto& letter : {_xLabel, _yLabel, _zLabel}) {
            const size_t ind = ((i - 1) * 3) + j++;
            const std::string expected{letter + std::to_string(i)};
            OPENSIM_THROW_IF(xyz_labels_found.at(ind) != expected,
                             UnexpectedColumnLabel,
                             fileName,
                             expected,
                             xyz_labels_found.at(ind));
        }
    }

    // Read the rows one at a time and fill up the time column container and
    // the data container.
    std::size_t line_num{_dataStartsAtLine - 1};
    auto row = nextLine();
    while(!row.empty()) {
        ++line_num;
        size_t expected{column_labels.size() * 3 + 2};
        OPENSIM_THROW_IF(row.size() != expected,
                         RowLengthMismatch,
                         fileName,
                         line_num,
                         expected,
                         row.size());

        // Columns 2 till the end are data.
        TimeSeriesTableVec3::RowVector
            row_vector{static_cast<int>(num_markers_expected)};
        int ind{0};
        for(std::size_t c = 2; c < column_labels.size() * 3 + 2; c += 3)
            row_vector[ind++] = SimTK::Vec3{std::stod(row.at(c)),
                                            std::stod(row.at(c+1)),
                                            std::stod(row.at(c+2))};

        // Column 1 is time.
        table->appendRow(std::stod(row.at(1)), std::move(row_vector));

        row = nextLine();
    }

    // Set the column labels of the table.
    ValueArray<std::string> value_array{};
    for(const auto& cl : column_labels)
        value_array.upd().push_back(SimTK::Value<std::string>{cl});
    TimeSeriesTableVec3::DependentsMetaData dep_metadata{};
    dep_metadata.setValueArrayForKey("labels", value_array);
    table->setDependentsMetaData(dep_metadata);

    OutputTables output_tables{};
    output_tables.emplace(_markers, table);

    return output_tables;
}

void
TRCFileAdapter::extendWrite(const InputTables& absTables,
                            const std::string& fileName) const {
    OPENSIM_THROW_IF(absTables.empty(),
                     NoTableFound);

    const TimeSeriesTableVec3* table{};
    try {
        auto abs_table = absTables.at(_markers);
        table = dynamic_cast<const TimeSeriesTableVec3*>(abs_table);
    } catch(std::out_of_range) {
        OPENSIM_THROW(KeyMissing,
                      _markers);
    } catch(std::bad_cast&) {
        OPENSIM_THROW(IncorrectTableType);
    }

    OPENSIM_THROW_IF(fileName.empty(),
                     EmptyFileName);

    std::ofstream out_stream{fileName};

    // First line of the stream is the header.
    try {
    out_stream << table->
                  getTableMetaData().
                  getValueForKey("header").
                  getValue<std::string>() << "\n";
    } catch(KeyNotFound&) {
        out_stream << "PathFileType\t4\t(X/Y/Z)\t" << fileName << "\n";
    }

    // Line containing metadata keys.
    out_stream << _metadataKeys[0];
    for(unsigned i = 1; i < _metadataKeys.size(); ++i)
        out_stream << _delimiterWrite << _metadataKeys[i];
    out_stream << "\n";

    // Line containing metadata values.
    std::string datarate;
    try {
        datarate = table->
                   getTableMetaData().
                   getValueForKey(_metadataKeys[0]).
                   getValue<std::string>();
    } catch(KeyNotFound&) {
        OPENSIM_THROW(MissingMetaData,
                      "DataRate");
    }
    out_stream << datarate << _delimiterWrite;
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[1]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << datarate << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[2]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << table->getNumRows() << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[3]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << table->getNumColumns() << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[4]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        OPENSIM_THROW(MissingMetaData,
                      "Units");
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[5]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << datarate << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[6]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << 0 << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[7]).
                      getValue<std::string>();
    } catch(KeyNotFound&) {
        out_stream << table->getNumRows();
    }
    out_stream << "\n";

    // Line containing column labels.
    out_stream << _frameNumColumnLabel << _delimiterWrite
               << _timeColumnLabel     << _delimiterWrite;
    for(unsigned col = 0; col < table->getNumColumns(); ++col)
        out_stream << table->
                      getDependentsMetaData().
                      getValueArrayForKey("labels")[col].
                      getValue<std::string>()
                   << _delimiterWrite << _delimiterWrite << _delimiterWrite;
    out_stream << "\n";

    // Line containing xyz component labels for each marker.
    out_stream << _delimiterWrite << _delimiterWrite;
    for(unsigned col = 1; col <= table->getNumColumns(); ++col)
        for(auto& letter : {_xLabel, _yLabel, _zLabel})
            out_stream << (letter + std::to_string(col)) << _delimiterWrite;
    out_stream << "\n";

    // Empty line.
    out_stream << "\n";

    // Data rows.
    for(unsigned row = 0; row < table->getNumRows(); ++row) {
        constexpr auto prec = std::numeric_limits<double>::digits10 + 1;
        out_stream << row + 1                           << _delimiterWrite
                   << std::setprecision(prec)
                   << table->getIndependentColumn()[row] << _delimiterWrite;
        const auto& row_r = table->getRowAtIndex(row);
        for(unsigned col = 0; col < table->getNumColumns(); ++col) {
            const auto& elt = row_r[col];
            out_stream << std::setprecision(prec)
                       << elt[0] << _delimiterWrite
                       << elt[1] << _delimiterWrite
                       << elt[2] << _delimiterWrite;
        }
        out_stream << "\n";
    }
}

}
<commit_msg>Change to unsigned to avoid warning.<commit_after>#include "TRCFileAdapter.h"
#include <fstream>

namespace OpenSim {

const std::string TRCFileAdapter::_markers{"markers"};
const std::string TRCFileAdapter::_delimiterWrite{"\t"};
const std::string TRCFileAdapter::_delimitersRead{" \t"};
const std::string TRCFileAdapter::_frameNumColumnLabel{"Frame#"};
const std::string TRCFileAdapter::_timeColumnLabel{"Time"};
const std::string TRCFileAdapter::_xLabel{"X"};
const std::string TRCFileAdapter::_yLabel{"Y"};
const std::string TRCFileAdapter::_zLabel{"Z"};
const std::string TRCFileAdapter::_numMarkersLabel{"NumMarkers"};
const std::string TRCFileAdapter::_numFramesLabel{"NumFrames"};
const unsigned    TRCFileAdapter::_dataStartsAtLine{6};
const std::vector<std::string> TRCFileAdapter::_metadataKeys{"DataRate",
        "CameraRate", "NumFrames", "NumMarkers", "Units", "OrigDataRate",
        "OrigDataStartFrame", "OrigNumFrames"};

TRCFileAdapter*
TRCFileAdapter::clone() const {
    return new TRCFileAdapter{*this};
}

TimeSeriesTableVec3
TRCFileAdapter::read(const std::string& fileName) const {
    auto abs_table = extendRead(fileName).at(_markers);
    return static_cast<TimeSeriesTableVec3&>(*abs_table);
}

void
TRCFileAdapter::write(const TimeSeriesTableVec3& table,
                      const std::string& fileName) const {
    InputTables tables{};
    tables.emplace(_markers, &table);
    extendWrite(tables, fileName);
}

TRCFileAdapter::OutputTables
TRCFileAdapter::extendRead(const std::string& fileName) const {
    OPENSIM_THROW_IF(fileName.empty(),
                     EmptyFileName);

    std::ifstream in_stream{fileName};
    OPENSIM_THROW_IF(!in_stream.good(),
                     FileDoesNotExist,
                     fileName);

    auto table = std::make_shared<TimeSeriesTableVec3>();

    // Callable to get the next line in form of vector of tokens.
    auto nextLine = [&] {
        return getNextLine(in_stream, _delimitersRead);
    };

    // First line of the stream is considered the header.
    std::string header{};
    std::getline(in_stream, header);
    auto header_tokens = tokenize(header, _delimitersRead);
    OPENSIM_THROW_IF(header_tokens.empty(),
                     FileIsEmpty,
                     fileName);
    OPENSIM_THROW_IF(header_tokens.at(0) != "PathFileType",
                     MissingHeader);
    table->updTableMetaData().setValueForKey("header", header);

    // Read the line containing metadata keys.
    auto keys = nextLine();
    OPENSIM_THROW_IF(keys.size() != _metadataKeys.size(),
                     IncorrectNumMetaDataKeys,
                     fileName,
                     _metadataKeys.size(),
                     keys.size());

    for(size_t i = 0; i < keys.size(); ++i)
        OPENSIM_THROW_IF(keys[i] != _metadataKeys[i],
                         UnexpectedMetaDataKey,
                         fileName,
                         _metadataKeys[i],
                         keys[i]);

    // Read the line containing metadata values.
    auto values = nextLine();
    OPENSIM_THROW_IF(keys.size() != values.size(),
                     MetaDataLengthMismatch,
                     fileName,
                     keys.size(),
                     values.size());

    // Fill up the metadata container.
    for(std::size_t i = 0; i < keys.size(); ++i)
        table->updTableMetaData().setValueForKey(keys[i], values[i]);

    auto num_markers_expected =
        std::stoul(table->
                   getTableMetaData().
                   getValueForKey(_numMarkersLabel).
                   template getValue<std::string>());

    // Read the line containing column labels and fill up the column labels
    // container.
    auto column_labels = nextLine();
    OPENSIM_THROW_IF(column_labels.size() != num_markers_expected + 2,
                     IncorrectNumColumnLabels,
                     fileName,
                     num_markers_expected + 2,
                     column_labels.size());

    // Column 0 should be the frame number. Check and get rid of it as it is
    // not used. The whole column is discarded as the data is read in.
    OPENSIM_THROW_IF(column_labels[0] != _frameNumColumnLabel,
                     UnexpectedColumnLabel,
                     fileName,
                     _frameNumColumnLabel,
                     column_labels[0]);
    column_labels.erase(column_labels.begin());

    // Column 0 (originally column 1 before removing frame number) should
    // now be the time column. Check and get rid of it. The data in this
    // column is maintained separately from rest of the data.
    OPENSIM_THROW_IF(column_labels[0] != _timeColumnLabel,
                     UnexpectedColumnLabel,
                     fileName,
                     _timeColumnLabel,
                     column_labels[0]);
    column_labels.erase(column_labels.begin());

    // Read in the next line of column labels containing (Xdd, Ydd, Zdd)
    // tuples where dd is a 1 or 2 digit subscript. For example --
    // X1, Y1, Z1, X2, Y2, Z2, ... so on.
    // Check and ignore these labels.
    auto xyz_labels_found = nextLine();
    for(unsigned i = 1; i <= num_markers_expected; ++i) {
        unsigned j = 0;
        for(auto& letter : {_xLabel, _yLabel, _zLabel}) {
            const unsigned ind = ((i - 1) * 3) + j++;
            const std::string expected{letter + std::to_string(i)};
            OPENSIM_THROW_IF(xyz_labels_found.at(ind) != expected,
                             UnexpectedColumnLabel,
                             fileName,
                             expected,
                             xyz_labels_found.at(ind));
        }
    }

    // Read the rows one at a time and fill up the time column container and
    // the data container.
    std::size_t line_num{_dataStartsAtLine - 1};
    auto row = nextLine();
    while(!row.empty()) {
        ++line_num;
        size_t expected{column_labels.size() * 3 + 2};
        OPENSIM_THROW_IF(row.size() != expected,
                         RowLengthMismatch,
                         fileName,
                         line_num,
                         expected,
                         row.size());

        // Columns 2 till the end are data.
        TimeSeriesTableVec3::RowVector
            row_vector{static_cast<int>(num_markers_expected)};
        int ind{0};
        for(std::size_t c = 2; c < column_labels.size() * 3 + 2; c += 3)
            row_vector[ind++] = SimTK::Vec3{std::stod(row.at(c)),
                                            std::stod(row.at(c+1)),
                                            std::stod(row.at(c+2))};

        // Column 1 is time.
        table->appendRow(std::stod(row.at(1)), std::move(row_vector));

        row = nextLine();
    }

    // Set the column labels of the table.
    ValueArray<std::string> value_array{};
    for(const auto& cl : column_labels)
        value_array.upd().push_back(SimTK::Value<std::string>{cl});
    TimeSeriesTableVec3::DependentsMetaData dep_metadata{};
    dep_metadata.setValueArrayForKey("labels", value_array);
    table->setDependentsMetaData(dep_metadata);

    OutputTables output_tables{};
    output_tables.emplace(_markers, table);

    return output_tables;
}

void
TRCFileAdapter::extendWrite(const InputTables& absTables,
                            const std::string& fileName) const {
    OPENSIM_THROW_IF(absTables.empty(),
                     NoTableFound);

    const TimeSeriesTableVec3* table{};
    try {
        auto abs_table = absTables.at(_markers);
        table = dynamic_cast<const TimeSeriesTableVec3*>(abs_table);
    } catch(std::out_of_range) {
        OPENSIM_THROW(KeyMissing,
                      _markers);
    } catch(std::bad_cast&) {
        OPENSIM_THROW(IncorrectTableType);
    }

    OPENSIM_THROW_IF(fileName.empty(),
                     EmptyFileName);

    std::ofstream out_stream{fileName};

    // First line of the stream is the header.
    try {
    out_stream << table->
                  getTableMetaData().
                  getValueForKey("header").
                  getValue<std::string>() << "\n";
    } catch(KeyNotFound&) {
        out_stream << "PathFileType\t4\t(X/Y/Z)\t" << fileName << "\n";
    }

    // Line containing metadata keys.
    out_stream << _metadataKeys[0];
    for(unsigned i = 1; i < _metadataKeys.size(); ++i)
        out_stream << _delimiterWrite << _metadataKeys[i];
    out_stream << "\n";

    // Line containing metadata values.
    std::string datarate;
    try {
        datarate = table->
                   getTableMetaData().
                   getValueForKey(_metadataKeys[0]).
                   getValue<std::string>();
    } catch(KeyNotFound&) {
        OPENSIM_THROW(MissingMetaData,
                      "DataRate");
    }
    out_stream << datarate << _delimiterWrite;
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[1]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << datarate << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[2]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << table->getNumRows() << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[3]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << table->getNumColumns() << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[4]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        OPENSIM_THROW(MissingMetaData,
                      "Units");
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[5]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << datarate << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[6]).
                      getValue<std::string>()
                   << _delimiterWrite;
    } catch(KeyNotFound&) {
        out_stream << 0 << _delimiterWrite;
    }
    try {
        out_stream << table->
                      getTableMetaData().
                      getValueForKey(_metadataKeys[7]).
                      getValue<std::string>();
    } catch(KeyNotFound&) {
        out_stream << table->getNumRows();
    }
    out_stream << "\n";

    // Line containing column labels.
    out_stream << _frameNumColumnLabel << _delimiterWrite
               << _timeColumnLabel     << _delimiterWrite;
    for(unsigned col = 0; col < table->getNumColumns(); ++col)
        out_stream << table->
                      getDependentsMetaData().
                      getValueArrayForKey("labels")[col].
                      getValue<std::string>()
                   << _delimiterWrite << _delimiterWrite << _delimiterWrite;
    out_stream << "\n";

    // Line containing xyz component labels for each marker.
    out_stream << _delimiterWrite << _delimiterWrite;
    for(unsigned col = 1; col <= table->getNumColumns(); ++col)
        for(auto& letter : {_xLabel, _yLabel, _zLabel})
            out_stream << (letter + std::to_string(col)) << _delimiterWrite;
    out_stream << "\n";

    // Empty line.
    out_stream << "\n";

    // Data rows.
    for(unsigned row = 0; row < table->getNumRows(); ++row) {
        constexpr auto prec = std::numeric_limits<double>::digits10 + 1;
        out_stream << row + 1                           << _delimiterWrite
                   << std::setprecision(prec)
                   << table->getIndependentColumn()[row] << _delimiterWrite;
        const auto& row_r = table->getRowAtIndex(row);
        for(unsigned col = 0; col < table->getNumColumns(); ++col) {
            const auto& elt = row_r[col];
            out_stream << std::setprecision(prec)
                       << elt[0] << _delimiterWrite
                       << elt[1] << _delimiterWrite
                       << elt[2] << _delimiterWrite;
        }
        out_stream << "\n";
    }
}

}
<|endoftext|>


<commit_before>// Time:  O(n * m)
// Space: O(m)

class Solution {
public:
    int change(int amount, vector<int>& coins) {
        vector<int> dp(amount + 1);
        dp[0] = 1;
        for (const auto& coin : coins) {
            for(int i = coin; i <= amount; ++i) {
                dp[i] += dp[i - coin];
            }
        }
        return dp[amount];
    }
};
<commit_msg>Update coin-change-2.cpp<commit_after>// Time:  O(n * m)
// Space: O(m)

class Solution {
public:
    int change(int amount, vector<int>& coins) {
        vector<int> dp(amount + 1);
        dp[0] = 1;
        for (const auto& coin : coins) {
            for (int i = coin; i <= amount; ++i) {
                dp[i] += dp[i - coin];
            }
        }
        return dp[amount];
    }
};
<|endoftext|>


<commit_before>#include "robot-devices.hpp"
#include "TaskSignals.hpp"

#include <rtos.h>
#include <RPCVariable.h>
#include <logger.hpp>

#include "motors.hpp"
#include "mpu-6050.hpp"

// Keep this pretty high for now. Ideally, drop it down to ~3 for production
// builds. Hopefully that'll be possible without the console
#define CONTROL_LOOP_WAIT_MS 20

// Declaration for an alternative control loop thread for when the accel/gyro
// can't be used for whatever reason
void Task_Controller_Sensorless(void const* args);

namespace {
// The gyro/accel values are given RPC read/write access here
float gyroVals[3] = {0};
float accelVals[3] = {0};
// RPCVariable<float> gyrox(&gyroVals[0], "gyro-x");
// RPCVariable<float> gyroy(&gyroVals[1], "gyro-y");
// RPCVariable<float> gyroz(&gyroVals[2], "gyro-z");
// RPCVariable<float> accelx(&accelVals[0], "accel-x");
// RPCVariable<float> accely(&accelVals[1], "accel-y");
// RPCVariable<float> accelz(&accelVals[2], "accel-z");

// Making a temporary variable to test out the writing side of RPC variables
// int testVar;
// RPCVariable<int> test_var(&testVar, "var1");
}

/**
 * initializes the motion controller thread
 */
void Task_Controller(void const* args) {
    // Store the thread's ID
    osThreadId threadID = Thread::gettid();

    // Store our priority so we know what to reset it to if ever needed
    osPriority threadPriority;

    if (threadID != nullptr)
        threadPriority = osThreadGetPriority(threadID);
    else
        threadPriority = osPriorityIdle;

#if RJ_MPU_EN
    MPU6050 imu(RJ_I2C_BUS);

    imu.setBW(MPU6050_BW_256);
    imu.setGyroRange(MPU6050_GYRO_RANGE_250);
    imu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
    imu.setSleepMode(false);

    char testResp;
    if ((testResp = imu.testConnection())) {
        float resultRatio[6];
        imu.selfTest(resultRatio);
        LOG(INIT,
            "IMU self test results:\r\n"
            "    Accel (X,Y,Z):\t(%2.2f%%, %2.2f%%, %2.2f%%)\r\n"
            "    Gyro  (X,Y,Z):\t(%2.2f%%, %2.2f%%, %2.2f%%)",
            resultRatio[0], resultRatio[1], resultRatio[2], resultRatio[3],
            resultRatio[4], resultRatio[5]);

        LOG(INIT,
            "Control loop ready!\r\n    Thread ID:\t%u\r\n    Priority:\t%d",
            threadID, threadPriority);

    } else {
        LOG(SEVERE,
            "MPU6050 not found!\t(response: 0x%02X)\r\n    Falling back to "
            "sensorless control loop.",
            testResp);
// TODO: Turn on the IMU's error LED here

#else
    LOG(INIT,
        "IMU disabled in config file\r\n    Falling back to sensorless control "
        "loop.");
#endif
        // Start a thread that can function without the IMU, then terminate this
        // thread
        Thread controller_task(Task_Controller_Sensorless, nullptr,
                               osPriorityRealtime);
        osThreadTerminate(threadID);

        return;

#if RJ_MPU_EN
    }

#endif

    osThreadSetPriority(threadID, osPriorityNormal);

    while (true) {
        imu.getGyro(gyroVals);
        imu.getAccelero(accelVals);

        LOG(INF2,
            "Gyro:\t"
            "(% 1.2f, % 1.2f, % 1.2f)\r\n"
            "Accel:\t"
            "(% 1.2f, % 1.2f, % 1.2f)",
            gyroVals[0], gyroVals[1], gyroVals[2], accelVals[0], accelVals[1],
            accelVals[2]);

        Thread::wait(CONTROL_LOOP_WAIT_MS);
        Thread::yield();
    }

    osThreadTerminate(threadID);
}

/**
 * [Task_Controller_Sensorless]
 * @param args [description]
 */
void Task_Controller_Sensorless(void const* args) {
    // Store the thread's ID
    osThreadId threadID = Thread::gettid();

    // Store our priority so we know what to reset it to if ever needed
    osPriority threadPriority;

    if (threadID != nullptr)
        threadPriority = osThreadGetPriority(threadID);
    else
        threadPriority = osPriorityIdle;

    LOG(INIT,
        "Sensorless control loop ready!\r\n    Thread ID:\t%u\r\n    "
        "Priority:\t%d",
        threadID, threadPriority);

    while (1) {
        Thread::wait(CONTROL_LOOP_WAIT_MS);
        Thread::yield();
    }

    osThreadTerminate(threadID);
}
<commit_msg>removing define and adding variable instead<commit_after>#include "robot-devices.hpp"
#include "TaskSignals.hpp"

#include <rtos.h>
#include <RPCVariable.h>
#include <logger.hpp>

#include "motors.hpp"
#include "mpu-6050.hpp"

// Keep this pretty high for now. Ideally, drop it down to ~3 for production
// builds. Hopefully that'll be possible without the console
static const int CONTROL_LOOP_WAIT_MS = 20;

// Declaration for an alternative control loop thread for when the accel/gyro
// can't be used for whatever reason
void Task_Controller_Sensorless(void const* args);

namespace {
// The gyro/accel values are given RPC read/write access here
float gyroVals[3] = {0};
float accelVals[3] = {0};
// RPCVariable<float> gyrox(&gyroVals[0], "gyro-x");
// RPCVariable<float> gyroy(&gyroVals[1], "gyro-y");
// RPCVariable<float> gyroz(&gyroVals[2], "gyro-z");
// RPCVariable<float> accelx(&accelVals[0], "accel-x");
// RPCVariable<float> accely(&accelVals[1], "accel-y");
// RPCVariable<float> accelz(&accelVals[2], "accel-z");

// Making a temporary variable to test out the writing side of RPC variables
// int testVar;
// RPCVariable<int> test_var(&testVar, "var1");
}

/**
 * initializes the motion controller thread
 */
void Task_Controller(void const* args) {
    // Store the thread's ID
    osThreadId threadID = Thread::gettid();

    // Store our priority so we know what to reset it to if ever needed
    osPriority threadPriority;

    if (threadID != nullptr)
        threadPriority = osThreadGetPriority(threadID);
    else
        threadPriority = osPriorityIdle;

#if RJ_MPU_EN
    MPU6050 imu(RJ_I2C_BUS);

    imu.setBW(MPU6050_BW_256);
    imu.setGyroRange(MPU6050_GYRO_RANGE_250);
    imu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
    imu.setSleepMode(false);

    char testResp;
    if ((testResp = imu.testConnection())) {
        float resultRatio[6];
        imu.selfTest(resultRatio);
        LOG(INIT,
            "IMU self test results:\r\n"
            "    Accel (X,Y,Z):\t(%2.2f%%, %2.2f%%, %2.2f%%)\r\n"
            "    Gyro  (X,Y,Z):\t(%2.2f%%, %2.2f%%, %2.2f%%)",
            resultRatio[0], resultRatio[1], resultRatio[2], resultRatio[3],
            resultRatio[4], resultRatio[5]);

        LOG(INIT,
            "Control loop ready!\r\n    Thread ID:\t%u\r\n    Priority:\t%d",
            threadID, threadPriority);

    } else {
        LOG(SEVERE,
            "MPU6050 not found!\t(response: 0x%02X)\r\n    Falling back to "
            "sensorless control loop.",
            testResp);
// TODO: Turn on the IMU's error LED here

#else
    LOG(INIT,
        "IMU disabled in config file\r\n    Falling back to sensorless control "
        "loop.");
#endif
        // Start a thread that can function without the IMU, then terminate this
        // thread
        Thread controller_task(Task_Controller_Sensorless, nullptr,
                               osPriorityRealtime);
        osThreadTerminate(threadID);

        return;

#if RJ_MPU_EN
    }

#endif

    osThreadSetPriority(threadID, osPriorityNormal);

    while (true) {
        imu.getGyro(gyroVals);
        imu.getAccelero(accelVals);

        LOG(INF2,
            "Gyro:\t"
            "(% 1.2f, % 1.2f, % 1.2f)\r\n"
            "Accel:\t"
            "(% 1.2f, % 1.2f, % 1.2f)",
            gyroVals[0], gyroVals[1], gyroVals[2], accelVals[0], accelVals[1],
            accelVals[2]);

        Thread::wait(CONTROL_LOOP_WAIT_MS);
        Thread::yield();
    }

    osThreadTerminate(threadID);
}

/**
 * [Task_Controller_Sensorless]
 * @param args [description]
 */
void Task_Controller_Sensorless(void const* args) {
    // Store the thread's ID
    osThreadId threadID = Thread::gettid();

    // Store our priority so we know what to reset it to if ever needed
    osPriority threadPriority;

    if (threadID != nullptr)
        threadPriority = osThreadGetPriority(threadID);
    else
        threadPriority = osPriorityIdle;

    LOG(INIT,
        "Sensorless control loop ready!\r\n    Thread ID:\t%u\r\n    "
        "Priority:\t%d",
        threadID, threadPriority);

    while (1) {
        Thread::wait(CONTROL_LOOP_WAIT_MS);
        Thread::yield();
    }

    osThreadTerminate(threadID);
}
<|endoftext|>


<commit_before>#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "BlockStats.h"
#include "TexComp.h"
#include "ImageFile.h"
#include "Image.h"

void PrintUsage() {
   fprintf(stderr, "Usage: tc [-l] [-q <quality>] [-n <num>] [-simd] [-t <threads> [-j <jobs>]] <imagefile>\n");
}

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

  int fileArg = 1;
  if(fileArg == argc) {
    PrintUsage();
    exit(1);
  }

  int numJobs = 0;
  int quality = 50;
  int numThreads = 1;
  int numCompressions = 1;
  bool bUseSIMD = false;
  bool bSaveLog = false;

  bool knowArg = false;
  do {
    knowArg = false;

    if(strcmp(argv[fileArg], "-n") == 0) {
      fileArg++;

      if(fileArg == argc || (numCompressions = atoi(argv[fileArg])) < 0) {
        PrintUsage();
        exit(1);
      }

      fileArg++;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-l") == 0) {
      fileArg++;
      bSaveLog = true;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-simd") == 0) {
      fileArg++;
      bUseSIMD = true;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-t") == 0) {
      fileArg++;

      if(fileArg == argc || (numThreads = atoi(argv[fileArg])) < 1) {
	PrintUsage();
	exit(1);
      }

      fileArg++;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-q") == 0) {
      fileArg++;

      if(fileArg == argc || (quality = atoi(argv[fileArg])) < 0) {
	PrintUsage();
	exit(1);
      }

      fileArg++;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-j") == 0) {
      fileArg++;

      if(fileArg == argc || (numJobs = atoi(argv[fileArg])) < 0) {
	PrintUsage();
	exit(1);
      }

      fileArg++;
      knowArg = true;
      continue;
    }

  } while(knowArg && fileArg < argc);

  if(numThreads > 1 && bSaveLog) {
    bSaveLog = false;
    fprintf(stderr, "WARNING: Will not save log because implementation is not thread safe.\n"
	    "If you'd like, send a complaint to pavel@cs.unc.edu to get this done faster.\n");
  }

  if(fileArg == argc) {
    PrintUsage();
    exit(1);
  }

  ImageFile file (argv[fileArg]);
	if(!file.Load()) {
    fprintf(stderr, "Error loading file: %s\n", argv[fileArg]);
    return 1;
	}

	const Image *img = file.GetImage();

  int numBlocks = (img->GetWidth() * img->GetHeight())/16;
  BlockStatManager *statManager = NULL;
  if(bSaveLog) {
    statManager = new BlockStatManager(numBlocks);
  }

  SCompressionSettings settings;
  settings.bUseSIMD = bUseSIMD;
  settings.iNumThreads = numThreads;
  settings.iQuality = quality;
  settings.iNumCompressions = numCompressions;
  settings.iJobSize = numJobs;
  settings.pStatManager = statManager;

  CompressedImage *ci = img->Compress(settings);
  if(NULL == ci) {
    fprintf(stderr, "Error compressing image!\n");
    return 1;
  }

  double PSNR = img->ComputePSNR(*ci);
  if(PSNR > 0.0) {
    fprintf(stdout, "PSNR: %.3f\n", PSNR);
  }
  else {
    fprintf(stderr, "Error computing PSNR\n");
  }

  if(bSaveLog) {
    statManager->ToFile(strcat(argv[fileArg], ".log"));
  }

	Image cImg (*ci);
	ImageFile cImgFile (strcat(argv[fileArg], "-bc7.png"), eFileFormat_PNG, cImg);
	cImgFile.Write();

  // Cleanup
  delete ci;
  if(statManager)
    delete statManager;

  return 0;
}
<commit_msg>Extract the basename for use in our output filenames...<commit_after>#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "BlockStats.h"
#include "TexComp.h"
#include "ImageFile.h"
#include "Image.h"

void PrintUsage() {
   fprintf(stderr, "Usage: tc [-l] [-q <quality>] [-n <num>] [-simd] [-t <threads> [-j <jobs>]] <imagefile>\n");
}

void ExtractBasename(const char *filename, char *buf, int bufSz) {
	int len = strlen(filename);
	const char *end = filename + len;
	while(--end != filename) {
		if(*end == '.')
		{
			int numChars = end - filename + 1;
			int toCopy = (numChars > bufSz)? bufSz : numChars;
			memcpy(buf, filename, toCopy);
			buf[toCopy - 1] = '\0';
			return;
		}
	}
}

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

  int fileArg = 1;
  if(fileArg == argc) {
    PrintUsage();
    exit(1);
  }

  int numJobs = 0;
  int quality = 50;
  int numThreads = 1;
  int numCompressions = 1;
  bool bUseSIMD = false;
  bool bSaveLog = false;

  bool knowArg = false;
  do {
    knowArg = false;

    if(strcmp(argv[fileArg], "-n") == 0) {
      fileArg++;

      if(fileArg == argc || (numCompressions = atoi(argv[fileArg])) < 0) {
        PrintUsage();
        exit(1);
      }

      fileArg++;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-l") == 0) {
      fileArg++;
      bSaveLog = true;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-simd") == 0) {
      fileArg++;
      bUseSIMD = true;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-t") == 0) {
      fileArg++;

      if(fileArg == argc || (numThreads = atoi(argv[fileArg])) < 1) {
	PrintUsage();
	exit(1);
      }

      fileArg++;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-q") == 0) {
      fileArg++;

      if(fileArg == argc || (quality = atoi(argv[fileArg])) < 0) {
	PrintUsage();
	exit(1);
      }

      fileArg++;
      knowArg = true;
      continue;
    }

    if(strcmp(argv[fileArg], "-j") == 0) {
      fileArg++;

      if(fileArg == argc || (numJobs = atoi(argv[fileArg])) < 0) {
	PrintUsage();
	exit(1);
      }

      fileArg++;
      knowArg = true;
      continue;
    }

  } while(knowArg && fileArg < argc);

  if(numThreads > 1 && bSaveLog) {
    bSaveLog = false;
    fprintf(stderr, "WARNING: Will not save log because implementation is not thread safe.\n"
	    "If you'd like, send a complaint to pavel@cs.unc.edu to get this done faster.\n");
  }

  if(fileArg == argc) {
    PrintUsage();
    exit(1);
  }

	char basename[256];
	ExtractBasename(argv[fileArg], basename, 256);

  ImageFile file (argv[fileArg]);
	if(!file.Load()) {
    fprintf(stderr, "Error loading file: %s\n", argv[fileArg]);
    return 1;
	}

	const Image *img = file.GetImage();

  int numBlocks = (img->GetWidth() * img->GetHeight())/16;
  BlockStatManager *statManager = NULL;
  if(bSaveLog) {
    statManager = new BlockStatManager(numBlocks);
  }

  SCompressionSettings settings;
  settings.bUseSIMD = bUseSIMD;
  settings.iNumThreads = numThreads;
  settings.iQuality = quality;
  settings.iNumCompressions = numCompressions;
  settings.iJobSize = numJobs;
  settings.pStatManager = statManager;

  CompressedImage *ci = img->Compress(settings);
  if(NULL == ci) {
    fprintf(stderr, "Error compressing image!\n");
    return 1;
  }

  double PSNR = img->ComputePSNR(*ci);
  if(PSNR > 0.0) {
    fprintf(stdout, "PSNR: %.3f\n", PSNR);
  }
  else {
    fprintf(stderr, "Error computing PSNR\n");
  }

  if(bSaveLog) {
    statManager->ToFile(strcat(basename, ".log"));
  }

	Image cImg (*ci);
	ImageFile cImgFile (strcat(basename, "-bc7.png"), eFileFormat_PNG, cImg);
	cImgFile.Write();

  // Cleanup
  delete ci;
  if(statManager)
    delete statManager;

  return 0;
}
<|endoftext|>


<commit_before>#include "teitz_bart.h"

#include <algorithm>
#include <ctime>
#include <iostream>

#include "dijkstra.h"

namespace rotas
{
	namespace algoritmos
	{
		std::vector<Cidade> TeitzBart::localiza_medianas(std::vector<Cidade> cidades, const unsigned int& p)
		{
			lista_vertices_t todos_os_vertices /* { V } */ = TeitzBart::inicializa_vertices(cidades);

			//
			// Passo 1: Contruir um conjunto inicial 'S', com 'p' elementos de 'V'

			lista_vertices_t medianas /* { S } */ = seleciona_medianas_aleatoriamente(todos_os_vertices, p);

			bool modificou;

			do
			{
				//
				// Passo 2: Rotular todos os pontos 'Vi' no pertencentes  { S } como "no-analisados"

				rotula_nao_analisados(todos_os_vertices, medianas);

				//
				// Passo 3: Enquanto existirem pontos "no-analisados" no conjunto { V - S }, faa:

				modificou = false;

				for (size_t i = 0; i < todos_os_vertices.size(); i++)
				{
					vertice_t& Vi = todos_os_vertices[i];

					if (Vi.analisado == true)
					{
						continue;
					}

					//
					// a) 'Vi'  um vrtice "no-analisado". Calcular reduo 'R' do nmero de transmisso
					//	  para todo 'Vj' pertencente  { S }. Rij = NT(S) - NT(S u { Vi } - { Vj })

					int VjCidade_id = -1;
					int VjMax_index = -1;
					double max = std::numeric_limits<double>::min();

					double ntS = calcula_numero_transmissao(Vi, todos_os_vertices);

					for (size_t j = 0; j < medianas.size(); j++)
					{
						vertice_t& Vj = medianas[j];

						double reducao = ntS - calcula_numero_transmissao(Vj, todos_os_vertices, Vi, Vj);

						if (reducao > max)
						{
							//
							// b) Faa Rij0 = Max[Rij].

							VjCidade_id = Vj.cidade.get_id();
							VjMax_index = j;
							max = reducao;
						}
					}

					if (VjMax_index >= 0 && max > 0)
					{
						//
						// c) Se Rij0 > 0, faa: S = S u { Vi } - { Vj0 } e rotule Vj0 como "analisado".

						// Remove Vj0
						medianas.erase(medianas.begin() + VjMax_index);

						// Adiciona Vi
						medianas.push_back(Vi);

						// Rotula Vj0 como "analisado"
						for (size_t aux = 0; aux < todos_os_vertices.size(); aux++)
						{
							vertice_t& v = todos_os_vertices[aux];

							if (v.cidade.get_id() == VjCidade_id)
							{
								v.analisado = true;
								break;
							}
						}

						modificou = true;
					}
					else
					{
						//
						// d) Se Rij0 <= 0, rotule 'Vi' como "analisado".

						Vi.analisado = true;
					}
				}

				//
				// Passo 4: Se durante a execuo do passo 3 ocorrer modificaes no conjunto S, volte para o passo 2.
				//          Caso contrrio PARE. O conjunto { S } ser uma aproximao para o problema das p-medianas.
			} while (
				existe_nao_analisados(todos_os_vertices) /* 'existe_nao_analisados()' pertence ao Passo 3 */ ||
				modificou == true /* 'modificou' pertence ao Passo 4 */);

			return vertices_para_cidades(medianas);
		}

		lista_vertices_t TeitzBart::seleciona_medianas_aleatoriamente(lista_vertices_t& vertices, const unsigned int& p)
		{
			//
			// TODO: verificar se o vetor vertices est vazio

			srand((unsigned int)time(NULL));

			lista_vertices_t medianas;

			for (unsigned int i = 0; i < p; i++)
			{
				unsigned int index = rand() % vertices.size();

				vertice_t& vertice = vertices[index];

				if (TeitzBart::contem_vertice(medianas, vertice) == true)
				{
					i--;
					continue;
				}

				medianas.push_back(vertice);
			}

			return medianas;
		}

		void TeitzBart::rotula_nao_analisados(lista_vertices_t& todos_os_vertices, lista_vertices_t& medianas)
		{
			for (size_t i = 0; i < todos_os_vertices.size(); i++)
			{
				vertice_t& vertice = todos_os_vertices[i];

				if (TeitzBart::contem_vertice(medianas, vertice) == false)
				{
					vertice.analisado = false;
				}
				else
				{
					vertice.analisado = true;
				}
			}
		}

		bool TeitzBart::existe_nao_analisados(lista_vertices_t& vertices)
		{
			for (size_t i = 0; i < vertices.size(); i++)
			{
				if (vertices[i].analisado == false)
				{
					return true;
				}
			}

			return false;
		}

		lista_vertices_t TeitzBart::inicializa_vertices(std::vector<Cidade>& cidades)
		{
			lista_vertices_t vertices;

			for (size_t i = 0; i < cidades.size(); i++)
			{
				vertices.push_back(vertice_t(cidades[i]));
			}

			return vertices;
		}

		std::vector<Cidade> TeitzBart::vertices_para_cidades(const  lista_vertices_t& vertices)
		{
			std::vector<Cidade> cidades;

			for (unsigned int i = 0; i < vertices.size(); i++)
			{
				cidades.push_back(vertices[i].cidade);
			}

			return cidades;
		}

		bool TeitzBart::contem_vertice(lista_vertices_t& vertices, vertice_t& vertice)
		{
			for (size_t i = 0; i < vertices.size(); i++)
			{
				vertice_t& v = vertices[i];

				if (v.cidade.get_id() == vertice.cidade.get_id())
				{
					return true;
				}
			}

			return false;
		}

		bool TeitzBart::contem_id(lista_vertices_t& vertices, const int& id)
		{
			for (size_t i = 0; i < vertices.size(); i++)
			{
				vertice_t& v = vertices[i];

				if (v.cidade.get_id() == id)
				{
					return true;
				}
			}

			return false;
		}

		double TeitzBart::calcula_numero_transmissao(vertice_t& vertice, lista_vertices_t& grafo)
		{
			std::vector<Rota> rotas = vertice.cidade.get_rotas();
			double soma = 0.0;

			for (size_t i = 0; i < rotas.size(); i++)
			{
				Rota rota = rotas.at(i);

				if (contem_id(grafo, rota.get_id_destino()) == false)
				{

					continue;
				}

				soma += rota.get_distancia();
			}

			return soma;
		}

		double TeitzBart::calcula_numero_transmissao(vertice_t& vertice, lista_vertices_t& grafo, vertice_t& adicionar, vertice_t& remover)
		{
			lista_vertices_t copia = lista_vertices_t(grafo);
			int index = -1;

			//
			// Remove um vertice

			for (size_t i = 0; i < grafo.size(); i++)
			{
				vertice_t& v = grafo[i];

				if (v.cidade.get_id() == remover.cidade.get_id())
				{
					index = i;
					break;
				}
			}

			if (index < 0)
			{
				return 0.0;
			}

			copia.erase(copia.begin() + index);

			//
			// Adiciona um vertice

			copia.push_back(adicionar);

			return calcula_numero_transmissao(vertice, copia);
		}
	} // algoritmos
} // rotas
<commit_msg>otimizando cálculo do numero de transmissão para não fazer cópia de vetores.<commit_after>#include "teitz_bart.h"

#include <algorithm>
#include <ctime>
#include <iostream>

#include "dijkstra.h"

namespace rotas
{
	namespace algoritmos
	{
		std::vector<Cidade> TeitzBart::localiza_medianas(std::vector<Cidade> cidades, const unsigned int& p)
		{
			lista_vertices_t todos_os_vertices /* { V } */ = TeitzBart::inicializa_vertices(cidades);

			//
			// Passo 1: Contruir um conjunto inicial 'S', com 'p' elementos de 'V'

			lista_vertices_t medianas /* { S } */ = seleciona_medianas_aleatoriamente(todos_os_vertices, p);

			bool modificou;

			do
			{
				//
				// Passo 2: Rotular todos os pontos 'Vi' no pertencentes  { S } como "no-analisados"

				rotula_nao_analisados(todos_os_vertices, medianas);

				//
				// Passo 3: Enquanto existirem pontos "no-analisados" no conjunto { V - S }, faa:

				modificou = false;

				for (size_t i = 0; i < todos_os_vertices.size(); i++)
				{
					vertice_t& Vi = todos_os_vertices[i];

					if (Vi.analisado == true)
					{
						continue;
					}

					//
					// a) 'Vi'  um vrtice "no-analisado". Calcular reduo 'R' do nmero de transmisso
					//	  para todo 'Vj' pertencente  { S }. Rij = NT(S) - NT(S u { Vi } - { Vj })

					int VjCidade_id = -1;
					int VjMax_index = -1;
					double max = std::numeric_limits<double>::min();

					double ntS = calcula_numero_transmissao(Vi, todos_os_vertices);

					for (size_t j = 0; j < medianas.size(); j++)
					{
						vertice_t& Vj = medianas[j];

						double reducao = ntS - calcula_numero_transmissao(Vj, todos_os_vertices, Vi, Vj);

						if (reducao > max)
						{
							//
							// b) Faa Rij0 = Max[Rij].

							VjCidade_id = Vj.cidade.get_id();
							VjMax_index = j;
							max = reducao;
						}
					}

					if (VjMax_index >= 0 && max > 0)
					{
						//
						// c) Se Rij0 > 0, faa: S = S u { Vi } - { Vj0 } e rotule Vj0 como "analisado".

						// Remove Vj0
						medianas.erase(medianas.begin() + VjMax_index);

						// Adiciona Vi
						medianas.push_back(Vi);

						// Rotula Vj0 como "analisado"
						for (size_t aux = 0; aux < todos_os_vertices.size(); aux++)
						{
							vertice_t& v = todos_os_vertices[aux];

							if (v.cidade.get_id() == VjCidade_id)
							{
								v.analisado = true;
								break;
							}
						}

						modificou = true;
					}
					else
					{
						//
						// d) Se Rij0 <= 0, rotule 'Vi' como "analisado".

						Vi.analisado = true;
					}
				}

				//
				// Passo 4: Se durante a execuo do passo 3 ocorrer modificaes no conjunto S, volte para o passo 2.
				//          Caso contrrio PARE. O conjunto { S } ser uma aproximao para o problema das p-medianas.
			} while (
				existe_nao_analisados(todos_os_vertices) /* 'existe_nao_analisados()' pertence ao Passo 3 */ ||
				modificou == true /* 'modificou' pertence ao Passo 4 */);

			return vertices_para_cidades(medianas);
		}

		lista_vertices_t TeitzBart::seleciona_medianas_aleatoriamente(lista_vertices_t& vertices, const unsigned int& p)
		{
			//
			// TODO: verificar se o vetor vertices est vazio

			srand((unsigned int)time(NULL));

			lista_vertices_t medianas;

			for (unsigned int i = 0; i < p; i++)
			{
				unsigned int index = rand() % vertices.size();

				vertice_t& vertice = vertices[index];

				if (TeitzBart::contem_vertice(medianas, vertice) == true)
				{
					i--;
					continue;
				}

				medianas.push_back(vertice);
			}

			return medianas;
		}

		void TeitzBart::rotula_nao_analisados(lista_vertices_t& todos_os_vertices, lista_vertices_t& medianas)
		{
			for (size_t i = 0; i < todos_os_vertices.size(); i++)
			{
				vertice_t& vertice = todos_os_vertices[i];

				if (TeitzBart::contem_vertice(medianas, vertice) == false)
				{
					vertice.analisado = false;
				}
				else
				{
					vertice.analisado = true;
				}
			}
		}

		bool TeitzBart::existe_nao_analisados(lista_vertices_t& vertices)
		{
			for (size_t i = 0; i < vertices.size(); i++)
			{
				if (vertices[i].analisado == false)
				{
					return true;
				}
			}

			return false;
		}

		lista_vertices_t TeitzBart::inicializa_vertices(std::vector<Cidade>& cidades)
		{
			lista_vertices_t vertices;

			for (size_t i = 0; i < cidades.size(); i++)
			{
				vertices.push_back(vertice_t(cidades[i]));
			}

			return vertices;
		}

		std::vector<Cidade> TeitzBart::vertices_para_cidades(const  lista_vertices_t& vertices)
		{
			std::vector<Cidade> cidades;

			for (unsigned int i = 0; i < vertices.size(); i++)
			{
				cidades.push_back(vertices[i].cidade);
			}

			return cidades;
		}

		bool TeitzBart::contem_vertice(lista_vertices_t& vertices, vertice_t& vertice)
		{
			for (size_t i = 0; i < vertices.size(); i++)
			{
				vertice_t& v = vertices[i];

				if (v.cidade.get_id() == vertice.cidade.get_id())
				{
					return true;
				}
			}

			return false;
		}

		bool TeitzBart::contem_id(lista_vertices_t& vertices, const int& id)
		{
			for (size_t i = 0; i < vertices.size(); i++)
			{
				vertice_t& v = vertices[i];

				if (v.cidade.get_id() == id)
				{
					return true;
				}
			}

			return false;
		}

		double TeitzBart::calcula_numero_transmissao(vertice_t& vertice, lista_vertices_t& grafo)
		{
			std::vector<Rota> rotas = vertice.cidade.get_rotas();
			double soma = 0.0;

			for (size_t i = 0; i < rotas.size(); i++)
			{
				Rota rota = rotas.at(i);

				if (contem_id(grafo, rota.get_id_destino()) == false)
				{
					continue;
				}

				soma += rota.get_distancia();
			}

			return soma;
		}

		double TeitzBart::calcula_numero_transmissao(vertice_t& vertice, lista_vertices_t& grafo, vertice_t& adicionar, vertice_t& remover)
		{
			std::vector<Rota> rotas = vertice.cidade.get_rotas();
			double soma = 0.0;

			for (size_t i = 0; i < rotas.size(); i++)
			{
				Rota rota = rotas.at(i);

				if ((contem_id(grafo, rota.get_id_destino()) == false && rota.get_id_destino() != adicionar.cidade.get_id()) ||
					rota.get_id_destino() == remover.cidade.get_id())
				{
					continue;
				}

				soma += rota.get_distancia();
			}

			return soma;
		}
	} // algoritmos
} // rotas
<|endoftext|>


<commit_before>/*--------------------------------------------------------------------------
File   : diskio_flash.cpp

Author : Hoang Nguyen Hoan          Aug. 30, 2016

Desc   : Generic flash disk I/O driver class

Copyright (c) 2016, Motsai, all rights reserved

Permission to use, copy, modify, and distribute this software for any purpose
with or without fee is hereby granted, provided that the above copyright
notice and this permission notice appear in all copies, and none of the
names : I-SYST or its contributors may be used to endorse or
promote products derived from this software without specific prior written
permission.

For info or contributing contact : nh.hoang at motsai dot com

THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

----------------------------------------------------------------------------
Modified by          Date              Description

----------------------------------------------------------------------------*/
#include <stdio.h>

#include "diskio_flash.h"
#include "idelay.h"

FlashDiskIO::FlashDiskIO() : DiskIO()
{
	vpWaitCB = NULL;
	vpInterf = NULL;
}

bool FlashDiskIO::Init(FLASHDISKIO_CFG &Cfg, SerialIntrf *pInterf,
                       DISKIO_CACHE_DESC *pCacheBlk, int NbCacheBlk)
{
    if (pInterf == NULL)
        return false;

    if (Cfg.pInitCB)
    {
        if (Cfg.pInitCB(Cfg.DevNo, pInterf) == false)
            return false;
    }

    if (Cfg.pWaitCB)
    	vpWaitCB = Cfg.pWaitCB;

    vDevNo          = Cfg.DevNo;
    vEraseSize      = Cfg.EraseSize;
    if (Cfg.WriteSize == 0)
        vWriteSize = DISKIO_SECT_SIZE;
    else
        vWriteSize      = Cfg.WriteSize;
    vTotalSize      = Cfg.TotalSize;
    vAddrSize       = Cfg.AddrSize;
    vpInterf        = pInterf;

    uint32_t d = ReadId();

    if (pCacheBlk && NbCacheBlk > 0)
    {
        SetCache(pCacheBlk, NbCacheBlk);
    }

    return true;
}

uint32_t FlashDiskIO::ReadId()
{
	uint32_t id = -1;
	uint8_t cmd;

	WaitReady();

	cmd = FLASH_CMD_READID;

	vpInterf->StartRx(vDevNo);
    vpInterf->TxData(&cmd, 1);
    vpInterf->RxData((uint8_t*)&id, 4);
    vpInterf->StopRx();

    return id;
}

uint8_t FlashDiskIO::ReadStatus()
{
    uint8_t d;

    d = FLASH_CMD_READSTATUS;
    vpInterf->StartRx(vDevNo);
    vpInterf->TxData(&d, 1);
    vpInterf->RxData(&d, 1);
    vpInterf->StopRx();

    return d;
}

bool FlashDiskIO::WaitReady(uint32_t Timeout, uint32_t usRtyDelay)
{
    uint8_t d;

    do {
        d = FLASH_CMD_READSTATUS;
        vpInterf->StartRx(vDevNo);
        vpInterf->TxData(&d, 1);
        vpInterf->RxData(&d, 1);
        vpInterf->StopRx();
        if (!(d & FLASH_STATUS_WIP))
            return true;

        if (usRtyDelay > 0)
        {
            if (vpWaitCB)
            	vpWaitCB(vDevNo, vpInterf);
            else
            	usDelay(usRtyDelay);
        }

    } while (Timeout-- > 0);

    return false;
}

void FlashDiskIO::WriteDisable()
{
    uint8_t d = FLASH_CMD_WRDISABLE;
    vpInterf->Tx(vDevNo, &d, 1);
}

bool FlashDiskIO::WriteEnable(uint32_t Timeout)
{
    uint8_t d;

    WaitReady(Timeout);

    d = FLASH_CMD_WRENABLE;
    vpInterf->Tx(vDevNo, &d, 1);

    return false;
}

void FlashDiskIO::Erase()
{
    uint8_t d;

    WriteEnable();
    WaitReady();

    d = FLASH_CMD_BULK_ERASE;

    vpInterf->Tx(vDevNo, &d, 1);

    // This is a long wait polling at every second only
    WaitReady(-1, 1000000);
    WriteDisable();
}

/**
 * Erase Flash block.
 *
 * @param   BlkNo   : Starting block number to erase.
 *          NbBlk   : Number of consecutive blocks to erase
 */
void FlashDiskIO::EraseBlock(uint32_t BlkNo, int NbBlk)
{
    uint8_t d[8];

    BlkNo *= vEraseSize;
    uint8_t *p = (uint8_t*)BlkNo;

    d[0] = FLASH_CMD_BLOCK_ERASE;

    WriteEnable();

    for (int i = 0; i < NbBlk; i++)
    {
        for (int i = 1; i <= vAddrSize; i++)
            d[i] = p[vAddrSize - i];
        WaitReady(-1, 10);
        vpInterf->Tx(vDevNo, d, vAddrSize + 1);
        BlkNo += vEraseSize;
    }
    WriteDisable();
}

/**
 * Read one sector from physical device
 */
bool FlashDiskIO::SectRead(uint32_t SectNo, uint8_t *pBuff)
{
    uint8_t d[9];
    uint32_t addr = SectNo * DISKIO_SECT_SIZE;
    uint8_t *p = (uint8_t*)&addr;
    int cnt = DISKIO_SECT_SIZE;

    // Makesure there is no write access pending
    WaitReady(100000);

    d[0] = FLASH_CMD_READ;

    while (cnt > 0)
    {
        for (int i = 1; i <= vAddrSize; i++)
            d[i] = p[vAddrSize - i];

        vpInterf->StartRx(vDevNo);
        vpInterf->TxData((uint8_t*)d, vAddrSize + 1);
        int l = vpInterf->RxData(pBuff, DISKIO_SECT_SIZE);
        vpInterf->StopRx();
        if (l <= 0)
            return false;
        cnt -= l;
        addr += l;
        pBuff += l;
    }
    //printf("RSect : %d 0x%02x 0x%02x 0x%02x 0x%02x\r\n", SectNo, pBuff[0], pBuff[1], pBuff[2], pBuff[3]);
    return true;
}

/**
 * Write one sector to physical device
 */
bool FlashDiskIO::SectWrite(uint32_t SectNo, uint8_t *pData)
{
    uint8_t d[9];
    uint32_t addr = SectNo * DISKIO_SECT_SIZE;
    uint8_t *p = (uint8_t*)&addr;

    int cnt = 0;

   // printf("Sect : %d 0x%02x 0x%02x 0x%02x 0x%02x\r\n", SectNo, pData[0], pData[1], pData[2], pData[3]);
    d[0] = FLASH_CMD_WRITE;

    cnt = DISKIO_SECT_SIZE;
    while (cnt > 0)
    {
        for (int i = 1; i <= vAddrSize; i++)
            d[i] = p[vAddrSize - i];

        int l = min(cnt, vWriteSize);

        WaitReady();

        // Some Flash will reset write enable bit at completion
        // when page size is less than 512 bytes.
        // We need to set it again
        WriteEnable();

        vpInterf->StartTx(vDevNo);
        vpInterf->TxData((uint8_t*)d, vAddrSize + 1);
        l = vpInterf->TxData(pData, l);
        vpInterf->StopTx();
        if (l <= 0)
            return false;
        cnt -= l;
        pData += l;
        addr += l;
    }
    WriteDisable();

    return true;
}

<commit_msg>Fixe erase block not erasing properly.<commit_after>/*--------------------------------------------------------------------------
File   : diskio_flash.cpp

Author : Hoang Nguyen Hoan          Aug. 30, 2016

Desc   : Generic flash disk I/O driver class

Copyright (c) 2016, Motsai, all rights reserved

Permission to use, copy, modify, and distribute this software for any purpose
with or without fee is hereby granted, provided that the above copyright
notice and this permission notice appear in all copies, and none of the
names : I-SYST or its contributors may be used to endorse or
promote products derived from this software without specific prior written
permission.

For info or contributing contact : nh.hoang at motsai dot com

THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

----------------------------------------------------------------------------
Modified by          Date              Description

----------------------------------------------------------------------------*/
#include <stdio.h>

#include "diskio_flash.h"
#include "idelay.h"

FlashDiskIO::FlashDiskIO() : DiskIO()
{
	vpWaitCB = NULL;
	vpInterf = NULL;
}

bool FlashDiskIO::Init(FLASHDISKIO_CFG &Cfg, SerialIntrf *pInterf,
                       DISKIO_CACHE_DESC *pCacheBlk, int NbCacheBlk)
{
    if (pInterf == NULL)
        return false;

    if (Cfg.pInitCB)
    {
        if (Cfg.pInitCB(Cfg.DevNo, pInterf) == false)
            return false;
    }

    if (Cfg.pWaitCB)
    	vpWaitCB = Cfg.pWaitCB;

    vDevNo          = Cfg.DevNo;
    vEraseSize      = Cfg.EraseSize;
    if (Cfg.WriteSize == 0)
        vWriteSize = DISKIO_SECT_SIZE;
    else
        vWriteSize      = Cfg.WriteSize;
    vTotalSize      = Cfg.TotalSize;
    vAddrSize       = Cfg.AddrSize;
    vpInterf        = pInterf;

    uint32_t d = ReadId();

    if (pCacheBlk && NbCacheBlk > 0)
    {
        SetCache(pCacheBlk, NbCacheBlk);
    }

    return true;
}

uint32_t FlashDiskIO::ReadId()
{
	uint32_t id = -1;
	uint8_t cmd;

	WaitReady();

	cmd = FLASH_CMD_READID;

	vpInterf->StartRx(vDevNo);
    vpInterf->TxData(&cmd, 1);
    vpInterf->RxData((uint8_t*)&id, 4);
    vpInterf->StopRx();

    return id;
}

uint8_t FlashDiskIO::ReadStatus()
{
    uint8_t d;

    d = FLASH_CMD_READSTATUS;
    vpInterf->StartRx(vDevNo);
    vpInterf->TxData(&d, 1);
    vpInterf->RxData(&d, 1);
    vpInterf->StopRx();

    return d;
}

bool FlashDiskIO::WaitReady(uint32_t Timeout, uint32_t usRtyDelay)
{
    uint8_t d;

    do {
        d = FLASH_CMD_READSTATUS;
        vpInterf->StartRx(vDevNo);
        vpInterf->TxData(&d, 1);
        vpInterf->RxData(&d, 1);
        vpInterf->StopRx();
        if (!(d & FLASH_STATUS_WIP))
            return true;

        if (usRtyDelay > 0)
        {
            if (vpWaitCB)
            	vpWaitCB(vDevNo, vpInterf);
            else
            	usDelay(usRtyDelay);
        }

    } while (Timeout-- > 0);

    return false;
}

void FlashDiskIO::WriteDisable()
{
    uint8_t d = FLASH_CMD_WRDISABLE;
    vpInterf->Tx(vDevNo, &d, 1);
}

bool FlashDiskIO::WriteEnable(uint32_t Timeout)
{
    uint8_t d;

    WaitReady(Timeout);

    d = FLASH_CMD_WRENABLE;
    vpInterf->Tx(vDevNo, &d, 1);

    return false;
}

void FlashDiskIO::Erase()
{
    uint8_t d;

    WriteEnable();
    WaitReady();

    d = FLASH_CMD_BULK_ERASE;

    vpInterf->Tx(vDevNo, &d, 1);

    // This is a long wait polling at every second only
    WaitReady(-1, 1000000);
    WriteDisable();
}

/**
 * Erase Flash block.
 *
 * @param   BlkNo   : Starting block number to erase.
 *          NbBlk   : Number of consecutive blocks to erase
 */
void FlashDiskIO::EraseBlock(uint32_t BlkNo, int NbBlk)
{
    uint8_t d[8];

    BlkNo *= vEraseSize;
    uint8_t *p = (uint8_t*)&BlkNo;

    d[0] = FLASH_CMD_BLOCK_ERASE;

    for (int k = 0; k < NbBlk; k++)
    {
        for (int i = 1; i <= vAddrSize; i++)
            d[i] = p[vAddrSize - i];
        WaitReady(-1, 10);

        // Need to re-enable write here, because some flash
        // devices may reset write enable after a write
        // complete
        WriteEnable();

        vpInterf->Tx(vDevNo, d, vAddrSize + 1);
        BlkNo += vEraseSize;
    }
    WriteDisable();
}

/**
 * Read one sector from physical device
 */
bool FlashDiskIO::SectRead(uint32_t SectNo, uint8_t *pBuff)
{
    uint8_t d[9];
    uint32_t addr = SectNo * DISKIO_SECT_SIZE;
    uint8_t *p = (uint8_t*)&addr;
    int cnt = DISKIO_SECT_SIZE;

    // Makesure there is no write access pending
    WaitReady(100000);

    d[0] = FLASH_CMD_READ;

    while (cnt > 0)
    {
        for (int i = 1; i <= vAddrSize; i++)
            d[i] = p[vAddrSize - i];

        vpInterf->StartRx(vDevNo);
        vpInterf->TxData((uint8_t*)d, vAddrSize + 1);
        int l = vpInterf->RxData(pBuff, DISKIO_SECT_SIZE);
        vpInterf->StopRx();
        if (l <= 0)
            return false;
        cnt -= l;
        addr += l;
        pBuff += l;
    }
    //printf("RSect : %d 0x%02x 0x%02x 0x%02x 0x%02x\r\n", SectNo, pBuff[0], pBuff[1], pBuff[2], pBuff[3]);
    return true;
}

/**
 * Write one sector to physical device
 */
bool FlashDiskIO::SectWrite(uint32_t SectNo, uint8_t *pData)
{
    uint8_t d[9];
    uint32_t addr = SectNo * DISKIO_SECT_SIZE;
    uint8_t *p = (uint8_t*)&addr;

    int cnt = 0;

   // printf("Sect : %d 0x%02x 0x%02x 0x%02x 0x%02x\r\n", SectNo, pData[0], pData[1], pData[2], pData[3]);
    d[0] = FLASH_CMD_WRITE;

    cnt = DISKIO_SECT_SIZE;
    while (cnt > 0)
    {
        for (int i = 1; i <= vAddrSize; i++)
            d[i] = p[vAddrSize - i];

        int l = min(cnt, vWriteSize);

        WaitReady();

        // Some Flash will reset write enable bit at completion
        // when page size is less than 512 bytes.
        // We need to set it again
        WriteEnable();

        vpInterf->StartTx(vDevNo);
        vpInterf->TxData((uint8_t*)d, vAddrSize + 1);
        l = vpInterf->TxData(pData, l);
        vpInterf->StopTx();
        if (l <= 0)
            return false;
        cnt -= l;
        pData += l;
        addr += l;
    }
    WriteDisable();

    return true;
}

<|endoftext|>