<commit_before>
#include <GL/glew.h>
#include <GLFW/glfw3.h>

#include <fstream>
#include <functional>
#include <iostream>
#include <map>
#include <stdexcept>
#include <streambuf>
#include <vector>

namespace graphics {

class ProgramBuilder {
public:
  ProgramBuilder() : programHandle(glCreateProgram()) {}

  ProgramBuilder AddVertexShader(std::string src) {
    shaders.insert({glCreateShader(GL_VERTEX_SHADER), src});
    return *this;
  }

  ProgramBuilder AddFragmentShader(std::string src) {
    shaders.insert({glCreateShader(GL_FRAGMENT_SHADER), src});
    return *this;
  }

  GLuint Build() {
    CompileShaders();
    DeleteShaders();
    return programHandle;
  }

private:
  GLuint programHandle;
  std::map<GLuint, std::string> shaders;

  void CompileShaders() {
    // Error checking wrapper function
    auto check = [=](void (*f)(GLuint, GLenum, GLint *), GLuint handle,
                     std::string err) {
      GLint res;
      f(handle, GL_COMPILE_STATUS, &res);
      int ll;
      f(handle, GL_INFO_LOG_LENGTH, &ll);
      std::cout << "log length: " << ll << std::endl;
      if (!res) {
        auto v = std::vector<char>(ll + 1);
        glGetShaderInfoLog(handle, ll, nullptr, v.data());
        throw std::runtime_error(err + ": " + std::string(v.begin(), v.end()));
      }
    };

    for (auto s : shaders) {
      auto sc = s.second.c_str();
      glShaderSource(s.first, 1, &sc, nullptr);
      glCompileShader(s.first);
      check(glGetShaderiv, s.first, "error compiling shader");
      glAttachShader(programHandle, s.first);
    }

    glLinkProgram(programHandle);
    check(glGetProgramiv, programHandle, "error linking program");
  }

  void DeleteShaders() {
    for (auto s : shaders) {
      glDetachShader(programHandle, s.first);
      glDeleteShader(s.first);
    }
  }
};

} // namespace graphics

int main() {
  if (!glfwInit()) {
    throw std::runtime_error("glfw initialization failed");
  }

  glfwWindowHint(GLFW_SAMPLES, 4);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
  glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
  glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

  auto window = glfwCreateWindow(1024, 768, "Tutorial 01", nullptr, nullptr);
  if (window == nullptr) {
    glfwTerminate();
    throw std::runtime_error(
        "failed to open glfw window. If you have an Intel GPU, they "
        "are not 3.3 compatible. Try the 2.1 version of the tutorials.");
  }
  glfwMakeContextCurrent(window);
  glewExperimental = true;
  if (glewInit() != GLEW_OK) {
    throw std::runtime_error("failed to initialize glew");
  }

  glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);

  GLuint vaID;
  glGenVertexArrays(1, &vaID);
  glBindVertexArray(vaID);

  std::vector<GLfloat> vertices = {-1, -1, 0, 1, -1, 0, 0, 1, 0};

  GLuint buf;
  glGenBuffers(1, &buf);
  glBindBuffer(GL_ARRAY_BUFFER, buf);
  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * vertices.size(),
               vertices.data(), GL_STATIC_DRAW);

  auto readFile = [=](std::string s) {
    auto f = std::ifstream(s);
    return std::string(std::istreambuf_iterator<char>(f),
                       std::istreambuf_iterator<char>());
  };

  auto programHandle = graphics::ProgramBuilder()
                           .AddVertexShader(readFile("shaders/triangle.vert"))
                           .AddFragmentShader(readFile("shaders/triangle.frag"))
                           .Build();

  do {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glEnableVertexAttribArray(0);
    glBindBuffer(GL_ARRAY_BUFFER, buf);
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, static_cast<void *>(0));
    glUseProgram(programHandle);
    glDrawArrays(GL_TRIANGLES, 0, 3);
    glDisableVertexAttribArray(0);

    glfwSwapBuffers(window);
    glfwPollEvents();

  } // Check if the ESC key was pressed or the window was closed
  while (glfwGetKey(window, GLFW_KEY_ESCAPE) != GLFW_PRESS &&
         !glfwWindowShouldClose(window));
}
<commit_msg>update<commit_after>
#include <GL/glew.h>
#include <GLFW/glfw3.h>

#include <fstream>
#include <functional>
#include <iostream>
#include <map>
#include <stdexcept>
#include <streambuf>
#include <vector>

namespace graphics {

class ProgramBuilder {
public:
  ProgramBuilder() : programHandle(glCreateProgram()) {}

  ProgramBuilder AddVertexShader(std::string src) {
    shaders.insert({glCreateShader(GL_VERTEX_SHADER), src});
    return *this;
  }

  ProgramBuilder AddFragmentShader(std::string src) {
    shaders.insert({glCreateShader(GL_FRAGMENT_SHADER), src});
    return *this;
  }

  GLuint Build() {
    CompileShaders();
    DeleteShaders();
    return programHandle;
  }

private:
  GLuint programHandle;
  std::map<GLuint, std::string> shaders;

  void CompileShaders() {
    // Error checking wrapper function
    auto check = [=](void (*f)(GLuint, GLenum, GLint *), GLuint handle,
                     std::string err) {
      GLint res;
      f(handle, GL_COMPILE_STATUS, &res);
      int ll;
      f(handle, GL_INFO_LOG_LENGTH, &ll);
      std::cout << "log length: " << ll << std::endl;
      if (!res) {
        auto v = std::vector<char>(ll + 1);
        glGetShaderInfoLog(handle, ll, nullptr, v.data());
        throw std::runtime_error(err + ": " + std::string(v.begin(), v.end()));
      }
    };

    for (auto s : shaders) {
      auto sc = s.second.c_str();
      glShaderSource(s.first, 1, &sc, nullptr);
      glCompileShader(s.first);
      check(glGetShaderiv, s.first, "error compiling shader");
      glAttachShader(programHandle, s.first);
    }

    glLinkProgram(programHandle);
    check(glGetProgramiv, programHandle, "error linking program");
  }

  void DeleteShaders() {
    for (auto s : shaders) {
      glDetachShader(programHandle, s.first);
      glDeleteShader(s.first);
    }
  }
};

} // namespace graphics

int main() {
  if (!glfwInit()) {
    throw std::runtime_error("glfw initialization failed");
  }

  glfwWindowHint(GLFW_SAMPLES, 4);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
  glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
  glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

  auto window = glfwCreateWindow(1024, 768, "Tutorial 01", nullptr, nullptr);
  if (window == nullptr) {
    glfwTerminate();
    throw std::runtime_error(
        "failed to open glfw window. If you have an Intel GPU, they "
        "are not 3.3 compatible. Try the 2.1 version of the tutorials.");
  }
  glfwMakeContextCurrent(window);
  glewExperimental = true;
  if (glewInit() != GLEW_OK) {
    throw std::runtime_error("failed to initialize glew");
  }

  glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);

  GLuint vaID;
  glGenVertexArrays(1, &vaID);
  glBindVertexArray(vaID);

  std::vector<GLfloat> vertices = {-1, -1, 0, 1, -1, 0, 0, 1, 0};

  GLuint buf;
  glGenBuffers(1, &buf);
  glBindBuffer(GL_ARRAY_BUFFER, buf);
  glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * vertices.size(),
               vertices.data(), GL_STATIC_DRAW);

  auto readFile = [=](std::string s) {
    auto f = std::ifstream(s);
    return std::string(std::istreambuf_iterator<char>(f),
                       std::istreambuf_iterator<char>());
  };

  auto programHandle = graphics::ProgramBuilder()
                           .AddVertexShader(readFile("shaders/triangle.vert"))
                           .AddFragmentShader(readFile("shaders/triangle.frag"))
                           .Build();

  for (;;) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glEnableVertexAttribArray(0);
    glBindBuffer(GL_ARRAY_BUFFER, buf);
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, static_cast<void *>(0));
    glUseProgram(programHandle);
    glDrawArrays(GL_TRIANGLES, 0, 3);
    glDisableVertexAttribArray(0);

    glfwSwapBuffers(window);
    glfwPollEvents();

    if (glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS ||
        glfwWindowShouldClose(window)) {
      break;
    }
  }
}
<|endoftext|>


<commit_before>// PointToPointSpring.cpp
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright (c)  2009, Stanford University. All rights reserved.
* Use of the OpenSim software in source form is permitted provided that the following
* conditions are met:
* 	1. The software is used only for non-commercial research and education. It may not
*     be used in relation to any commercial activity.
* 	2. The software is not distributed or redistributed.  Software distribution is allowed
*     only through https://simtk.org/home/opensim.
* 	3. Use of the OpenSim software or derivatives must be acknowledged in all publications,
*      presentations, or documents describing work in which OpenSim or derivatives are used.
* 	4. Credits to developers may not be removed from executables
*     created from modifications of the source.
* 	5. Modifications of source code must retain the above copyright notice, this list of
*     conditions and the following disclaimer.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 COPYRIGHT OWNER 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;
*  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR BUSINESS INTERRUPTION) 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.
*/

/*
 * Author: Ajay Seth
 */

//=============================================================================
// INCLUDES
//=============================================================================
#include "PointToPointSpring.h"
#include <OpenSim/Simulation/Model/BodySet.h>
#include <OpenSim/Simulation/Model/Model.h>

using namespace OpenSim;
using namespace std;

//=============================================================================
// STATICS
//=============================================================================

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================

//_____________________________________________________________________________
//Default constructor.
PointToPointSpring::PointToPointSpring()
{
	setNull();
    constructProperties();
}
//_____________________________________________________________________________
// Convenience constructor for API users.
PointToPointSpring::PointToPointSpring(string body1Name, SimTK::Vec3 point1,
									   string body2Name, SimTK::Vec3 point2,
                                       double stiffness, double restlength )
{
	setNull();
    constructProperties();

    // Set properties to the passed-in values.
	setBody1Name(body1Name);
	setBody2Name(body2Name);

	setPoint1(point1);
	setPoint2(point2);

	setStiffness(stiffness);
	setRestlength(restlength);
}

//=============================================================================
// CONSTRUCTION
//=============================================================================
//_____________________________________________________________________________
/**
 * Set the data members of this force to their null values.
 */
void PointToPointSpring::setNull()
{
}

//_____________________________________________________________________________
/**
 * Construct properties and initialize to their default values.
 */
void PointToPointSpring::constructProperties()
{
	constructProperty_body1();
	constructProperty_body2();

	const SimTK::Vec3 bodyOrigin(0.0, 0.0, 0.0);
	constructProperty_point1(bodyOrigin);
	constructProperty_point2(bodyOrigin);

	constructProperty_stiffness(1.0);
	constructProperty_rest_length(0.0);
}

//_____________________________________________________________________________
/**
 * Get the visible object used to represent the spring.
 */
VisibleObject* PointToPointSpring::getDisplayer() const
{
	return const_cast<VisibleObject*>(&_displayer);
}

void PointToPointSpring::updateDisplayer(const SimTK::State& s)
{
	SimTK::Vec3 globalLocation1, globalLocation2;
	const OpenSim::Body& body1 = _model->getBodySet().get(getBody1Name());
	const OpenSim::Body& body2 = _model->getBodySet().get(getBody2Name());
	_model->getSimbodyEngine().transformPosition(s, body1, getPoint1(),
                                                 globalLocation1);
	_model->getSimbodyEngine().transformPosition(s, body2, getPoint2(),
                                                 globalLocation2);
	((LineGeometry *)_displayer.getGeometry(0))->
        setPoints(globalLocation1, globalLocation2);
}

void PointToPointSpring::updateGeometry(const SimTK::State& s)
{
	if (_displayer.countGeometry()==0){
		Geometry *g = new LineGeometry();
		g->setFixed(false);
		_displayer.addGeometry(g);
	}
	updateDisplayer(s);
}

//=============================================================================
// Connect this force element to the rest of the model.
//=============================================================================
void PointToPointSpring::connectToModel(Model& model)
{
	Super::connectToModel(model);	// Let base class connect first.

	string errorMessage;

	const string& body1Name = getBody1Name();
	const string& body2Name = getBody2Name();

	// Look up the two bodies being connected together by name in the model
	if (!model.updBodySet().contains(body1Name)) {
		errorMessage = "Invalid body1 (" + body1Name + ") specified for Spring "
                        + getName();
		throw (Exception(errorMessage));
	}
	if (!model.updBodySet().contains(body2Name)) {
		errorMessage = "Invalid body2 (" + body2Name + ") specified for Spring "
                        + getName();
		throw (Exception(errorMessage));
	}

	if(getName() == "")
		setName("spring");
}

//=============================================================================
// Create the underlying system component(s)
//=============================================================================
void PointToPointSpring::addToSystem(SimTK::MultibodySystem& system) const
{
	Super::addToSystem(system);    // Base class first.

	Body& body1 = _model->updBodySet().get(getBody1Name());
	Body& body2 = _model->updBodySet().get(getBody2Name());

	// Get underlying mobilized bodies
	SimTK::MobilizedBody b1 = _model->getMatterSubsystem()
                                        .getMobilizedBody(body1.getIndex());
	SimTK::MobilizedBody b2 = _model->getMatterSubsystem()
                                        .getMobilizedBody(body2.getIndex());

    // Now create a Simbody Force::TwoPointLinearSpring
    SimTK::Force::TwoPointLinearSpring simtkSpring
       (_model->updForceSubsystem(), b1, getPoint1(), b2, getPoint2(),
        getStiffness(), getRestlength());

    // Beyond the const Component get the index so we can access the
    // SimTK::Force later.
	PointToPointSpring* mutableThis = const_cast<PointToPointSpring *>(this);
	mutableThis->_index = simtkSpring.getForceIndex();
}

//=============================================================================
// Reporting
//=============================================================================
// Provide names of the quantities (column labels) of the force value(s)
// reported.
OpenSim::Array<std::string> PointToPointSpring::getRecordLabels() const
{
	const string& body1Name = getBody1Name();
	const string& body2Name = getBody2Name();

	OpenSim::Array<std::string> labels("");
	labels.append(getName()+"."+body1Name+".force.X");
	labels.append(getName()+"."+body1Name+".force.Y");
	labels.append(getName()+"."+body1Name+".force.Z");
	labels.append(getName()+"."+body1Name+".point.X");
	labels.append(getName()+"."+body1Name+".point.Y");
	labels.append(getName()+"."+body1Name+".point.Z");
	labels.append(getName()+"."+body2Name+".force.X");
	labels.append(getName()+"."+body2Name+".force.Y");
	labels.append(getName()+"."+body2Name+".force.Z");
	labels.append(getName()+"."+body2Name+".point.X");
	labels.append(getName()+"."+body2Name+".point.Y");
	labels.append(getName()+"."+body2Name+".point.Z");

	return labels;
}

// Provide the value(s) to be reported that correspond to the labels.
OpenSim::Array<double> PointToPointSpring::
getRecordValues(const SimTK::State& state) const
{
	OpenSim::Array<double> values(1);

	const SimTK::Force::TwoPointLinearSpring&
        simtkSpring = SimTK::Force::TwoPointLinearSpring::downcast
                                (_model->getForceSubsystem().getForce(_index));

	SimTK::Vector_<SimTK::SpatialVec> bodyForces(0);
	SimTK::Vector_<SimTK::Vec3> particleForces(0);
	SimTK::Vector mobilityForces(0);

	const Body& body1 = _model->getBodySet().get(getBody1Name());
	const Body& body2 = _model->getBodySet().get(getBody2Name());

	//get the net force added to the system contributed by the Spring
	simtkSpring.calcForceContribution(state, bodyForces, particleForces,
                                      mobilityForces);
	SimTK::Vec3 forces = bodyForces(body1.getIndex())[1];
	values.append(3, &forces[0]);

	SimTK::Vec3 gpoint(0);
	_model->getSimbodyEngine().getPosition(state, body1, getPoint1(), gpoint);
	values.append(3, &gpoint[0]);

	forces = bodyForces(body2.getIndex())[1];
	values.append(3, &forces[0]);

	_model->getSimbodyEngine().getPosition(state, body2, getPoint2(), gpoint);
	values.append(3, &gpoint[0]);

	return values;
}

<commit_msg>Fix display of PointToPointSpring in GUI<commit_after>// PointToPointSpring.cpp
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright (c)  2009, Stanford University. All rights reserved.
* Use of the OpenSim software in source form is permitted provided that the following
* conditions are met:
* 	1. The software is used only for non-commercial research and education. It may not
*     be used in relation to any commercial activity.
* 	2. The software is not distributed or redistributed.  Software distribution is allowed
*     only through https://simtk.org/home/opensim.
* 	3. Use of the OpenSim software or derivatives must be acknowledged in all publications,
*      presentations, or documents describing work in which OpenSim or derivatives are used.
* 	4. Credits to developers may not be removed from executables
*     created from modifications of the source.
* 	5. Modifications of source code must retain the above copyright notice, this list of
*     conditions and the following disclaimer.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 COPYRIGHT OWNER 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;
*  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR BUSINESS INTERRUPTION) 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.
*/

/*
 * Author: Ajay Seth
 */

//=============================================================================
// INCLUDES
//=============================================================================
#include "PointToPointSpring.h"
#include <OpenSim/Simulation/Model/BodySet.h>
#include <OpenSim/Simulation/Model/Model.h>

using namespace OpenSim;
using namespace std;

//=============================================================================
// STATICS
//=============================================================================

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================

//_____________________________________________________________________________
//Default constructor.
PointToPointSpring::PointToPointSpring()
{
	setNull();
    constructProperties();
}
//_____________________________________________________________________________
// Convenience constructor for API users.
PointToPointSpring::PointToPointSpring(string body1Name, SimTK::Vec3 point1,
									   string body2Name, SimTK::Vec3 point2,
                                       double stiffness, double restlength )
{
	setNull();
    constructProperties();

    // Set properties to the passed-in values.
	setBody1Name(body1Name);
	setBody2Name(body2Name);

	setPoint1(point1);
	setPoint2(point2);

	setStiffness(stiffness);
	setRestlength(restlength);
}

//=============================================================================
// CONSTRUCTION
//=============================================================================
//_____________________________________________________________________________
/**
 * Set the data members of this force to their null values.
 */
void PointToPointSpring::setNull()
{
}

//_____________________________________________________________________________
/**
 * Construct properties and initialize to their default values.
 */
void PointToPointSpring::constructProperties()
{
	constructProperty_body1();
	constructProperty_body2();

	const SimTK::Vec3 bodyOrigin(0.0, 0.0, 0.0);
	constructProperty_point1(bodyOrigin);
	constructProperty_point2(bodyOrigin);

	constructProperty_stiffness(1.0);
	constructProperty_rest_length(0.0);
}

//_____________________________________________________________________________
/**
 * Get the visible object used to represent the spring.
 */
VisibleObject* PointToPointSpring::getDisplayer() const
{
	return const_cast<VisibleObject*>(&_displayer);
}

void PointToPointSpring::updateDisplayer(const SimTK::State& s)
{
	SimTK::Vec3 globalLocation1, globalLocation2;
	const OpenSim::Body& body1 = _model->getBodySet().get(getBody1Name());
	const OpenSim::Body& body2 = _model->getBodySet().get(getBody2Name());
	_model->getSimbodyEngine().transformPosition(s, body1, getPoint1(),
                                                 globalLocation1);
	_model->getSimbodyEngine().transformPosition(s, body2, getPoint2(),
                                                 globalLocation2);

	if (_displayer.countGeometry()==0){
		Geometry *g = new LineGeometry();
		g->setFixed(false);
		_displayer.addGeometry(g);
	}
	((LineGeometry *)_displayer.getGeometry(0))->
        setPoints(globalLocation1, globalLocation2);
}

void PointToPointSpring::updateGeometry(const SimTK::State& s)
{
	if (_displayer.countGeometry()==0){
		Geometry *g = new LineGeometry();
		g->setFixed(false);
		_displayer.addGeometry(g);
	}
	updateDisplayer(s);
}

//=============================================================================
// Connect this force element to the rest of the model.
//=============================================================================
void PointToPointSpring::connectToModel(Model& model)
{
	Super::connectToModel(model);	// Let base class connect first.

	string errorMessage;

	const string& body1Name = getBody1Name();
	const string& body2Name = getBody2Name();

	// Look up the two bodies being connected together by name in the model
	if (!model.updBodySet().contains(body1Name)) {
		errorMessage = "Invalid body1 (" + body1Name + ") specified for Spring "
                        + getName();
		throw (Exception(errorMessage));
	}
	if (!model.updBodySet().contains(body2Name)) {
		errorMessage = "Invalid body2 (" + body2Name + ") specified for Spring "
                        + getName();
		throw (Exception(errorMessage));
	}

	if(getName() == "")
		setName("spring");
}

//=============================================================================
// Create the underlying system component(s)
//=============================================================================
void PointToPointSpring::addToSystem(SimTK::MultibodySystem& system) const
{
	Super::addToSystem(system);    // Base class first.

	Body& body1 = _model->updBodySet().get(getBody1Name());
	Body& body2 = _model->updBodySet().get(getBody2Name());

	// Get underlying mobilized bodies
	SimTK::MobilizedBody b1 = _model->getMatterSubsystem()
                                        .getMobilizedBody(body1.getIndex());
	SimTK::MobilizedBody b2 = _model->getMatterSubsystem()
                                        .getMobilizedBody(body2.getIndex());

    // Now create a Simbody Force::TwoPointLinearSpring
    SimTK::Force::TwoPointLinearSpring simtkSpring
       (_model->updForceSubsystem(), b1, getPoint1(), b2, getPoint2(),
        getStiffness(), getRestlength());

    // Beyond the const Component get the index so we can access the
    // SimTK::Force later.
	PointToPointSpring* mutableThis = const_cast<PointToPointSpring *>(this);
	mutableThis->_index = simtkSpring.getForceIndex();
}

//=============================================================================
// Reporting
//=============================================================================
// Provide names of the quantities (column labels) of the force value(s)
// reported.
OpenSim::Array<std::string> PointToPointSpring::getRecordLabels() const
{
	const string& body1Name = getBody1Name();
	const string& body2Name = getBody2Name();

	OpenSim::Array<std::string> labels("");
	labels.append(getName()+"."+body1Name+".force.X");
	labels.append(getName()+"."+body1Name+".force.Y");
	labels.append(getName()+"."+body1Name+".force.Z");
	labels.append(getName()+"."+body1Name+".point.X");
	labels.append(getName()+"."+body1Name+".point.Y");
	labels.append(getName()+"."+body1Name+".point.Z");
	labels.append(getName()+"."+body2Name+".force.X");
	labels.append(getName()+"."+body2Name+".force.Y");
	labels.append(getName()+"."+body2Name+".force.Z");
	labels.append(getName()+"."+body2Name+".point.X");
	labels.append(getName()+"."+body2Name+".point.Y");
	labels.append(getName()+"."+body2Name+".point.Z");

	return labels;
}

// Provide the value(s) to be reported that correspond to the labels.
OpenSim::Array<double> PointToPointSpring::
getRecordValues(const SimTK::State& state) const
{
	OpenSim::Array<double> values(1);

	const SimTK::Force::TwoPointLinearSpring&
        simtkSpring = SimTK::Force::TwoPointLinearSpring::downcast
                                (_model->getForceSubsystem().getForce(_index));

	SimTK::Vector_<SimTK::SpatialVec> bodyForces(0);
	SimTK::Vector_<SimTK::Vec3> particleForces(0);
	SimTK::Vector mobilityForces(0);

	const Body& body1 = _model->getBodySet().get(getBody1Name());
	const Body& body2 = _model->getBodySet().get(getBody2Name());

	//get the net force added to the system contributed by the Spring
	simtkSpring.calcForceContribution(state, bodyForces, particleForces,
                                      mobilityForces);
	SimTK::Vec3 forces = bodyForces(body1.getIndex())[1];
	values.append(3, &forces[0]);

	SimTK::Vec3 gpoint(0);
	_model->getSimbodyEngine().getPosition(state, body1, getPoint1(), gpoint);
	values.append(3, &gpoint[0]);

	forces = bodyForces(body2.getIndex())[1];
	values.append(3, &forces[0]);

	_model->getSimbodyEngine().getPosition(state, body2, getPoint2(), gpoint);
	values.append(3, &gpoint[0]);

	return values;
}

<|endoftext|>


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

  Program:   Visualization Toolkit
  Module:    vtkTIFFWriter.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 1993-2002 Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm 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 notice for more information.

=========================================================================*/
#include "vtkTIFFWriter.h"

#include "vtkObjectFactory.h"
#include "vtkImageData.h"
#include "vtkPointData.h"
#include "vtkErrorCode.h"
#include <tiffio.h>

vtkCxxRevisionMacro(vtkTIFFWriter, "1.27");
vtkStandardNewMacro(vtkTIFFWriter);

//----------------------------------------------------------------------------
vtkTIFFWriter::vtkTIFFWriter()
{
  this->TIFFPtr = 0;
  this->Compression = vtkTIFFWriter::PackBits;
};

class vtkTIFFWriterIO
{
public:
  // Writing file no reading
  static tsize_t TIFFRead(thandle_t, tdata_t, tsize_t) { return 0; }

  // Write data
  static tsize_t TIFFWrite(thandle_t fd, tdata_t buf, tsize_t size)
    {
    ostream *out = reinterpret_cast<ostream *>(fd);
    out->write(static_cast<char *>(buf), size);
    return out->fail() ? static_cast<tsize_t>(0) : size;
    }

  static toff_t TIFFSeek(thandle_t fd, toff_t off, int whence)
    {
    ostream *out = reinterpret_cast<ostream *>(fd);

    streampos dir;
    //ios::seekdir dir;
    switch (whence)
      {
    case SEEK_SET:
      dir = ios::beg;
      break;

    case SEEK_END:
      dir = ios::end;
      break;

    case SEEK_CUR:
      dir = ios::cur;
      break;

    default:
      return out->tellp();
      }

    out->seekp(off, dir);
    return out->tellp();
    }

  // File will be closed by the superclass
  static int TIFFClose(thandle_t) { return true; }

  static toff_t TIFFSize(thandle_t fd)
    {
    ostream *out = reinterpret_cast<ostream *>(fd);
    out->seekp(0, ios::end);
    return out->tellp();
    }

  static int TIFFMapFile(thandle_t, tdata_t*, toff_t*) { return (0); }
  static void TIFFUnmapFile(thandle_t, tdata_t, toff_t) {}
};

//----------------------------------------------------------------------------
void vtkTIFFWriter::WriteFileHeader(ofstream *file, vtkImageData *data)
{
  int dims[3];
  int width, height;
  data->GetDimensions(dims);
  int scomponents = data->GetNumberOfScalarComponents();
  int stype = data->GetScalarType();
  double resolution = -1;
  uint32 rowsperstrip = (uint32) -1;

  int min0, min1, max0, max1, min2, max2;

  int bps;
  switch (stype)
    {
  case VTK_CHAR:
  case VTK_UNSIGNED_CHAR:
    bps = 8;
    break;
  case VTK_SHORT:
  case VTK_UNSIGNED_SHORT:
    bps = 16;
    break;
  default:
    vtkErrorMacro(<< "Unsupported data type: " << data->GetScalarTypeAsString());
    this->SetErrorCode(vtkErrorCode::FileFormatError);
    return;
    }

  int predictor = 0;

  // Find the length of the rows to write.
  data->GetWholeExtent(min0, max0, min1, max1, min2, max2);
  width = (max0 - min0 + 1);
  height = (max1 - min1 + 1);

  TIFF* tif = TIFFClientOpen(this->GetFileName(), "w",
    (thandle_t) file,
    vtkTIFFWriterIO::TIFFRead, vtkTIFFWriterIO::TIFFWrite,
    vtkTIFFWriterIO::TIFFSeek,
    vtkTIFFWriterIO::TIFFClose, vtkTIFFWriterIO::TIFFSize,
    vtkTIFFWriterIO::TIFFMapFile, vtkTIFFWriterIO::TIFFUnmapFile);
  if ( !tif )
    {
    this->TIFFPtr = 0;
    return;
    }
  this->TIFFPtr = tif;

  uint32 w = width;
  uint32 h = height;
  TIFFSetField(tif, TIFFTAG_IMAGEWIDTH, w);
  TIFFSetField(tif, TIFFTAG_IMAGELENGTH, h);
  TIFFSetField(tif, TIFFTAG_ORIENTATION, ORIENTATION_TOPLEFT);
  TIFFSetField(tif, TIFFTAG_SAMPLESPERPIXEL, scomponents);
  TIFFSetField(tif, TIFFTAG_BITSPERSAMPLE, bps); // Fix for stype
  TIFFSetField(tif, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG);

  if ( scomponents > 3 )
    {
    // if number of scalar components is greater than 3, that means we assume
    // there is alpha.
    uint16 extra_samples = scomponents-3;
    uint16 *sample_info = new uint16[scomponents-3];
    sample_info[0]=EXTRASAMPLE_ASSOCALPHA;
    int cc;
    for ( cc = 1; cc < scomponents-3; cc ++ )
      {
      sample_info[cc] = EXTRASAMPLE_UNSPECIFIED;
      }
    TIFFSetField(tif,TIFFTAG_EXTRASAMPLES,extra_samples,
      sample_info);
    delete [] sample_info;
    }

  int compression = COMPRESSION_PACKBITS;
  switch ( this->Compression )
    {
  case vtkTIFFWriter::PackBits: compression = COMPRESSION_PACKBITS; break;
  case vtkTIFFWriter::JPEG:     compression = COMPRESSION_JPEG; break;
  case vtkTIFFWriter::Deflate:  compression = COMPRESSION_DEFLATE; break;
  case vtkTIFFWriter::LZW:      compression = COMPRESSION_LZW; break;
  default: compression = COMPRESSION_NONE;
    }
  //compression = COMPRESSION_JPEG;
  TIFFSetField(tif, TIFFTAG_COMPRESSION, compression); // Fix for compression
  uint16 photometric = PHOTOMETRIC_RGB;
  if ( compression == COMPRESSION_JPEG )
    {
    TIFFSetField(tif, TIFFTAG_JPEGQUALITY, 75); // Parameter
    TIFFSetField(tif, TIFFTAG_JPEGCOLORMODE, JPEGCOLORMODE_RGB);
    photometric = PHOTOMETRIC_YCBCR;
    }
  else if ( compression == COMPRESSION_LZW )
    {
    predictor = 2;
    TIFFSetField(tif, TIFFTAG_PREDICTOR, predictor);
    vtkErrorMacro("LZW compression is patented outside US so it is disabled");
    }
  else if ( compression == COMPRESSION_DEFLATE )
    {
    predictor = 2;
    TIFFSetField(tif, TIFFTAG_PREDICTOR, predictor);
    }

  TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, photometric); // Fix for scomponents
  TIFFSetField(tif, TIFFTAG_ROWSPERSTRIP,
    TIFFDefaultStripSize(tif, rowsperstrip));
  if (resolution > 0) {
    TIFFSetField(tif, TIFFTAG_XRESOLUTION, resolution);
    TIFFSetField(tif, TIFFTAG_YRESOLUTION, resolution);
    TIFFSetField(tif, TIFFTAG_RESOLUTIONUNIT, RESUNIT_INCH);
  }
}

//----------------------------------------------------------------------------
void vtkTIFFWriter::WriteFile(ofstream *, vtkImageData *data,
  int extent[6])
{
  int idx1, idx2;
  int rowLength; // in bytes
  void *ptr;

  // Make sure we actually have data.
  if ( !data->GetPointData()->GetScalars())
    {
    vtkErrorMacro(<< "Could not get data from input.");
    return;
    }

  TIFF* tif = static_cast<TIFF*>(this->TIFFPtr);
  if ( !tif )
    {
    vtkErrorMacro("Problem writting trailer.");
    this->SetErrorCode(vtkErrorCode::FileFormatError);
    return;
    }

  // take into consideration the scalar type
  switch (data->GetScalarType())
    {
  case VTK_UNSIGNED_CHAR:
    rowLength = sizeof(unsigned char);
    break;
  case VTK_UNSIGNED_SHORT:
    rowLength = sizeof(unsigned short);
  default:
    vtkErrorMacro("TIFFWriter only accepts unsigned char scalars!");
    return;
    }
  rowLength *= data->GetNumberOfScalarComponents();
  rowLength *= (extent[1] - extent[0] + 1);

  int row = 0;
  for (idx2 = extent[4]; idx2 <= extent[5]; ++idx2)
    {
    for (idx1 = extent[3]; idx1 >= extent[2]; idx1--)
      {
      ptr = data->GetScalarPointer(extent[0], idx1, idx2);
      if ( TIFFWriteScanline(tif, static_cast<unsigned char*>(ptr), row, 0) < 0)
        {
        this->SetErrorCode(vtkErrorCode::OutOfDiskSpaceError);
        break;
        }
      row ++;
      }
    }
}

//----------------------------------------------------------------------------
void vtkTIFFWriter::WriteFileTrailer(ofstream *, vtkImageData *)
{
  TIFF* tif = static_cast<TIFF*>(this->TIFFPtr);
  if ( !tif )
    {
    vtkErrorMacro("Problem writting trailer.");
    this->SetErrorCode(vtkErrorCode::FileFormatError);
    }
  TIFFClose(tif);
  this->TIFFPtr = 0;
}
//----------------------------------------------------------------------------
void vtkTIFFWriter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
}
<commit_msg>BUG: fix compile errors for all compilers....<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkTIFFWriter.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 1993-2002 Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm 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 notice for more information.

=========================================================================*/
#include "vtkTIFFWriter.h"

#include "vtkObjectFactory.h"
#include "vtkImageData.h"
#include "vtkPointData.h"
#include "vtkErrorCode.h"
#include <tiffio.h>

vtkCxxRevisionMacro(vtkTIFFWriter, "1.28");
vtkStandardNewMacro(vtkTIFFWriter);

//----------------------------------------------------------------------------
vtkTIFFWriter::vtkTIFFWriter()
{
  this->TIFFPtr = 0;
  this->Compression = vtkTIFFWriter::PackBits;
};

class vtkTIFFWriterIO
{
public:
  // Writing file no reading
  static tsize_t TIFFRead(thandle_t, tdata_t, tsize_t) { return 0; }

  // Write data
  static tsize_t TIFFWrite(thandle_t fd, tdata_t buf, tsize_t size)
    {
    ostream *out = reinterpret_cast<ostream *>(fd);
    out->write(static_cast<char *>(buf), size);
    return out->fail() ? static_cast<tsize_t>(0) : size;
    }

  static toff_t TIFFSeek(thandle_t fd, toff_t off, int whence)
    {
    ostream *out = reinterpret_cast<ostream *>(fd);
    switch (whence)
      {
      case SEEK_SET:
        out->seekp(off, ios::beg);
        break;
      case SEEK_END:
        out->seekp(off, ios::end);
        break;
      case SEEK_CUR:
        out->seekp(off, ios::cur);
        break;
      default:
        return out->tellp();
      }
    return out->tellp();
    }

  // File will be closed by the superclass
  static int TIFFClose(thandle_t) { return true; }

  static toff_t TIFFSize(thandle_t fd)
    {
    ostream *out = reinterpret_cast<ostream *>(fd);
    out->seekp(0, ios::end);
    return out->tellp();
    }

  static int TIFFMapFile(thandle_t, tdata_t*, toff_t*) { return (0); }
  static void TIFFUnmapFile(thandle_t, tdata_t, toff_t) {}
};

//----------------------------------------------------------------------------
void vtkTIFFWriter::WriteFileHeader(ofstream *file, vtkImageData *data)
{
  int dims[3];
  int width, height;
  data->GetDimensions(dims);
  int scomponents = data->GetNumberOfScalarComponents();
  int stype = data->GetScalarType();
  double resolution = -1;
  uint32 rowsperstrip = (uint32) -1;

  int min0, min1, max0, max1, min2, max2;

  int bps;
  switch (stype)
    {
  case VTK_CHAR:
  case VTK_UNSIGNED_CHAR:
    bps = 8;
    break;
  case VTK_SHORT:
  case VTK_UNSIGNED_SHORT:
    bps = 16;
    break;
  default:
    vtkErrorMacro(<< "Unsupported data type: " << data->GetScalarTypeAsString());
    this->SetErrorCode(vtkErrorCode::FileFormatError);
    return;
    }

  int predictor = 0;

  // Find the length of the rows to write.
  data->GetWholeExtent(min0, max0, min1, max1, min2, max2);
  width = (max0 - min0 + 1);
  height = (max1 - min1 + 1);

  TIFF* tif = TIFFClientOpen(this->GetFileName(), "w",
    (thandle_t) file,
    vtkTIFFWriterIO::TIFFRead, vtkTIFFWriterIO::TIFFWrite,
    vtkTIFFWriterIO::TIFFSeek,
    vtkTIFFWriterIO::TIFFClose, vtkTIFFWriterIO::TIFFSize,
    vtkTIFFWriterIO::TIFFMapFile, vtkTIFFWriterIO::TIFFUnmapFile);
  if ( !tif )
    {
    this->TIFFPtr = 0;
    return;
    }
  this->TIFFPtr = tif;

  uint32 w = width;
  uint32 h = height;
  TIFFSetField(tif, TIFFTAG_IMAGEWIDTH, w);
  TIFFSetField(tif, TIFFTAG_IMAGELENGTH, h);
  TIFFSetField(tif, TIFFTAG_ORIENTATION, ORIENTATION_TOPLEFT);
  TIFFSetField(tif, TIFFTAG_SAMPLESPERPIXEL, scomponents);
  TIFFSetField(tif, TIFFTAG_BITSPERSAMPLE, bps); // Fix for stype
  TIFFSetField(tif, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG);

  if ( scomponents > 3 )
    {
    // if number of scalar components is greater than 3, that means we assume
    // there is alpha.
    uint16 extra_samples = scomponents-3;
    uint16 *sample_info = new uint16[scomponents-3];
    sample_info[0]=EXTRASAMPLE_ASSOCALPHA;
    int cc;
    for ( cc = 1; cc < scomponents-3; cc ++ )
      {
      sample_info[cc] = EXTRASAMPLE_UNSPECIFIED;
      }
    TIFFSetField(tif,TIFFTAG_EXTRASAMPLES,extra_samples,
      sample_info);
    delete [] sample_info;
    }

  int compression = COMPRESSION_PACKBITS;
  switch ( this->Compression )
    {
  case vtkTIFFWriter::PackBits: compression = COMPRESSION_PACKBITS; break;
  case vtkTIFFWriter::JPEG:     compression = COMPRESSION_JPEG; break;
  case vtkTIFFWriter::Deflate:  compression = COMPRESSION_DEFLATE; break;
  case vtkTIFFWriter::LZW:      compression = COMPRESSION_LZW; break;
  default: compression = COMPRESSION_NONE;
    }
  //compression = COMPRESSION_JPEG;
  TIFFSetField(tif, TIFFTAG_COMPRESSION, compression); // Fix for compression
  uint16 photometric = PHOTOMETRIC_RGB;
  if ( compression == COMPRESSION_JPEG )
    {
    TIFFSetField(tif, TIFFTAG_JPEGQUALITY, 75); // Parameter
    TIFFSetField(tif, TIFFTAG_JPEGCOLORMODE, JPEGCOLORMODE_RGB);
    photometric = PHOTOMETRIC_YCBCR;
    }
  else if ( compression == COMPRESSION_LZW )
    {
    predictor = 2;
    TIFFSetField(tif, TIFFTAG_PREDICTOR, predictor);
    vtkErrorMacro("LZW compression is patented outside US so it is disabled");
    }
  else if ( compression == COMPRESSION_DEFLATE )
    {
    predictor = 2;
    TIFFSetField(tif, TIFFTAG_PREDICTOR, predictor);
    }

  TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, photometric); // Fix for scomponents
  TIFFSetField(tif, TIFFTAG_ROWSPERSTRIP,
    TIFFDefaultStripSize(tif, rowsperstrip));
  if (resolution > 0) {
    TIFFSetField(tif, TIFFTAG_XRESOLUTION, resolution);
    TIFFSetField(tif, TIFFTAG_YRESOLUTION, resolution);
    TIFFSetField(tif, TIFFTAG_RESOLUTIONUNIT, RESUNIT_INCH);
  }
}

//----------------------------------------------------------------------------
void vtkTIFFWriter::WriteFile(ofstream *, vtkImageData *data,
  int extent[6])
{
  int idx1, idx2;
  int rowLength; // in bytes
  void *ptr;

  // Make sure we actually have data.
  if ( !data->GetPointData()->GetScalars())
    {
    vtkErrorMacro(<< "Could not get data from input.");
    return;
    }

  TIFF* tif = static_cast<TIFF*>(this->TIFFPtr);
  if ( !tif )
    {
    vtkErrorMacro("Problem writting trailer.");
    this->SetErrorCode(vtkErrorCode::FileFormatError);
    return;
    }

  // take into consideration the scalar type
  switch (data->GetScalarType())
    {
  case VTK_UNSIGNED_CHAR:
    rowLength = sizeof(unsigned char);
    break;
  case VTK_UNSIGNED_SHORT:
    rowLength = sizeof(unsigned short);
  default:
    vtkErrorMacro("TIFFWriter only accepts unsigned char scalars!");
    return;
    }
  rowLength *= data->GetNumberOfScalarComponents();
  rowLength *= (extent[1] - extent[0] + 1);

  int row = 0;
  for (idx2 = extent[4]; idx2 <= extent[5]; ++idx2)
    {
    for (idx1 = extent[3]; idx1 >= extent[2]; idx1--)
      {
      ptr = data->GetScalarPointer(extent[0], idx1, idx2);
      if ( TIFFWriteScanline(tif, static_cast<unsigned char*>(ptr), row, 0) < 0)
        {
        this->SetErrorCode(vtkErrorCode::OutOfDiskSpaceError);
        break;
        }
      row ++;
      }
    }
}

//----------------------------------------------------------------------------
void vtkTIFFWriter::WriteFileTrailer(ofstream *, vtkImageData *)
{
  TIFF* tif = static_cast<TIFF*>(this->TIFFPtr);
  if ( !tif )
    {
    vtkErrorMacro("Problem writting trailer.");
    this->SetErrorCode(vtkErrorCode::FileFormatError);
    }
  TIFFClose(tif);
  this->TIFFPtr = 0;
}
//----------------------------------------------------------------------------
void vtkTIFFWriter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
}
<|endoftext|>


<commit_before>/**
 * @file MosaicImageFactory.cpp
 *
 * Copyright (C) 2004, 2017, 2020  Ossama Othman
 *
 * SPDX-License-Identifier: GPL-2.0-or-later
 *
 * @author Ossama Othman
 */

#include "MosaicImageFactory.h"

#include <marc/first_read.h>
#include <marc/unweighted_average.h>
#include <marc/weighted_average.h>

#include <stdexcept>
#include <cassert>

namespace
{
    std::unique_ptr<MaRC::compositing_strategy>
    make_compositor(std::size_t count,
                    MaRC::MosaicImageFactory::average_type type)
    {
        // Compositing should not be used for single images.
        assert(count > 1);

        /**
         * @todo Drop the hard coding.  Make mosaic compositor
         *       selection extensible, such as through a compositor
         *       abstract factory.
         */
        switch (type) {
        case MaRC::MosaicImageFactory::AVG_NONE:
        default:
            return std::make_unique<MaRC::first_read>();
        case MaRC::MosaicImageFactory::AVG_UNWEIGHTED:
            return std::make_unique<MaRC::unweighted_average>();
        case MaRC::MosaicImageFactory::AVG_WEIGHTED:
            return std::make_unique<MaRC::weighted_average>();
        }
    }
}

// ----------------------------------------------------------------

MaRC::MosaicImageFactory::MosaicImageFactory (
    list_type && factories,
    average_type type)
    : SourceImageFactory()
    , factories_(std::move(factories))
    , average_type_(type)
{
    // No need to mosaic a single image.  This also covers the empty
    // factory list case.
    if (this->factories_.size() < 2)
        throw std::invalid_argument(
            "MosaicImageFactory used for less than two images.");
}

bool
MaRC::MosaicImageFactory::populate_parameters(
    MaRC::map_parameters &p) const
{
    /**
     * @todo Verify this achieves the desired result since all photos
     *       in this mosaic will plotted to the same map plane, rather
     *       than different planes.
     */
    for (auto const & image : this->factories_)
        if (!image->populate_parameters(p))
            return false;

    return true;
}

std::unique_ptr<MaRC::SourceImage>
MaRC::MosaicImageFactory::make(scale_offset_functor calc_so)
{
    /**
     * @todo Verify that the extrema handling in this method is
     *       correct.
     */

    extrema_type ex;
    bool valid_minimum = true;
    bool valid_maximum = true;
    MosaicImage::list_type photos;

    for (auto & factory : this->factories_) {
        auto const & minmax = factory->minmax();

        if (!minmax.minimum())
            valid_minimum = false;
        if (!minmax.maximum())
            valid_maximum = false;

        ex.update(minmax);

        photos.push_back(factory->make(calc_so));
    }

    /*
      Only set the mosaic image extrema if all photos in the mosaic
      have set extrema (e.g. FITS DATAMIN and/or DATAMAX values) to
      prevent inadvertently blocking out data from photos that don't
      set extrema values.  Previously set values, e.g. user-specified,
      will not be overridden.
    */
    if (valid_minimum)
        this->minimum(*ex.minimum());
    if (valid_maximum)
        this->maximum(*ex.maximum());

    auto compositor = make_compositor(photos.size(), this->average_type_);

    return
        std::make_unique<MosaicImage>(std::move(photos),
                                      std::move(compositor));
}
<commit_msg>src: Remove redundant image count check.<commit_after>/**
 * @file MosaicImageFactory.cpp
 *
 * Copyright (C) 2004, 2017, 2020  Ossama Othman
 *
 * SPDX-License-Identifier: GPL-2.0-or-later
 *
 * @author Ossama Othman
 */

#include "MosaicImageFactory.h"

#include <marc/first_read.h>
#include <marc/unweighted_average.h>
#include <marc/weighted_average.h>

#include <stdexcept>

namespace
{
    std::unique_ptr<MaRC::compositing_strategy>
    make_compositor(MaRC::MosaicImageFactory::average_type type)
    {
        /**
         * @todo Drop the hard coding.  Make mosaic compositor
         *       selection extensible, such as through a compositor
         *       abstract factory.
         */
        switch (type) {
        case MaRC::MosaicImageFactory::AVG_NONE:
        default:
            return std::make_unique<MaRC::first_read>();
        case MaRC::MosaicImageFactory::AVG_UNWEIGHTED:
            return std::make_unique<MaRC::unweighted_average>();
        case MaRC::MosaicImageFactory::AVG_WEIGHTED:
            return std::make_unique<MaRC::weighted_average>();
        }
    }
}

// ----------------------------------------------------------------

MaRC::MosaicImageFactory::MosaicImageFactory (
    list_type && factories,
    average_type type)
    : SourceImageFactory()
    , factories_(std::move(factories))
    , average_type_(type)
{
    // No need to mosaic a single image.  This also covers the empty
    // factory list case.
    if (this->factories_.size() < 2)
        throw std::invalid_argument(
            "MosaicImageFactory used for less than two images.");
}

bool
MaRC::MosaicImageFactory::populate_parameters(
    MaRC::map_parameters &p) const
{
    /**
     * @todo Verify this achieves the desired result since all photos
     *       in this mosaic will plotted to the same map plane, rather
     *       than different planes.
     */
    for (auto const & image : this->factories_)
        if (!image->populate_parameters(p))
            return false;

    return true;
}

std::unique_ptr<MaRC::SourceImage>
MaRC::MosaicImageFactory::make(scale_offset_functor calc_so)
{
    /**
     * @todo Verify that the extrema handling in this method is
     *       correct.
     */

    extrema_type ex;
    bool valid_minimum = true;
    bool valid_maximum = true;
    MosaicImage::list_type photos;

    for (auto & factory : this->factories_) {
        auto const & minmax = factory->minmax();

        if (!minmax.minimum())
            valid_minimum = false;
        if (!minmax.maximum())
            valid_maximum = false;

        ex.update(minmax);

        photos.push_back(factory->make(calc_so));
    }

    /*
      Only set the mosaic image extrema if all photos in the mosaic
      have set extrema (e.g. FITS DATAMIN and/or DATAMAX values) to
      prevent inadvertently blocking out data from photos that don't
      set extrema values.  Previously set values, e.g. user-specified,
      will not be overridden.
    */
    if (valid_minimum)
        this->minimum(*ex.minimum());
    if (valid_maximum)
        this->maximum(*ex.maximum());

    auto compositor = make_compositor(this->average_type_);

    return
        std::make_unique<MosaicImage>(std::move(photos),
                                      std::move(compositor));
}
<|endoftext|>


<commit_before>// Copyright 2016 AUV-IITK
#include <cv.h>
#include <highgui.h>
#include <ros/ros.h>
#include "std_msgs/String.h"
#include "std_msgs/Int8.h"
#include <fstream>
#include <vector>
#include <std_msgs/Bool.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <dynamic_reconfigure/server.h>
#include <task_gate/gateConfig.h>
#include <opencv2/opencv.hpp>
#include <opencv/highgui.h>
#include <image_transport/image_transport.h>
#include "std_msgs/Float64MultiArray.h"
#include <cv_bridge/cv_bridge.h>
#include <sstream>
#include <string>

int w = -2, x = -2, y = -2, z = -2;
bool IP = true;
bool flag = false;
bool video = false;
int t1min, t1max, t2min, t2max, t3min, t3max;

cv::Mat frame;
cv::Mat newframe;
int count = 0, count_avg = 0, p = -1;

void callback(task_gate::gateConfig &config, uint32_t level)
{
  t1min = config.t1min_param;
  t1max = config.t1max_param;
  t2min = config.t2min_param;
  t2max = config.t2max_param;
  t3min = config.t3min_param;
  t3max = config.t3max_param;
  ROS_INFO("Gate_Reconfigure Request : New parameters : %d %d %d %d %d %d ", t1min, t1max, t2min, t2max, t3min, t3max);
}

void gateListener(std_msgs::Bool msg)
{
  IP = msg.data;
}

void imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
  if (p == 32)
    return;
  try
  {
    count++;
    newframe = cv_bridge::toCvShare(msg, "bgr8")->image;
    cvNamedWindow("newframe", CV_WINDOW_NORMAL);
    ///////////////////////////// DO NOT REMOVE THIS, IT COULD BE INGERIOUS TO HEALTH /////////////////////
    newframe.copyTo(frame);
    cv::imshow("newframe", newframe);
    ////////////////////////// FATAL ///////////////////////////////////////////////////
  }
  catch (cv_bridge::Exception &e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char *argv[])
{
  int height, width, step, channels;  // parameters of the image we are working on
  std::string Video_Name = "Random_Video";
  if (argc >= 2)
    flag = true;
  if (argc == 3)
  {
    video = true;
    std::string avi = ".avi";
    Video_Name = (argv[2]) + avi;
  }

  cv::VideoWriter output_cap(Video_Name, CV_FOURCC('D', 'I', 'V', 'X'), 9, cv::Size(640, 480));

  ros::init(argc, argv, "gate_detection");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<std_msgs::Float64MultiArray>("/varun/ip/gate", 1000);
  ros::Subscriber sub = n.subscribe<std_msgs::Bool>("gate_detection_switch", 1000, &gateListener);
  ros::Rate loop_rate(10);

  n.getParam("gate_detection/t1max", t1max);
  n.getParam("gate_detection/t1min", t1min);
  n.getParam("gate_detection/t2max", t2max);
  n.getParam("gate_detection/t2min", t2min);
  n.getParam("gate_detection/t3max", t3max);
  n.getParam("gate_detection/t3min", t3min);

  image_transport::ImageTransport it(n);
  image_transport::Subscriber sub1 = it.subscribe("/varun/sensors/front_camera/image_raw", 1, imageCallback);

  dynamic_reconfigure::Server<task_gate::gateConfig> server;
  dynamic_reconfigure::Server<task_gate::gateConfig>::CallbackType f;
  f = boost::bind(&callback, _1, _2);
  server.setCallback(f);

  cvNamedWindow("After Color Filtering", CV_WINDOW_NORMAL);
  cvNamedWindow("Contours", CV_WINDOW_NORMAL);
  cvNamedWindow("RealPic", CV_WINDOW_NORMAL);

  if (flag)
  {
    cvNamedWindow("F1", CV_WINDOW_NORMAL);
    cvNamedWindow("F2", CV_WINDOW_NORMAL);
    cvNamedWindow("F3", CV_WINDOW_NORMAL);
  }

  // capture size -
  CvSize size = cvSize(width, height);

  // Initialize different images that are going to be used in the program
  cv::Mat hsv_frame, thresholded, thresholded1, thresholded2, thresholded3, filtered;  // image converted to HSV plane
  // asking for the minimum distance where bwe fire torpedo

  while (ros::ok())
  {
    std_msgs::Float64MultiArray array;
    loop_rate.sleep();
    // Get one frame
    if (frame.empty())
    {
      ROS_INFO("%s: empty frame", ros::this_node::getName().c_str());
      ros::spinOnce();
      continue;
    }

    if (video)
      output_cap.write(frame);
    // get the image data
    height = frame.rows;
    width = frame.cols;
    step = frame.step;

    // Covert color space to HSV as it is much easier to filter colors in the HSV color-space.
    cv::cvtColor(frame, hsv_frame, CV_BGR2HSV);
    cv::Scalar hsv_min = cv::Scalar(t1min, t2min, t3min, 0);
    cv::Scalar hsv_max = cv::Scalar(t1max, t2max, t3max, 0);
    // Filter out colors which are out of range.
    cv::inRange(hsv_frame, hsv_min, hsv_max, thresholded);
    // Split image into its 3 one dimensional images
    cv::Mat thresholded_hsv[3];
    cv::split(hsv_frame, thresholded_hsv);

    // Filter out colors which are out of range.
    cv::inRange(thresholded_hsv[0], cv::Scalar(t1min, 0, 0, 0), cv::Scalar(t1max, 0, 0, 0), thresholded_hsv[0]);
    cv::inRange(thresholded_hsv[1], cv::Scalar(t2min, 0, 0, 0), cv::Scalar(t2max, 0, 0, 0), thresholded_hsv[1]);
    cv::inRange(thresholded_hsv[2], cv::Scalar(t3min, 0, 0, 0), cv::Scalar(t3max, 0, 0, 0), thresholded_hsv[2]);
    cv::GaussianBlur(thresholded, thresholded, cv::Size(9, 9), 0, 0, 0);
    cv::imshow("After Color Filtering", thresholded);  // The stream after color filtering

    if (flag)
    {
      cv::imshow("F1", thresholded_hsv[0]);  // individual filters
      cv::imshow("F2", thresholded_hsv[1]);
      cv::imshow("F3", thresholded_hsv[2]);
    }

    if ((cvWaitKey(10) & 255) == 27)
      break;

    if (!IP)
    {
      // find contours
      std::vector<std::vector<cv::Point> > contours;
      cv::Mat thresholded_Mat = thresholded;
      findContours(thresholded_Mat, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);  // Find the contours in the image
      double largest_area = 0, largest_contour_index = 0;

      if (contours.empty())
      {
        array.data.push_back(0);
        array.data.push_back(0);

        pub.publish(array);
        ros::spinOnce();
        // If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
        // remove higher bits using AND operator
        if ((cvWaitKey(10) & 255) == 27)
          break;
        continue;
      }

      for (int i = 0; i < contours.size(); i++)  // iterate through each contour.
      {
        double a = contourArea(contours[i], false);  //  Find the area of contour
        if (a > largest_area)
        {
          largest_area = a;
          largest_contour_index = i;  // Store the index of largest contour
        }
      }
      // Convex HULL
      std::vector<std::vector<cv::Point> > hull(contours.size());
      convexHull(cv::Mat(contours[largest_contour_index]), hull[largest_contour_index], false);

      cv::Mat Drawing(thresholded_Mat.rows, thresholded_Mat.cols, CV_8UC1, cv::Scalar::all(0));
      std::vector<cv::Vec4i> hierarchy;
      cv::Scalar color(255, 255, 255);

      std::vector<cv::Rect> boundRect(1);

      boundRect[0] = boundingRect(cv::Mat(contours[largest_contour_index]));

      rectangle(Drawing, boundRect[0].tl(), boundRect[0].br(), color, 2, 8, 0);

      cv::Point center;
      center.x = ((boundRect[0].br()).x + (boundRect[0].tl()).x) / 2;
      center.y = ((boundRect[0].tl()).y + (boundRect[0].br()).y) / 2;
      float side_x = (boundRect[0].br()).x - (boundRect[0].tl()).x;
      float side_y = -(boundRect[0].tl()).y - (boundRect[0].br()).y;
      drawContours(Drawing, contours, largest_contour_index, color, 2, 8, hierarchy);

      cv::Mat frame_mat = frame;
      cv::Point2f screen_center;
      screen_center.x = 320;  // size of my screen
      screen_center.y = 240;

      circle(frame_mat, center, 5, cv::Scalar(0, 250, 0), -1, 8, 1);
      rectangle(frame_mat, boundRect[0].tl(), boundRect[0].br(), color, 2, 8, 0);
      circle(frame_mat, screen_center, 4, cv::Scalar(150, 150, 150), -1, 8, 0);  // center of screen

      cv::imshow("Contours", Drawing);
      cv::imshow("RealPic", frame_mat);

      w = (boundRect[0].br()).x;
      x = (boundRect[0].br()).y;
      y = (boundRect[0].tl()).y;
      z = (boundRect[0].tl()).x;
      if ((side_y < 40)&&(z == 1))
      {
        array.data.push_back(-2);
        array.data.push_back(-2);  //  hits left
      }
      else if ((side_y < 40)&&(x == frame.cols - 1))
      {
        array.data.push_back(-4);
        array.data.push_back(-4);  //  hits left
      }
      else if (w == (frame.rows) - 1 || x == (frame.cols) - 1 || y == 1 || z == 1)
      {
        if (y == 1)
        {
          array.data.push_back(-1);
          array.data.push_back(-1);  //  hits top
        }
        else if (w == frame.rows - 1)
        {
          array.data.push_back(-3);
          array.data.push_back(-3);  //  hits bottom
        }
        else if (z == 1)
        {
          array.data.push_back(-2);
          array.data.push_back(-2);  //  hits left
        }
        else if (x == frame.cols - 1)
        {
          array.data.push_back(-4);
          array.data.push_back(-4);  //  hits right
        }
      }
      else
      {
        array.data.push_back((320 - center.x));
        array.data.push_back(-(240 - center.y));
      }
      pub.publish(array);
      ros::spinOnce();

      // If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
      // remove higher bits using AND operator
      if ((cvWaitKey(10) & 255) == 27)
        break;
    }
    else
    {
      ROS_INFO("%s: empty frame", ros::this_node::getName().c_str());
      if ((cvWaitKey(10) & 255) == 32)
      {
        if (p == 32)
          p = -1;
        else
          p = 32;
      }
      if (p == 32)
      ROS_INFO("%s: PAUSED\n", ros::this_node::getName().c_str());
      ros::spinOnce();
    }
  }
  output_cap.release();
  return 0;
}

<commit_msg>misc_errors_3<commit_after>// Copyright 2016 AUV-IITK
#include <cv.h>
#include <highgui.h>
#include <ros/ros.h>
#include "std_msgs/String.h"
#include "std_msgs/Int8.h"
#include <fstream>
#include <vector>
#include <std_msgs/Bool.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <dynamic_reconfigure/server.h>
#include <task_gate/gateConfig.h>
#include <opencv2/opencv.hpp>
#include <opencv/highgui.h>
#include <image_transport/image_transport.h>
#include "std_msgs/Float64MultiArray.h"
#include <cv_bridge/cv_bridge.h>
#include <sstream>
#include <string>

int w = -2, x = -2, y = -2, z = -2;
bool IP = true;
bool flag = false;
bool video = false;
int t1min, t1max, t2min, t2max, t3min, t3max;

cv::Mat frame;
cv::Mat newframe;
int count = 0, count_avg = 0, p = -1;

void callback(task_gate::gateConfig &config, uint32_t level)
{
  t1min = config.t1min_param;
  t1max = config.t1max_param;
  t2min = config.t2min_param;
  t2max = config.t2max_param;
  t3min = config.t3min_param;
  t3max = config.t3max_param;
  ROS_INFO("Gate_Reconfigure Request : New parameters : %d %d %d %d %d %d ", t1min, t1max, t2min, t2max, t3min, t3max);
}

void gateListener(std_msgs::Bool msg)
{
  IP = msg.data;
}

void imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
  if (p == 32)
    return;
  try
  {
    count++;
    newframe = cv_bridge::toCvShare(msg, "bgr8")->image;
    cvNamedWindow("newframe", CV_WINDOW_NORMAL);
    ///////////////////////////// DO NOT REMOVE THIS, IT COULD BE INGERIOUS TO HEALTH /////////////////////
    newframe.copyTo(frame);
    cv::imshow("newframe", newframe);
    ////////////////////////// FATAL ///////////////////////////////////////////////////
  }
  catch (cv_bridge::Exception &e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char *argv[])
{
  int height, width, step, channels;  // parameters of the image we are working on
  std::string Video_Name = "Random_Video";
  if (argc >= 2)
    flag = true;
  if (argc == 3)
  {
    video = true;
    std::string avi = ".avi";
    Video_Name = (argv[2]) + avi;
  }

  cv::VideoWriter output_cap(Video_Name, CV_FOURCC('D', 'I', 'V', 'X'), 9, cv::Size(640, 480));

  ros::init(argc, argv, "gate_detection");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<std_msgs::Float64MultiArray>("/varun/ip/gate", 1000);
  ros::Subscriber sub = n.subscribe<std_msgs::Bool>("gate_detection_switch", 1000, &gateListener);
  ros::Rate loop_rate(10);

  n.getParam("gate_detection/t1max", t1max);
  n.getParam("gate_detection/t1min", t1min);
  n.getParam("gate_detection/t2max", t2max);
  n.getParam("gate_detection/t2min", t2min);
  n.getParam("gate_detection/t3max", t3max);
  n.getParam("gate_detection/t3min", t3min);

  image_transport::ImageTransport it(n);
  image_transport::Subscriber sub1 = it.subscribe("/varun/sensors/front_camera/image_raw", 1, imageCallback);

  dynamic_reconfigure::Server<task_gate::gateConfig> server;
  dynamic_reconfigure::Server<task_gate::gateConfig>::CallbackType f;
  f = boost::bind(&callback, _1, _2);
  server.setCallback(f);

  cvNamedWindow("After Color Filtering", CV_WINDOW_NORMAL);
  cvNamedWindow("Contours", CV_WINDOW_NORMAL);
  cvNamedWindow("RealPic", CV_WINDOW_NORMAL);

  if (flag)
  {
    cvNamedWindow("F1", CV_WINDOW_NORMAL);
    cvNamedWindow("F2", CV_WINDOW_NORMAL);
    cvNamedWindow("F3", CV_WINDOW_NORMAL);
  }

  // capture size -
  CvSize size = cvSize(width, height);

  // Initialize different images that are going to be used in the program
  cv::Mat hsv_frame, thresholded, thresholded1, thresholded2, thresholded3, filtered;  // image converted to HSV plane
  // asking for the minimum distance where bwe fire torpedo

  while (ros::ok())
  {
    std_msgs::Float64MultiArray array;
    loop_rate.sleep();
    // Get one frame
    if (frame.empty())
    {
      ROS_INFO("%s: empty frame", ros::this_node::getName().c_str());
      ros::spinOnce();
      continue;
    }

    if (video)
      output_cap.write(frame);
    // get the image data
    height = frame.rows;
    width = frame.cols;
    step = frame.step;

    // Covert color space to HSV as it is much easier to filter colors in the HSV color-space.
    cv::cvtColor(frame, hsv_frame, CV_BGR2HSV);
    cv::Scalar hsv_min = cv::Scalar(t1min, t2min, t3min, 0);
    cv::Scalar hsv_max = cv::Scalar(t1max, t2max, t3max, 0);
    // Filter out colors which are out of range.
    cv::inRange(hsv_frame, hsv_min, hsv_max, thresholded);
    // Split image into its 3 one dimensional images
    cv::Mat thresholded_hsv[3];
    cv::split(hsv_frame, thresholded_hsv);

    // Filter out colors which are out of range.
    cv::inRange(thresholded_hsv[0], cv::Scalar(t1min, 0, 0, 0), cv::Scalar(t1max, 0, 0, 0), thresholded_hsv[0]);
    cv::inRange(thresholded_hsv[1], cv::Scalar(t2min, 0, 0, 0), cv::Scalar(t2max, 0, 0, 0), thresholded_hsv[1]);
    cv::inRange(thresholded_hsv[2], cv::Scalar(t3min, 0, 0, 0), cv::Scalar(t3max, 0, 0, 0), thresholded_hsv[2]);
    cv::GaussianBlur(thresholded, thresholded, cv::Size(9, 9), 0, 0, 0);
    cv::imshow("After Color Filtering", thresholded);  // The stream after color filtering

    if (flag)
    {
      cv::imshow("F1", thresholded_hsv[0]);  // individual filters
      cv::imshow("F2", thresholded_hsv[1]);
      cv::imshow("F3", thresholded_hsv[2]);
    }

    if ((cvWaitKey(10) & 255) == 27)
      break;

    if (!IP)
    {
      // find contours
      std::vector<std::vector<cv::Point> > contours;
      cv::Mat thresholded_Mat = thresholded;
      findContours(thresholded_Mat, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);  // Find the contours in the image
      double largest_area = 0, largest_contour_index = 0;

      if (contours.empty())
      {
        array.data.push_back(0);
        array.data.push_back(0);

        pub.publish(array);
        ros::spinOnce();
        // If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
        // remove higher bits using AND operator
        if ((cvWaitKey(10) & 255) == 27)
          break;
        continue;
      }

      for (int i = 0; i < contours.size(); i++)  // iterate through each contour.
      {
        double a = contourArea(contours[i], false);  //  Find the area of contour
        if (a > largest_area)
        {
          largest_area = a;
          largest_contour_index = i;  // Store the index of largest contour
        }
      }
      // Convex HULL
      std::vector<std::vector<cv::Point> > hull(contours.size());
      convexHull(cv::Mat(contours[largest_contour_index]), hull[largest_contour_index], false);

      cv::Mat Drawing(thresholded_Mat.rows, thresholded_Mat.cols, CV_8UC1, cv::Scalar::all(0));
      std::vector<cv::Vec4i> hierarchy;
      cv::Scalar color(255, 255, 255);

      std::vector<cv::Rect> boundRect(1);

      boundRect[0] = boundingRect(cv::Mat(contours[largest_contour_index]));

      rectangle(Drawing, boundRect[0].tl(), boundRect[0].br(), color, 2, 8, 0);

      cv::Point center;
      center.x = ((boundRect[0].br()).x + (boundRect[0].tl()).x) / 2;
      center.y = ((boundRect[0].tl()).y + (boundRect[0].br()).y) / 2;
      float side_x = (boundRect[0].br()).x - (boundRect[0].tl()).x;
      float side_y = -(boundRect[0].tl()).y - (boundRect[0].br()).y;
      drawContours(Drawing, contours, largest_contour_index, color, 2, 8, hierarchy);

      cv::Mat frame_mat = frame;
      cv::Point2f screen_center;
      screen_center.x = 320;  // size of my screen
      screen_center.y = 240;

      circle(frame_mat, center, 5, cv::Scalar(0, 250, 0), -1, 8, 1);
      rectangle(frame_mat, boundRect[0].tl(), boundRect[0].br(), color, 2, 8, 0);
      circle(frame_mat, screen_center, 4, cv::Scalar(150, 150, 150), -1, 8, 0);  // center of screen

      cv::imshow("Contours", Drawing);
      cv::imshow("RealPic", frame_mat);

      w = (boundRect[0].br()).x;
      x = (boundRect[0].br()).y;
      y = (boundRect[0].tl()).y;
      z = (boundRect[0].tl()).x;
      if ((side_y < 40) && (z == 1))
      {
        array.data.push_back(-2);
        array.data.push_back(-2);  //  hits left
      }
      else if ((side_y < 40) && (x == frame.cols - 1))
      {
        array.data.push_back(-4);
        array.data.push_back(-4);  //  hits left
      }
      else if (w == (frame.rows) - 1 || x == (frame.cols) - 1 || y == 1 || z == 1)
      {
        if (y == 1)
        {
          array.data.push_back(-1);
          array.data.push_back(-1);  //  hits top
        }
        else if (w == frame.rows - 1)
        {
          array.data.push_back(-3);
          array.data.push_back(-3);  //  hits bottom
        }
        else if (z == 1)
        {
          array.data.push_back(-2);
          array.data.push_back(-2);  //  hits left
        }
        else if (x == frame.cols - 1)
        {
          array.data.push_back(-4);
          array.data.push_back(-4);  //  hits right
        }
      }
      else
      {
        array.data.push_back((320 - center.x));
        array.data.push_back(-(240 - center.y));
      }
      pub.publish(array);
      ros::spinOnce();

      // If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
      // remove higher bits using AND operator
      if ((cvWaitKey(10) & 255) == 27)
        break;
    }
    else
    {
      ROS_INFO("%s: empty frame", ros::this_node::getName().c_str());
      if ((cvWaitKey(10) & 255) == 32)
      {
        if (p == 32)
          p = -1;
        else
          p = 32;
      }
      if (p == 32)
      ROS_INFO("%s: PAUSED\n", ros::this_node::getName().c_str());
      ros::spinOnce();
    }
  }
  output_cap.release();
  return 0;
}

<|endoftext|>


<commit_before>/*
 * DiscoveryListener
 * Aug 2014
 * nathan lachenmyer
 */

#include <memory>
#include "DiscoveryListener.h"
#include "DeviceHeader.h"

using namespace ofxPixelPusher;

std::shared_ptr<DiscoveryListener> DiscoveryListener::mDiscoveryService = nullptr;

std::shared_ptr<DiscoveryListener> DiscoveryListener::getInstance() {
	if (mDiscoveryService == nullptr) {
		mDiscoveryService = std::shared_ptr<DiscoveryListener>(new DiscoveryListener());
	}
	return mDiscoveryService;
}

int DiscoveryListener::getFrameLimit() {
	return mFrameLimit;
}

std::vector<std::shared_ptr<PixelPusher> > DiscoveryListener::getPushers() {
	mUpdateMutex.lock();
	std::vector<std::shared_ptr<PixelPusher> > pusherVector;
	for (std::map<std::string, std::shared_ptr<PixelPusher> >::iterator it = mPusherMap.begin();
	it != mPusherMap.end();
		++it) {
		pusherVector.push_back(it->second);
	}
	mUpdateMutex.unlock();
	return pusherVector;
}

std::vector<std::shared_ptr<PixelPusher> > DiscoveryListener::getGroup(long groupId) {
	mUpdateMutex.lock();
	std::vector<std::shared_ptr<PixelPusher> > pusherVector;
	for (std::map<long, std::shared_ptr<PixelPusher> >::iterator it = mGroupMap.lower_bound(groupId);
	it != mGroupMap.upper_bound(groupId);
		++it) {
		pusherVector.push_back(it->second);
	}
	mUpdateMutex.unlock();
	return pusherVector;
}

std::shared_ptr<PixelPusher> DiscoveryListener::getController(long groupId, long controllerId) {
	mUpdateMutex.lock();
	for (std::map<long, std::shared_ptr<PixelPusher> >::iterator it = mGroupMap.lower_bound(groupId);
	it != mGroupMap.upper_bound(groupId);
		++it) {
		if (it->second->getControllerId() == controllerId) {
			mUpdateMutex.unlock();
			return it->second;
		}
	}
	//if no matching PixelPusher is found, return a nullPtr
	std::shared_ptr<PixelPusher> nullPtr;
	mUpdateMutex.unlock();
	return nullPtr;
}

DiscoveryListener::DiscoveryListener() {
	mDiscoveryServiceSocket = std::make_shared<ofxAsio::UdpReceiver>("0.0.0.0", 7331);
	mDiscoveryServiceSocket->addOnReceiveFn([=](std::shared_ptr<ofxAsio::Datagram> datagram) {
		DiscoveryListener::getInstance()->update(datagram->getMessage());
	});
	mDiscoveryServiceSocket->start();
	std::printf("Starting Discovery Listener Service...\n");

	mAutoThrottle = true;
	mFrameLimit = 60;

	mUpdateMapThread = std::thread(&DiscoveryListener::updatePusherMap, this);
}

DiscoveryListener::~DiscoveryListener() {
	if (mUpdateMapThread.joinable()) {
		mUpdateMapThread.join();
	}
}

void DiscoveryListener::update(std::string udpMessage) {
	std::printf("Updating registry...\n");
	mUpdateMutex.lock();
	DeviceHeader* header;

	header = new DeviceHeader(reinterpret_cast<const unsigned char*> (udpMessage.c_str()), udpMessage.length());
	if (header->getDeviceType() != PIXELPUSHER) {
		//if the device type isn't PixelPusher, end processing it right here.
		return;
	}

	std::shared_ptr<PixelPusher> incomingDevice(new PixelPusher(header));
	std::string macAddress = incomingDevice->getMacAddress();
	std::string ipAddress = incomingDevice->getIpAddress();
	mLastSeenMap[macAddress] = std::clock() / CLOCKS_PER_SEC;

	if (mPusherMap.count(macAddress) == 0) {
		//does not already exist in the map
		addNewPusher(macAddress, incomingDevice);
		std::printf("Adding new PixelPusher %s at address %s\n", macAddress.c_str(), ipAddress.c_str());
	}
	else {
		//already exists in the map
		if (!mPusherMap[macAddress]->isEqual(incomingDevice)) {
			//if the pushers are not equal, replace it with this one
			updatePusher(macAddress, incomingDevice);
			std::printf("Updating PixelPusher %s at address %s\n", macAddress.c_str(), ipAddress.c_str());
		}
		else {
			//if they're the same, then just update it
			mPusherMap[macAddress]->updateVariables(incomingDevice);
			std::printf("Updating PixelPusher %s at address %s\n", macAddress.c_str(), ipAddress.c_str());
			if (incomingDevice->getDeltaSequence() > 3) {
				mPusherMap[macAddress]->increaseExtraDelay(5);
			}
			if (incomingDevice->getDeltaSequence() < 1) {
				mPusherMap[macAddress]->decreaseExtraDelay(1);
			}
		}
	}

	mUpdateMutex.unlock();
}

void DiscoveryListener::addNewPusher(std::string macAddress, std::shared_ptr<PixelPusher> pusher) {
	mPusherMap.insert(std::make_pair(macAddress, pusher));
	mGroupMap.insert(std::make_pair(pusher->getGroupId(), pusher));
	pusher->createCardThread();
}

void DiscoveryListener::updatePusher(std::string macAddress, std::shared_ptr<PixelPusher> pusher) {
	mPusherMap[macAddress]->copyHeader(pusher);
}

void DiscoveryListener::updatePusherMap() {
	mRunUpdateMapThread = true;
	while (mRunUpdateMapThread) {
		mUpdateMutex.lock();
		for (std::map<std::string, std::shared_ptr<PixelPusher> >::iterator pusher = mPusherMap.begin(); pusher != mPusherMap.end();) {
			//pusher->first is Mac Address, pusher->second is the shared pointer to the PixelPusher
			if (!pusher->second->isAlive()) {
				std::printf("DiscoveryListener removing PixelPusher %s from all maps.\n", pusher->first.c_str());
				pusher->second->destroyCardThread();
				//remove from multimap -- more complicated
				for (auto it = mGroupMap.lower_bound(pusher->second->getGroupId()); it != mGroupMap.end();) {
					if (it->second->isEqual(pusher->second)) {
						mGroupMap.erase(it++);
					}
					else {
						++it;
					}
				}
				//remove pusher from maps
				mLastSeenMap.erase(pusher->first);
				mPusherMap.erase(pusher++);
			}
			else {
				++pusher;
			}
		}
		mUpdateMutex.unlock();
		std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	}
}

void DiscoveryListener::addRegistrationCallback(std::function<void(std::shared_ptr<PixelPusher>)> callback_function) {
  mRegistrationCallbacks.push_back(callback_function);
}

void DiscoveryListener::addRemovalCallback(std::function<void(std::shared_ptr<PixelPusher>)> callback_function) {
  mRemovalCallbacks.push_back(callback_function);
}
<commit_msg>small changes to DiscoveryListener to make it compatible with ofxAsio v1.1<commit_after>/*
 * DiscoveryListener
 * Aug 2014
 * nathan lachenmyer
 */

#include <memory>
#include "DiscoveryListener.h"
#include "DeviceHeader.h"

using namespace ofxPixelPusher;

std::shared_ptr<DiscoveryListener> DiscoveryListener::mDiscoveryService = nullptr;

std::shared_ptr<DiscoveryListener> DiscoveryListener::getInstance() {
	if (mDiscoveryService == nullptr) {
		mDiscoveryService = std::shared_ptr<DiscoveryListener>(new DiscoveryListener());
	}
	return mDiscoveryService;
}

int DiscoveryListener::getFrameLimit() {
	return mFrameLimit;
}

std::vector<std::shared_ptr<PixelPusher> > DiscoveryListener::getPushers() {
	mUpdateMutex.lock();
	std::vector<std::shared_ptr<PixelPusher> > pusherVector;
	for (std::map<std::string, std::shared_ptr<PixelPusher> >::iterator it = mPusherMap.begin();
	it != mPusherMap.end();
		++it) {
		pusherVector.push_back(it->second);
	}
	mUpdateMutex.unlock();
	return pusherVector;
}

std::vector<std::shared_ptr<PixelPusher> > DiscoveryListener::getGroup(long groupId) {
	mUpdateMutex.lock();
	std::vector<std::shared_ptr<PixelPusher> > pusherVector;
	for (std::map<long, std::shared_ptr<PixelPusher> >::iterator it = mGroupMap.lower_bound(groupId);
	it != mGroupMap.upper_bound(groupId);
		++it) {
		pusherVector.push_back(it->second);
	}
	mUpdateMutex.unlock();
	return pusherVector;
}

std::shared_ptr<PixelPusher> DiscoveryListener::getController(long groupId, long controllerId) {
	mUpdateMutex.lock();
	for (std::map<long, std::shared_ptr<PixelPusher> >::iterator it = mGroupMap.lower_bound(groupId);
	it != mGroupMap.upper_bound(groupId);
		++it) {
		if (it->second->getControllerId() == controllerId) {
			mUpdateMutex.unlock();
			return it->second;
		}
	}
	//if no matching PixelPusher is found, return a nullPtr
	std::shared_ptr<PixelPusher> nullPtr;
	mUpdateMutex.unlock();
	return nullPtr;
}

DiscoveryListener::DiscoveryListener() {
	mDiscoveryServiceSocket = std::make_shared<ofxAsio::UdpReceiver>("0.0.0.0", 7331);
	mDiscoveryServiceSocket->addOnReceiveFn([=](std::shared_ptr<ofxAsio::Message> datagram) {
		DiscoveryListener::getInstance()->update(datagram->getMessage());
	});
	mDiscoveryServiceSocket->start();
	std::printf("Starting Discovery Listener Service...\n");

	mAutoThrottle = true;
	mFrameLimit = 60;

	mUpdateMapThread = std::thread(&DiscoveryListener::updatePusherMap, this);
}

DiscoveryListener::~DiscoveryListener() {
	if (mUpdateMapThread.joinable()) {
		mUpdateMapThread.join();
	}
}

void DiscoveryListener::update(std::string udpMessage) {
	std::printf("Updating registry...\n");
	mUpdateMutex.lock();
	DeviceHeader* header;

	header = new DeviceHeader(reinterpret_cast<const unsigned char*> (udpMessage.c_str()), udpMessage.length());
	if (header->getDeviceType() != PIXELPUSHER) {
		//if the device type isn't PixelPusher, end processing it right here.
		return;
	}

	std::shared_ptr<PixelPusher> incomingDevice(new PixelPusher(header));
	std::string macAddress = incomingDevice->getMacAddress();
	std::string ipAddress = incomingDevice->getIpAddress();
	mLastSeenMap[macAddress] = std::clock() / CLOCKS_PER_SEC;

	if (mPusherMap.count(macAddress) == 0) {
		//does not already exist in the map
		addNewPusher(macAddress, incomingDevice);
		std::printf("Adding new PixelPusher %s at address %s\n", macAddress.c_str(), ipAddress.c_str());
	}
	else {
		//already exists in the map
		if (!mPusherMap[macAddress]->isEqual(incomingDevice)) {
			//if the pushers are not equal, replace it with this one
			updatePusher(macAddress, incomingDevice);
			std::printf("Updating PixelPusher %s at address %s\n", macAddress.c_str(), ipAddress.c_str());
		}
		else {
			//if they're the same, then just update it
			mPusherMap[macAddress]->updateVariables(incomingDevice);
			std::printf("Updating PixelPusher %s at address %s\n", macAddress.c_str(), ipAddress.c_str());
			if (incomingDevice->getDeltaSequence() > 3) {
				mPusherMap[macAddress]->increaseExtraDelay(5);
			}
			if (incomingDevice->getDeltaSequence() < 1) {
				mPusherMap[macAddress]->decreaseExtraDelay(1);
			}
		}
	}

	mUpdateMutex.unlock();
}

void DiscoveryListener::addNewPusher(std::string macAddress, std::shared_ptr<PixelPusher> pusher) {
	mPusherMap.insert(std::make_pair(macAddress, pusher));
	mGroupMap.insert(std::make_pair(pusher->getGroupId(), pusher));
	pusher->createCardThread();
}

void DiscoveryListener::updatePusher(std::string macAddress, std::shared_ptr<PixelPusher> pusher) {
	mPusherMap[macAddress]->copyHeader(pusher);
}

void DiscoveryListener::updatePusherMap() {
	mRunUpdateMapThread = true;
	while (mRunUpdateMapThread) {
		mUpdateMutex.lock();
		for (std::map<std::string, std::shared_ptr<PixelPusher> >::iterator pusher = mPusherMap.begin(); pusher != mPusherMap.end();) {
			//pusher->first is Mac Address, pusher->second is the shared pointer to the PixelPusher
			if (!pusher->second->isAlive()) {
				std::printf("DiscoveryListener removing PixelPusher %s from all maps.\n", pusher->first.c_str());
				pusher->second->destroyCardThread();
				//remove from multimap -- more complicated
				for (auto it = mGroupMap.lower_bound(pusher->second->getGroupId()); it != mGroupMap.end();) {
					if (it->second->isEqual(pusher->second)) {
						mGroupMap.erase(it++);
					}
					else {
						++it;
					}
				}
				//remove pusher from maps
				mLastSeenMap.erase(pusher->first);
				mPusherMap.erase(pusher++);
			}
			else {
				++pusher;
			}
		}
		mUpdateMutex.unlock();
		std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	}
}

void DiscoveryListener::addRegistrationCallback(std::function<void(std::shared_ptr<PixelPusher>)> callback_function) {
  mRegistrationCallbacks.push_back(callback_function);
}

void DiscoveryListener::addRemovalCallback(std::function<void(std::shared_ptr<PixelPusher>)> callback_function) {
  mRemovalCallbacks.push_back(callback_function);
}
<|endoftext|>


<commit_before>// This file is part of the dune-stuff project:
//   https://users.dune-project.org/projects/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//
// Contributors: Kirsten Weber

#ifndef DUNE_STUFF_FUNCTION_EXPRESSION_BASE_HH
#define DUNE_STUFF_FUNCTION_EXPRESSION_BASE_HH

#include <sstream>
#include <vector>

#include <dune/common/fvector.hh>
#include <dune/common/dynvector.hh>
#include <dune/common/exceptions.hh>
#include <dune/common/static_assert.hh>

#include <dune/stuff/common/parameter/tree.hh>
#include <dune/stuff/common/string.hh>
#include <dune/stuff/common/color.hh>

#include "mathexpr.hh"

namespace Dune {
namespace Stuff {
namespace Functions {

/**
 *  \brief base class that makes a function out of the stuff from mathexpr.hh
 *  \attention  Most surely you do not want to use this class directly, but Functions::Expression!
 */
template< class DomainFieldImp, int domainDim, class RangeFieldImp, int rangeDim >
class MathExpressionBase
{
public:
  typedef MathExpressionBase< DomainFieldImp, domainDim, RangeFieldImp, rangeDim > ThisType;

  typedef DomainFieldImp    DomainFieldType;
  static const unsigned int dimDomain = domainDim;

  typedef RangeFieldImp     RangeFieldType;
  static const unsigned int dimRange = rangeDim;

  MathExpressionBase(const std::string _variable, const std::string _expression)
  {
    const std::vector< std::string > expressions(1, _expression);
    setup(_variable, expressions);
  }

  MathExpressionBase(const std::string _variable, const std::vector< std::string > _expressions)
  {
    setup(_variable, _expressions);
  }

  MathExpressionBase(const ThisType& _other)
  {
    setup(_other.variable(), _other.expression());
  }

  ThisType& operator=(const ThisType& _other)
  {
    if (this != &_other) {
      cleanup();
      variable_ = "";
      variables_ = std::vector< std::string >();
      expressions_ = std::vector< std::string >();
      setup(_other.variable(), _other.expression());
    }
    return this;
  }

  ~MathExpressionBase()
  {
    cleanup();
  }

  std::string variable() const
  {
    return variable_;
  }

  const std::vector< std::string >& expression() const
  {
    return expressions_;
  }

  void evaluate(const Dune::FieldVector< DomainFieldType, dimDomain >& arg,
                Dune::FieldVector< RangeFieldType, dimRange >& ret) const
  {
    // copy arg
    for (typename Dune::FieldVector< DomainFieldType, dimDomain >::size_type ii = 0; ii < dimDomain; ++ii)
      *(arg_[ii]) = arg[ii];
    // copy ret
    for (typename Dune::FieldVector< RangeFieldType, dimRange >::size_type ii = 0; ii < dimRange; ++ii)
      ret[ii] = op_[ii]->Val();
  }

  /**
   *  \attention  arg will be used up to its size, ret will be resized!
   */
  void evaluate(const Dune::DynamicVector< DomainFieldType >& arg,
                Dune::DynamicVector< RangeFieldType >& ret) const
  {
    // check for sizes
    assert(arg.size() > 0);
    if (ret.size() != dimRange)
      ret = Dune::DynamicVector< RangeFieldType >(dimRange);
    // copy arg
    for (int ii = 0; ii < std::min(domainDim, int(arg.size())); ++ii)
      *(arg_[ii]) = arg[ii];
    // copy ret
    for (typename Dune::DynamicVector< RangeFieldType >::size_type ii = 0; ii < dimRange; ++ii)
      ret[ii] = op_[ii]->Val();
  }

  void evaluate(const Dune::FieldVector< DomainFieldType, dimDomain >& arg,
                Dune::DynamicVector< RangeFieldType >& ret) const
  {
    // check for sizes
    if (ret.size() != dimRange)
      ret = Dune::DynamicVector< RangeFieldType >(dimRange);
    // copy arg
    for (typename Dune::FieldVector< DomainFieldType, dimDomain >::size_type ii = 0; ii < dimDomain; ++ii)
      *(arg_[ii]) = arg[ii];
    // copy ret
    for (typename Dune::DynamicVector< RangeFieldType >::size_type ii = 0; ii < dimRange; ++ii)
      ret[ii] = op_[ii]->Val();
  }

  /**
   *  \attention  arg will be used up to its size
   */
  void evaluate(const Dune::DynamicVector< DomainFieldType >& arg,
                Dune::FieldVector< RangeFieldType, dimRange >& ret) const
  {
    assert(arg.size() > 0);
    // copy arg
    for (int ii = 0; ii < std::min(size_t(dimDomain), size_t(arg.size())); ++ii)
      *(arg_[ii]) = arg[ii];
    // copy ret
    for (size_t ii = 0; ii < dimRange; ++ii)
      ret[ii] = op_[ii]->Val();
  }

  void report(const std::string _name = "dune.stuff.function.mathexpressionbase",
              std::ostream& stream = std::cout,
              const std::string& _prefix = "") const
  {
    const std::string tmp = _name + "(" + variable() + ") = ";
    stream << _prefix << tmp;
    if (expression().size() == 1)
      stream << expression()[0] << std::endl;
    else {
      stream << "[ " << expression()[0] << ";" << std::endl;
      const std::string whitespace = Dune::Stuff::Common::whitespaceify(tmp + "[ ");
      for (unsigned int i = 1; i < expression().size() - 1; ++i)
        stream << _prefix << whitespace << expression()[i] << ";" << std::endl;
      stream << _prefix << whitespace << expression()[expression().size() -1] << " ]" << std::endl;
    }
  } // void report(const std::string, std::ostream&, const std::string&) const

private:
//  void evaluate(const Dune::DynamicVector< DomainFieldType >& arg, RangeFieldType& ret) const
//  {
//    assert(dimRange == 1 && "I'm only here to be used by Functions::Parametric::Coefficient, which has dimrange == 1");
//    // copy arg
//    for (int ii = 0; ii < std::min(domainDim, int(arg.size())); ++ii)
//      *(arg_[ii]) = arg[ii];
//    // copy ret
//    ret = op_[0]->Val();
//  }

  void setup(const std::string& _variable, const std::vector< std::string >& _expression)
  {
    dune_static_assert((dimDomain > 0), "Really?");
    dune_static_assert((dimRange > 0), "Really?");
    // set expressions
    if (_expression.size() < dimRange)
      DUNE_THROW(Dune::InvalidStateException,
                 "\n" << Dune::Stuff::Common::colorStringRed("ERROR:")
                 << " '_expression' too short (is " << _expression.size()
                 << ", should be " << dimRange << ")!");
    for (size_t ii = 0; ii < dimRange; ++ii)
      expressions_.push_back(_expression[ii]);
    // set variable (i.e. "x")
    variable_ = _variable;
    // fill variables (i.e. "x[0]", "x[1]", ...)
    for (size_t ii = 0; ii < dimDomain; ++ii) {
      std::stringstream variableStream;
      variableStream << variable_ << "[" << ii << "]";
      variables_.push_back(variableStream.str());
    }
    // create epressions
    for (size_t ii = 0; ii < dimDomain; ++ii) {
      arg_[ii] = new DomainFieldType(0.0);
      var_arg_[ii] = new RVar(variables_[ii].c_str(), arg_[ii]);
      vararray_[ii] = var_arg_[ii];
    }
    for (size_t ii = 0; ii < dimRange; ++ ii) {
      op_[ii] = new ROperation(expressions_[ii].c_str(), dimDomain, vararray_);
    }
  } // void setup(const std::string& _variable, const std::vector< std::string >& expressions)

  void cleanup()
  {
    for (size_t ii = 0; ii < dimRange; ++ii) {
      delete op_[ii];
    }
    for (size_t ii = 0; ii < dimDomain; ++ii) {
      delete var_arg_[ii];
      delete arg_[ii];
    }
  } // void cleanup()

  std::string                variable_;
  std::vector< std::string > variables_;
  std::vector< std::string > expressions_;
  unsigned int actualDimRange_;
  mutable DomainFieldType* arg_[dimDomain];
  RVar* var_arg_[dimDomain];
  RVar* vararray_[dimDomain];
  ROperation* op_[dimRange];
}; // class MathExpressionBase

} // namespace Functions
} // namespace Stuff
} // namespace Dune

#endif // DUNE_STUFF_FUNCTION_EXPRESSION_BASE_HH
<commit_msg>[functions.expression.base] remove unnecessary include, fix typo<commit_after>// This file is part of the dune-stuff project:
//   https://users.dune-project.org/projects/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//
// Contributors: Kirsten Weber

#ifndef DUNE_STUFF_FUNCTION_EXPRESSION_BASE_HH
#define DUNE_STUFF_FUNCTION_EXPRESSION_BASE_HH

#include <sstream>
#include <vector>

#include <dune/common/fvector.hh>
#include <dune/common/dynvector.hh>
#include <dune/common/exceptions.hh>
#include <dune/common/static_assert.hh>

#include <dune/stuff/common/string.hh>
#include <dune/stuff/common/color.hh>

#include "mathexpr.hh"

namespace Dune {
namespace Stuff {
namespace Functions {

/**
 *  \brief base class that makes a function out of the stuff from mathexpr.hh
 *  \attention  Most surely you do not want to use this class directly, but Functions::Expression!
 */
template< class DomainFieldImp, int domainDim, class RangeFieldImp, int rangeDim >
class MathExpressionBase
{
public:
  typedef MathExpressionBase< DomainFieldImp, domainDim, RangeFieldImp, rangeDim > ThisType;

  typedef DomainFieldImp    DomainFieldType;
  static const unsigned int dimDomain = domainDim;

  typedef RangeFieldImp     RangeFieldType;
  static const unsigned int dimRange = rangeDim;

  MathExpressionBase(const std::string _variable, const std::string _expression)
  {
    const std::vector< std::string > expressions(1, _expression);
    setup(_variable, expressions);
  }

  MathExpressionBase(const std::string _variable, const std::vector< std::string > _expressions)
  {
    setup(_variable, _expressions);
  }

  MathExpressionBase(const ThisType& _other)
  {
    setup(_other.variable(), _other.expression());
  }

  ThisType& operator=(const ThisType& _other)
  {
    if (this != &_other) {
      cleanup();
      variable_ = "";
      variables_ = std::vector< std::string >();
      expressions_ = std::vector< std::string >();
      setup(_other.variable(), _other.expression());
    }
    return this;
  }

  ~MathExpressionBase()
  {
    cleanup();
  }

  std::string variable() const
  {
    return variable_;
  }

  const std::vector< std::string >& expression() const
  {
    return expressions_;
  }

  void evaluate(const Dune::FieldVector< DomainFieldType, dimDomain >& arg,
                Dune::FieldVector< RangeFieldType, dimRange >& ret) const
  {
    // copy arg
    for (typename Dune::FieldVector< DomainFieldType, dimDomain >::size_type ii = 0; ii < dimDomain; ++ii)
      *(arg_[ii]) = arg[ii];
    // copy ret
    for (typename Dune::FieldVector< RangeFieldType, dimRange >::size_type ii = 0; ii < dimRange; ++ii)
      ret[ii] = op_[ii]->Val();
  }

  /**
   *  \attention  arg will be used up to its size, ret will be resized!
   */
  void evaluate(const Dune::DynamicVector< DomainFieldType >& arg,
                Dune::DynamicVector< RangeFieldType >& ret) const
  {
    // check for sizes
    assert(arg.size() > 0);
    if (ret.size() != dimRange)
      ret = Dune::DynamicVector< RangeFieldType >(dimRange);
    // copy arg
    for (int ii = 0; ii < std::min(domainDim, int(arg.size())); ++ii)
      *(arg_[ii]) = arg[ii];
    // copy ret
    for (typename Dune::DynamicVector< RangeFieldType >::size_type ii = 0; ii < dimRange; ++ii)
      ret[ii] = op_[ii]->Val();
  }

  void evaluate(const Dune::FieldVector< DomainFieldType, dimDomain >& arg,
                Dune::DynamicVector< RangeFieldType >& ret) const
  {
    // check for sizes
    if (ret.size() != dimRange)
      ret = Dune::DynamicVector< RangeFieldType >(dimRange);
    // copy arg
    for (typename Dune::FieldVector< DomainFieldType, dimDomain >::size_type ii = 0; ii < dimDomain; ++ii)
      *(arg_[ii]) = arg[ii];
    // copy ret
    for (typename Dune::DynamicVector< RangeFieldType >::size_type ii = 0; ii < dimRange; ++ii)
      ret[ii] = op_[ii]->Val();
  }

  /**
   *  \attention  arg will be used up to its size
   */
  void evaluate(const Dune::DynamicVector< DomainFieldType >& arg,
                Dune::FieldVector< RangeFieldType, dimRange >& ret) const
  {
    assert(arg.size() > 0);
    // copy arg
    for (int ii = 0; ii < std::min(size_t(dimDomain), size_t(arg.size())); ++ii)
      *(arg_[ii]) = arg[ii];
    // copy ret
    for (size_t ii = 0; ii < dimRange; ++ii)
      ret[ii] = op_[ii]->Val();
  }

  void report(const std::string _name = "dune.stuff.function.mathexpressionbase",
              std::ostream& stream = std::cout,
              const std::string& _prefix = "") const
  {
    const std::string tmp = _name + "(" + variable() + ") = ";
    stream << _prefix << tmp;
    if (expression().size() == 1)
      stream << expression()[0] << std::endl;
    else {
      stream << "[ " << expression()[0] << ";" << std::endl;
      const std::string whitespace = Dune::Stuff::Common::whitespaceify(tmp + "[ ");
      for (unsigned int i = 1; i < expression().size() - 1; ++i)
        stream << _prefix << whitespace << expression()[i] << ";" << std::endl;
      stream << _prefix << whitespace << expression()[expression().size() -1] << " ]" << std::endl;
    }
  } // void report(const std::string, std::ostream&, const std::string&) const

private:
//  void evaluate(const Dune::DynamicVector< DomainFieldType >& arg, RangeFieldType& ret) const
//  {
//    assert(dimRange == 1 && "I'm only here to be used by Functions::Parametric::Coefficient, which has dimrange == 1");
//    // copy arg
//    for (int ii = 0; ii < std::min(domainDim, int(arg.size())); ++ii)
//      *(arg_[ii]) = arg[ii];
//    // copy ret
//    ret = op_[0]->Val();
//  }

  void setup(const std::string& _variable, const std::vector< std::string >& _expression)
  {
    dune_static_assert((dimDomain > 0), "Really?");
    dune_static_assert((dimRange > 0), "Really?");
    // set expressions
    if (_expression.size() < dimRange)
      DUNE_THROW(Dune::InvalidStateException,
                 "\n" << Dune::Stuff::Common::colorStringRed("ERROR:")
                 << " '_expression' too short (is " << _expression.size()
                 << ", should be " << dimRange << ")!");
    for (size_t ii = 0; ii < dimRange; ++ii)
      expressions_.push_back(_expression[ii]);
    // set variable (i.e. "x")
    variable_ = _variable;
    // fill variables (i.e. "x[0]", "x[1]", ...)
    for (size_t ii = 0; ii < dimDomain; ++ii) {
      std::stringstream variableStream;
      variableStream << variable_ << "[" << ii << "]";
      variables_.push_back(variableStream.str());
    }
    // create expressions
    for (size_t ii = 0; ii < dimDomain; ++ii) {
      arg_[ii] = new DomainFieldType(0.0);
      var_arg_[ii] = new RVar(variables_[ii].c_str(), arg_[ii]);
      vararray_[ii] = var_arg_[ii];
    }
    for (size_t ii = 0; ii < dimRange; ++ ii) {
      op_[ii] = new ROperation(expressions_[ii].c_str(), dimDomain, vararray_);
    }
  } // void setup(const std::string& _variable, const std::vector< std::string >& expressions)

  void cleanup()
  {
    for (size_t ii = 0; ii < dimRange; ++ii) {
      delete op_[ii];
    }
    for (size_t ii = 0; ii < dimDomain; ++ii) {
      delete var_arg_[ii];
      delete arg_[ii];
    }
  } // void cleanup()

  std::string                variable_;
  std::vector< std::string > variables_;
  std::vector< std::string > expressions_;
  unsigned int actualDimRange_;
  mutable DomainFieldType* arg_[dimDomain];
  RVar* var_arg_[dimDomain];
  RVar* vararray_[dimDomain];
  ROperation* op_[dimRange];
}; // class MathExpressionBase

} // namespace Functions
} // namespace Stuff
} // namespace Dune

#endif // DUNE_STUFF_FUNCTION_EXPRESSION_BASE_HH
<|endoftext|>