<commit_before>/***********************************************************************************************************************
**
** Copyright (c) 2011, 2015 ETH Zurich
** All rights reserved.
**
** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
** following conditions are met:
**
**    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following
**      disclaimer.
**    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
**      following disclaimer in the documentation and/or other materials provided with the distribution.
**    * Neither the name of the ETH Zurich nor the names of its contributors may be used to endorse or promote products
**      derived from this software without specific prior written permission.
**
**
** 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 HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
** WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**
***********************************************************************************************************************/

#include "QueryResultVisualizer.h"

#include "ModelBase/src/nodes/Node.h"

#include "OOModel/src/declarations/Declaration.h"

#include "VisualizationBase/src/Scene.h"
#include "VisualizationBase/src/items/Item.h"
#include "VisualizationBase/src/overlays/ArrowOverlay.h"
#include "VisualizationBase/src/VisualizationManager.h"
#include "VisualizationBase/src/views/MainView.h"
#include "VisualizationBase/src/items/WebBrowserItem.h"

#include "HighlightOverlay.h"
#include "QueryRegistry.h"
#include "QueryExecutor.h"

namespace InformationScripting {

const QString QueryResultVisualizer::HIGHLIGHT_OVERLAY_GROUP = {"default graph highlight"};
const QString QueryResultVisualizer::ARROW_OVERLAY_GROUP = {"default arrow"};

const QStringList QueryResultVisualizer::INFO_ARGUMENT_NAMES{"i", "info"};
const QStringList QueryResultVisualizer::SORT_ARGUMENT_NAMES{"s", "sort"};

QueryResultVisualizer::QueryResultVisualizer(Model::Node* target, QStringList args, QueryExecutor* executor)
	: LinearQuery{target}, arguments_{{
		{INFO_ARGUMENT_NAMES, "Name of info values to be printed", INFO_ARGUMENT_NAMES[1]},
		{SORT_ARGUMENT_NAMES, "Name of info sort key for infos", SORT_ARGUMENT_NAMES[1]}
	}, args}, executor_{executor}
{}

Optional<TupleSet> QueryResultVisualizer::executeLinear(TupleSet input)
{
	auto result = visualize(input);
	if (result.hasErrors())
		return {result.errors()[0]};

	Q_ASSERT(executor_);
	executor_->setVisualizationExecuted();

	return input;
}

void QueryResultVisualizer::registerDefaultQueries()
{
	QueryRegistry::registerQuery<QueryResultVisualizer, QueryRegistry::ExtraArguments::QueryExecutor>("show");
	QueryRegistry::registerAlias("info", "show", [](QStringList& args) {args.prepend("-info");});
}

Optional<int> QueryResultVisualizer::visualize(const TupleSet& ts)
{
	cleanScene();
	showASTRelation(ts, "calls");

	auto colors = extractColors(ts);
	auto infoOptional = extractInfo(ts);
	if (infoOptional.hasErrors())
		return {infoOptional.errors()[0]};
	auto infos = infoOptional.value();

	auto astTuples = ts.tuples("ast");
	// Set default color for non covered ast nodes.
	for (const auto& astTuple : astTuples)
	{
		auto& color = colors[astTuple["ast"]];
		if (color.isNull()) color = "red";
	}

	for (auto it = infos.begin(); it != infos.end(); ++it)
	{
		auto& color = colors[it.key()];
		if (color.isNull()) color = "red";
	}

	for (auto it = colors.begin(); it != colors.end(); ++it)
	{
		Model::Node* node = it.key();
		Q_ASSERT(node);

		QString info = infos[node];

		auto nodeVisualizationIt = Visualization::Item::nodeItemsMap().find(node);
		if (nodeVisualizationIt == Visualization::Item::nodeItemsMap().end())
			qWarning() << "no visualization for" << node->typeName();
		while (nodeVisualizationIt != Visualization::Item::nodeItemsMap().end() && nodeVisualizationIt.key() == node)
		{
			auto item = *nodeVisualizationIt++;
			auto overlay = new HighlightOverlay{item};
			overlay->setText(info);
			setColor(overlay, it.value());
			item->addOverlay(overlay, HIGHLIGHT_OVERLAY_GROUP);
		}
	}

	auto htmlTuples = ts.tuples("html");
	if (htmlTuples.size()) {
		QString htmlContent = (*htmlTuples.begin())["html"];
		// TODO an overlay or something would probably be better for this:
		// TODO position it a bit better? right now it is just centered.
		auto browser = new Visualization::WebBrowserItem{nullptr, htmlContent,
				Visualization::WebBrowserItem::itemStyles().get("frame")};
		browser->setZValue(Visualization::Item::LAYER_OVERLAY_Z);
		auto mainScene = Visualization::VisualizationManager::instance().mainScene();
		for (auto view : mainScene->views())
			if (auto mainView = dynamic_cast<Visualization::MainView*>(view))
				browser->setPos(mainView->sceneRect().center());
		mainScene->addTopLevelItem(browser);
	}

	return {1};
}

void QueryResultVisualizer::cleanScene()
{
	for (auto scene : Visualization::Scene::allScenes())
	{
		scene->removeOverlayGroup(HIGHLIGHT_OVERLAY_GROUP);
		scene->removeOverlayGroup(ARROW_OVERLAY_GROUP);
	}
}

void QueryResultVisualizer::showASTRelation(const TupleSet& ts, const QString& relationName)
{
	auto relationTuples = ts.tuples(relationName);
	for (auto t : relationTuples)
	{
		auto allAsts = t.valuesOfType<Model::Node*>();
		if (allAsts.size() > 1)
		{
			Model::Node* fromNode = allAsts[0];
			Model::Node* toNode = allAsts[1];

			auto fromVisualization = Visualization::Item::nodeItemsMap().find(fromNode);
			Q_ASSERT(fromVisualization != Visualization::Item::nodeItemsMap().end());
			auto toVisualization = Visualization::Item::nodeItemsMap().find(toNode);
			Q_ASSERT(toVisualization != Visualization::Item::nodeItemsMap().end());
			auto fromVisualizationItem = *fromVisualization;

			auto overlay = new Visualization::ArrowOverlay{
						fromVisualizationItem, *toVisualization, Visualization::ArrowOverlay::itemStyles().get()};
			fromVisualizationItem->addOverlay(overlay, ARROW_OVERLAY_GROUP);
		}
	}
}

QHash<Model::Node*, QString> QueryResultVisualizer::extractColors(const TupleSet& ts)
{
	QHash<Model::Node*, QString> colors;
	for (const auto& colorTuple : ts.tuples("color"))
	{
		auto it = colorTuple.find("ast");
		if (it != colorTuple.end())
		{
			QString color = colorTuple["color"];
			colors[it->second] = color;
		}
	}
	return colors;
}

Optional<QHash<Model::Node*, QString>> QueryResultVisualizer::extractInfo(const TupleSet& ts)
{
	QHash<Model::Node*, QList<Tuple>> infos;

	auto infoValues = infoArgumentValues();
	if (infoValues.hasErrors()) return infoValues.errors()[0];

	auto values = infoValues.value();
	if (values.empty()) return QHash<Model::Node*, QString>();

	const auto tag = values[0].first;
	bool allSame = std::all_of(values.begin(), values.end(), [&tag](const auto& values) {return values.first == tag;});
	if (!allSame) return {"Info values have to have same tag"};

	for (const auto& infoTuple : ts.tuples(tag))
	{
		auto allAsts = infoTuple.valuesOfType<Model::Node*>();
		if (allAsts.isEmpty() || allAsts.size() > 1) return {"Infotuple has to have a value which refers to an AST node"};
		auto astNode = allAsts[0];

		QList<NamedProperty> tupleValues;

		for (const auto& taggedVal : values)
		{
			auto valIt = infoTuple.find(taggedVal.second);
			if (valIt != infoTuple.end()) tupleValues.push_back(*valIt);
			else return {QString{"info: tuple %1 has no entry named %2"}.arg(tag, taggedVal.second)};
		}
		infos[astNode].append(Tuple{tupleValues});
	}
	return convertTuplesToString(infos);
}

Optional<QHash<Model::Node*, QString>> QueryResultVisualizer::convertTuplesToString(
		const QHash<Model::Node*, QList<Tuple>>& infos)
{
	QHash<Model::Node*, QString> result;
	auto sortKey = arguments_.argument(SORT_ARGUMENT_NAMES[1]);
	for (auto it = infos.begin(); it != infos.end(); ++it)
	{
		auto values = it.value();
		if (!sortKey.isEmpty())
			std::sort(values.begin(), values.end(), [sortKey](const Tuple& left, const Tuple& right) {
				return left[sortKey] < right[sortKey];}); // TODO check if sortkey is contained

		QStringList stringTuples;
		for (const auto& tuple : values)
		{
			QStringList values;
			for (const auto& value : tuple)
				values << value.second.toString();
			stringTuples << values.join(", ");
		}
		result[it.key()] = stringTuples.join("
");
	}
	return result;
}

Optional<std::vector<QueryResultVisualizer::TaggedValue>> QueryResultVisualizer::infoArgumentValues()
{
	std::vector<QueryResultVisualizer::TaggedValue> values;
	const QRegularExpression valueMatch{"((\\w+)\\.)?(\\w+)"};
	auto valueMatchIt = valueMatch.globalMatch(arguments_.argument(INFO_ARGUMENT_NAMES[1]));
	while (valueMatchIt.hasNext())
	{
		auto match = valueMatchIt.next();
		if (!match.hasMatch()) return {"Info values have to be of form: tag.value"};
		auto tag = match.captured(2);
		auto value = match.captured(3);

		values.push_back({tag, value});
	}
	return values;
}

void QueryResultVisualizer::setColor(HighlightOverlay* overlay, QColor color)
{
	color.setAlpha(DEFAULT_ALPHA_);
	overlay->setColor(color);
}

}
<commit_msg>Enlarge and center the HTML tuple visualization<commit_after>/***********************************************************************************************************************
**
** Copyright (c) 2011, 2015 ETH Zurich
** All rights reserved.
**
** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
** following conditions are met:
**
**    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following
**      disclaimer.
**    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
**      following disclaimer in the documentation and/or other materials provided with the distribution.
**    * Neither the name of the ETH Zurich nor the names of its contributors may be used to endorse or promote products
**      derived from this software without specific prior written permission.
**
**
** 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 HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
** WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**
***********************************************************************************************************************/

#include "QueryResultVisualizer.h"

#include "ModelBase/src/nodes/Node.h"

#include "OOModel/src/declarations/Declaration.h"

#include "VisualizationBase/src/Scene.h"
#include "VisualizationBase/src/items/Item.h"
#include "VisualizationBase/src/overlays/ArrowOverlay.h"
#include "VisualizationBase/src/VisualizationManager.h"
#include "VisualizationBase/src/views/MainView.h"
#include "VisualizationBase/src/items/WebBrowserItem.h"

#include "HighlightOverlay.h"
#include "QueryRegistry.h"
#include "QueryExecutor.h"

namespace InformationScripting {

const QString QueryResultVisualizer::HIGHLIGHT_OVERLAY_GROUP = {"default graph highlight"};
const QString QueryResultVisualizer::ARROW_OVERLAY_GROUP = {"default arrow"};

const QStringList QueryResultVisualizer::INFO_ARGUMENT_NAMES{"i", "info"};
const QStringList QueryResultVisualizer::SORT_ARGUMENT_NAMES{"s", "sort"};

QueryResultVisualizer::QueryResultVisualizer(Model::Node* target, QStringList args, QueryExecutor* executor)
	: LinearQuery{target}, arguments_{{
		{INFO_ARGUMENT_NAMES, "Name of info values to be printed", INFO_ARGUMENT_NAMES[1]},
		{SORT_ARGUMENT_NAMES, "Name of info sort key for infos", SORT_ARGUMENT_NAMES[1]}
	}, args}, executor_{executor}
{}

Optional<TupleSet> QueryResultVisualizer::executeLinear(TupleSet input)
{
	auto result = visualize(input);
	if (result.hasErrors())
		return {result.errors()[0]};

	Q_ASSERT(executor_);
	executor_->setVisualizationExecuted();

	return input;
}

void QueryResultVisualizer::registerDefaultQueries()
{
	QueryRegistry::registerQuery<QueryResultVisualizer, QueryRegistry::ExtraArguments::QueryExecutor>("show");
	QueryRegistry::registerAlias("info", "show", [](QStringList& args) {args.prepend("-info");});
}

Optional<int> QueryResultVisualizer::visualize(const TupleSet& ts)
{
	cleanScene();
	showASTRelation(ts, "calls");

	auto colors = extractColors(ts);
	auto infoOptional = extractInfo(ts);
	if (infoOptional.hasErrors())
		return {infoOptional.errors()[0]};
	auto infos = infoOptional.value();

	auto astTuples = ts.tuples("ast");
	// Set default color for non covered ast nodes.
	for (const auto& astTuple : astTuples)
	{
		auto& color = colors[astTuple["ast"]];
		if (color.isNull()) color = "red";
	}

	for (auto it = infos.begin(); it != infos.end(); ++it)
	{
		auto& color = colors[it.key()];
		if (color.isNull()) color = "red";
	}

	for (auto it = colors.begin(); it != colors.end(); ++it)
	{
		Model::Node* node = it.key();
		Q_ASSERT(node);

		QString info = infos[node];

		auto nodeVisualizationIt = Visualization::Item::nodeItemsMap().find(node);
		if (nodeVisualizationIt == Visualization::Item::nodeItemsMap().end())
			qWarning() << "no visualization for" << node->typeName();
		while (nodeVisualizationIt != Visualization::Item::nodeItemsMap().end() && nodeVisualizationIt.key() == node)
		{
			auto item = *nodeVisualizationIt++;
			auto overlay = new HighlightOverlay{item};
			overlay->setText(info);
			setColor(overlay, it.value());
			item->addOverlay(overlay, HIGHLIGHT_OVERLAY_GROUP);
		}
	}

	auto htmlTuples = ts.tuples("html");
	if (htmlTuples.size()) {
		QString htmlContent = (*htmlTuples.begin())["html"];
		// TODO an overlay or something would probably be better for this:
		// TODO position it a bit better? right now it is just centered.
		auto browser = new Visualization::WebBrowserItem{nullptr, htmlContent,
				Visualization::WebBrowserItem::itemStyles().get("frame")};
		browser->setZValue(Visualization::Item::LAYER_OVERLAY_Z);
		browser->updateSize({800, 600});
		auto mainScene = Visualization::VisualizationManager::instance().mainScene();
		for (auto view : mainScene->views())
			if (auto mainView = dynamic_cast<Visualization::MainView*>(view))
				browser->setPos(mainView->sceneRect().center() - QPoint{400, 300});
		mainScene->addTopLevelItem(browser);
	}

	return {1};
}

void QueryResultVisualizer::cleanScene()
{
	for (auto scene : Visualization::Scene::allScenes())
	{
		scene->removeOverlayGroup(HIGHLIGHT_OVERLAY_GROUP);
		scene->removeOverlayGroup(ARROW_OVERLAY_GROUP);
	}
}

void QueryResultVisualizer::showASTRelation(const TupleSet& ts, const QString& relationName)
{
	auto relationTuples = ts.tuples(relationName);
	for (auto t : relationTuples)
	{
		auto allAsts = t.valuesOfType<Model::Node*>();
		if (allAsts.size() > 1)
		{
			Model::Node* fromNode = allAsts[0];
			Model::Node* toNode = allAsts[1];

			auto fromVisualization = Visualization::Item::nodeItemsMap().find(fromNode);
			Q_ASSERT(fromVisualization != Visualization::Item::nodeItemsMap().end());
			auto toVisualization = Visualization::Item::nodeItemsMap().find(toNode);
			Q_ASSERT(toVisualization != Visualization::Item::nodeItemsMap().end());
			auto fromVisualizationItem = *fromVisualization;

			auto overlay = new Visualization::ArrowOverlay{
						fromVisualizationItem, *toVisualization, Visualization::ArrowOverlay::itemStyles().get()};
			fromVisualizationItem->addOverlay(overlay, ARROW_OVERLAY_GROUP);
		}
	}
}

QHash<Model::Node*, QString> QueryResultVisualizer::extractColors(const TupleSet& ts)
{
	QHash<Model::Node*, QString> colors;
	for (const auto& colorTuple : ts.tuples("color"))
	{
		auto it = colorTuple.find("ast");
		if (it != colorTuple.end())
		{
			QString color = colorTuple["color"];
			colors[it->second] = color;
		}
	}
	return colors;
}

Optional<QHash<Model::Node*, QString>> QueryResultVisualizer::extractInfo(const TupleSet& ts)
{
	QHash<Model::Node*, QList<Tuple>> infos;

	auto infoValues = infoArgumentValues();
	if (infoValues.hasErrors()) return infoValues.errors()[0];

	auto values = infoValues.value();
	if (values.empty()) return QHash<Model::Node*, QString>();

	const auto tag = values[0].first;
	bool allSame = std::all_of(values.begin(), values.end(), [&tag](const auto& values) {return values.first == tag;});
	if (!allSame) return {"Info values have to have same tag"};

	for (const auto& infoTuple : ts.tuples(tag))
	{
		auto allAsts = infoTuple.valuesOfType<Model::Node*>();
		if (allAsts.isEmpty() || allAsts.size() > 1) return {"Infotuple has to have a value which refers to an AST node"};
		auto astNode = allAsts[0];

		QList<NamedProperty> tupleValues;

		for (const auto& taggedVal : values)
		{
			auto valIt = infoTuple.find(taggedVal.second);
			if (valIt != infoTuple.end()) tupleValues.push_back(*valIt);
			else return {QString{"info: tuple %1 has no entry named %2"}.arg(tag, taggedVal.second)};
		}
		infos[astNode].append(Tuple{tupleValues});
	}
	return convertTuplesToString(infos);
}

Optional<QHash<Model::Node*, QString>> QueryResultVisualizer::convertTuplesToString(
		const QHash<Model::Node*, QList<Tuple>>& infos)
{
	QHash<Model::Node*, QString> result;
	auto sortKey = arguments_.argument(SORT_ARGUMENT_NAMES[1]);
	for (auto it = infos.begin(); it != infos.end(); ++it)
	{
		auto values = it.value();
		if (!sortKey.isEmpty())
			std::sort(values.begin(), values.end(), [sortKey](const Tuple& left, const Tuple& right) {
				return left[sortKey] < right[sortKey];}); // TODO check if sortkey is contained

		QStringList stringTuples;
		for (const auto& tuple : values)
		{
			QStringList values;
			for (const auto& value : tuple)
				values << value.second.toString();
			stringTuples << values.join(", ");
		}
		result[it.key()] = stringTuples.join("
");
	}
	return result;
}

Optional<std::vector<QueryResultVisualizer::TaggedValue>> QueryResultVisualizer::infoArgumentValues()
{
	std::vector<QueryResultVisualizer::TaggedValue> values;
	const QRegularExpression valueMatch{"((\\w+)\\.)?(\\w+)"};
	auto valueMatchIt = valueMatch.globalMatch(arguments_.argument(INFO_ARGUMENT_NAMES[1]));
	while (valueMatchIt.hasNext())
	{
		auto match = valueMatchIt.next();
		if (!match.hasMatch()) return {"Info values have to be of form: tag.value"};
		auto tag = match.captured(2);
		auto value = match.captured(3);

		values.push_back({tag, value});
	}
	return values;
}

void QueryResultVisualizer::setColor(HighlightOverlay* overlay, QColor color)
{
	color.setAlpha(DEFAULT_ALPHA_);
	overlay->setColor(color);
}

}
<|endoftext|>


<commit_before>// This file is part of the ustl library, an STL implementation.
//
// Copyright (C) 2005 by Mike Sharov <msharov@users.sourceforge.net>
// This file is free software, distributed under the MIT License.
//
// sostream.h
//

#include "mistream.h"	// for istream_iterator, referenced in utf8.h
#include "sostream.h"
#include "ustring.h"
#include "utf8.h"
#include "ulimits.h"
#include <stdio.h>

namespace ustl {

/// Default constructor.
ostringstream::ostringstream (void)
: ostream (),
  m_pResizable (NULL),
  m_Flags (0),
  m_Base (10),
  m_Precision (2),
  m_Width (0),
  m_DecimalSeparator ('.'),
  m_ThousandSeparator (',')
{
}

/// Creates a stream for writing into \p p of size \p n.
ostringstream::ostringstream (void* p, size_type n)
: ostream (),
  m_pResizable (NULL),
  m_Flags (0),
  m_Base (10),
  m_Precision (2),
  m_Width (0),
  m_DecimalSeparator ('.'),
  m_ThousandSeparator (',')
{
    link (p, n);
}

/// Creates a stream for writing into string \p dest.
///
/// dest may be resized by the stream if insufficient space is available.
///
ostringstream::ostringstream (string& dest)
: ostream (),
  m_pResizable (NULL),
  m_Flags (0),
  m_Base (10),
  m_Precision (2),
  m_Width (0),
  m_DecimalSeparator ('.'),
  m_ThousandSeparator (',')
{
    link (dest);
}

/// Creates a stream for writing into fixed block \p dest.
ostringstream::ostringstream (memlink& dest)
: ostream (),
  m_pResizable (NULL),
  m_Flags (0),
  m_Base (10),
  m_Precision (2),
  m_Width (0),
  m_DecimalSeparator ('.'),
  m_ThousandSeparator (',')
{
    link (dest);
}

/// Writes a single character into the stream.
void ostringstream::iwrite (uint8_t v)
{
    if (remaining() >= sizeof(uint8_t) || overflow() >= sizeof(uint8_t))
	ostream::iwrite (v);
}

/// Writes \p buf of size \p bufSize through the internal buffer.
void ostringstream::write_buffer (const char* buf, size_type bufSize)
{
    size_type btw = 0, written = 0;
    while ((written += btw) < bufSize && (remaining() || overflow(bufSize - written)))
	write (buf + written, btw = min (remaining(), bufSize - written));
}

/// Simple decimal encoding of \p n into \p fmt.
inline char* ostringstream::encode_dec (char* fmt, uint32_t n) const
{
    do {
	*fmt++ = '0' + n % 10;
    } while (n /= 10);
    return (fmt);
}

/// Generates a sprintf format string for the given type.
void ostringstream::fmtstring (char* fmt, const char* typestr, bool bInteger) const
{
    *fmt++ = '%';
    if (m_Width)
	fmt = encode_dec (fmt, m_Width);
    if (m_Flags & ios::left)
	*fmt++ = '-';
    if (!bInteger) {
	*fmt++ = '.';
	fmt = encode_dec (fmt, m_Precision);
    }
    while (*typestr)
	*fmt++ = *typestr++;
    if (bInteger) {
	if (m_Base == 16)
	    fmt[-1] = 'X';
	else if (m_Base == 8)
	    fmt[-1] = 'o';
    } else {
	if (m_Flags & ios::scientific)
	    fmt[-1] = 'E';
    }
    *fmt = 0;
}

template <typename T>
inline void ostringstream::sprintf_iwrite (T v, const char* typestr)
{
    const size_type c_BufSize = 64, c_FmtStrSize = 16;
    char fmt [c_FmtStrSize], buffer [c_BufSize];
    fmtstring (fmt, typestr, numeric_limits<T>::is_integer);
    size_type i = snprintf (buffer, c_BufSize, fmt, v);
    i = min (i, c_BufSize - 1);
    write_buffer (buffer, i);
}

void ostringstream::iwrite (int v)		{ sprintf_iwrite (v, "d"); }
void ostringstream::iwrite (unsigned int v)	{ sprintf_iwrite (v, "u"); }
void ostringstream::iwrite (long v)		{ sprintf_iwrite (v, "ld"); }
void ostringstream::iwrite (unsigned long v)	{ sprintf_iwrite (v, "lu"); }
void ostringstream::iwrite (float v)		{ simd::reset_mmx(); sprintf_iwrite (v, "f"); }
void ostringstream::iwrite (double v)		{ simd::reset_mmx(); sprintf_iwrite (v, "lf"); }
#if HAVE_LONG_LONG
void ostringstream::iwrite (long long v)	{ sprintf_iwrite (v, "lld"); }
void ostringstream::iwrite (unsigned long long v) { sprintf_iwrite (v, "llu"); }
#endif

/// Writes \p v into the stream as utf8
void ostringstream::iwrite (wchar_t v)
{
    char buffer [9];
    *utf8out(buffer) = v;
    write_buffer (buffer, Utf8Bytes(v));
}

/// Writes value \p v into the stream as text.
void ostringstream::iwrite (bool v)
{
    static const char* c_Names[2] = { "false", "true" };
    write_buffer (c_Names[v], 5 - v);
}

/// Writes string \p s into the stream.
void ostringstream::iwrite (const char* s)
{
    write_buffer (s, strlen(s));
}

/// Writes string \p v into the stream.
void ostringstream::iwrite (const string& v)
{
    write_buffer (v.begin(), v.size());
}

/// Equivalent to a vsprintf on the string.
int ostringstream::vformat (const char* fmt, va_list args)
{
#if HAVE_VA_COPY
    va_list args2;
    __va_copy (args2, args);    // Because vsnprintf will iterate over args, changing them.
#else
    #define args2 args
#endif
    const bool bIsString (m_pResizable);
    int rv = vsnprintf (ipos(), remaining() + bIsString, fmt, args);
    if (uoff_t(rv) >= remaining() + 0)
	rv = vsnprintf (ipos(), overflow(rv) + bIsString, fmt, args2);
    skip (min (uoff_t(rv), remaining()));
    return (rv);
}

/// Equivalent to a sprintf on the string.
int ostringstream::format (const char* fmt, ...)
{
    simd::reset_mmx();
    va_list args;
    va_start (args, fmt);
    const int rv = vformat (fmt, args);
    va_end (args);
    return (rv);
}

/// Sets the flag \p f in the stream.
void ostringstream::iwrite (ios::fmtflags f)
{
    switch (f) {
	case ios::oct:		set_base (8); break;
	case ios::dec:		set_base (10); break;
	case ios::hex:		set_base (16); break;
	case ios::left:
	    m_Flags |= ios::left;
	    m_Flags &= ~ios::right;
	    break;
	case ios::right:
	    m_Flags |= ios::right;
	    m_Flags &= ~ios::left;
	    break;
	default:
	    m_Flags |= f;
	    break;
    }
}

/// Links to string \p l as resizable.
void ostringstream::link (string& l)
{
    if (l.is_linked())
	l.reserve (l.capacity());
    assert (l.data() && "The output string buffer must not be read-only");
    ostream::link (l);
    m_pResizable = &l;
}

/// Unlinks the stream from its bound buffer.
void ostringstream::unlink (void)
{
    ostream::unlink();
    m_pResizable = NULL;
}

/// Writes the contents of \p buffer of \p size into the stream.
void ostringstream::write (const void* buffer, size_type sz)
{
    if (remaining() < sz && overflow(sz) < sz)
	return;
    ostream::write (buffer, sz);
}

/// Writes the contents of \p buf into the stream.
void ostringstream::write (const cmemlink& buf)
{
    if (remaining() < buf.size() && overflow(buf.size()) < buf.size())
	return;
    ostream::write (buf);
}

/// Attempts to create more output space. Returns remaining().
ostringstream::size_type ostringstream::overflow (size_type n)
{
    if (m_pResizable && n > remaining()) {
	const uoff_t oldPos (pos());
	m_pResizable->reserve (oldPos + n, false);
	m_pResizable->resize (oldPos + n);
	link (*m_pResizable);
	seek (oldPos);
    }
    if (n > remaining())
#ifdef WANT_STREAM_BOUNDS_CHECKING
	throw stream_bounds_exception ("write", "text", pos(), n, remaining());
#else
	assert (remaining() >= n && "Buffer overrun. Check your stream size calculations.");
#endif
    return (remaining());
}

} // namespace ustl

<commit_msg>Reverted incorrectly committed hack<commit_after>// This file is part of the ustl library, an STL implementation.
//
// Copyright (C) 2005 by Mike Sharov <msharov@users.sourceforge.net>
// This file is free software, distributed under the MIT License.
//
// sostream.h
//

#include "mistream.h"	// for istream_iterator, referenced in utf8.h
#include "sostream.h"
#include "ustring.h"
#include "utf8.h"
#include "ulimits.h"
#include <stdio.h>

namespace ustl {

/// Default constructor.
ostringstream::ostringstream (void)
: ostream (),
  m_pResizable (NULL),
  m_Flags (0),
  m_Base (10),
  m_Precision (2),
  m_Width (0),
  m_DecimalSeparator ('.'),
  m_ThousandSeparator (',')
{
}

/// Creates a stream for writing into \p p of size \p n.
ostringstream::ostringstream (void* p, size_type n)
: ostream (),
  m_pResizable (NULL),
  m_Flags (0),
  m_Base (10),
  m_Precision (2),
  m_Width (0),
  m_DecimalSeparator ('.'),
  m_ThousandSeparator (',')
{
    link (p, n);
}

/// Creates a stream for writing into string \p dest.
///
/// dest may be resized by the stream if insufficient space is available.
///
ostringstream::ostringstream (string& dest)
: ostream (),
  m_pResizable (NULL),
  m_Flags (0),
  m_Base (10),
  m_Precision (2),
  m_Width (0),
  m_DecimalSeparator ('.'),
  m_ThousandSeparator (',')
{
    link (dest);
}

/// Creates a stream for writing into fixed block \p dest.
ostringstream::ostringstream (memlink& dest)
: ostream (),
  m_pResizable (NULL),
  m_Flags (0),
  m_Base (10),
  m_Precision (2),
  m_Width (0),
  m_DecimalSeparator ('.'),
  m_ThousandSeparator (',')
{
    link (dest);
}

/// Writes a single character into the stream.
void ostringstream::iwrite (uint8_t v)
{
    if (remaining() >= sizeof(uint8_t) || overflow() >= sizeof(uint8_t))
	ostream::iwrite (v);
}

/// Writes \p buf of size \p bufSize through the internal buffer.
void ostringstream::write_buffer (const char* buf, size_type bufSize)
{
    size_type btw = 0, written = 0;
    while ((written += btw) < bufSize && (remaining() || overflow(bufSize - written)))
	write (buf + written, btw = min (remaining(), bufSize - written));
}

/// Simple decimal encoding of \p n into \p fmt.
inline char* ostringstream::encode_dec (char* fmt, uint32_t n) const
{
    do {
	*fmt++ = '0' + n % 10;
    } while (n /= 10);
    return (fmt);
}

/// Generates a sprintf format string for the given type.
void ostringstream::fmtstring (char* fmt, const char* typestr, bool bInteger) const
{
    *fmt++ = '%';
    if (m_Width)
	fmt = encode_dec (fmt, m_Width);
    if (m_Flags & ios::left)
	*fmt++ = '-';
    if (!bInteger) {
	*fmt++ = '.';
	fmt = encode_dec (fmt, m_Precision);
    }
    while (*typestr)
	*fmt++ = *typestr++;
    if (bInteger) {
	if (m_Base == 16)
	    fmt[-1] = 'X';
	else if (m_Base == 8)
	    fmt[-1] = 'o';
    } else {
	if (m_Flags & ios::scientific)
	    fmt[-1] = 'E';
    }
    *fmt = 0;
}

template <typename T>
inline void ostringstream::sprintf_iwrite (T v, const char* typestr)
{
    const size_type c_BufSize = 64, c_FmtStrSize = 16;
    char fmt [c_FmtStrSize], buffer [c_BufSize];
    fmtstring (fmt, typestr, numeric_limits<T>::is_integer);
    size_type i = snprintf (buffer, c_BufSize, fmt, v);
    i = min (i, c_BufSize - 1);
    write_buffer (buffer, i);
}

void ostringstream::iwrite (int v)		{ sprintf_iwrite (v, "d"); }
void ostringstream::iwrite (unsigned int v)	{ sprintf_iwrite (v, "u"); }
void ostringstream::iwrite (long v)		{ sprintf_iwrite (v, "ld"); }
void ostringstream::iwrite (unsigned long v)	{ sprintf_iwrite (v, "lu"); }
void ostringstream::iwrite (float v)		{ simd::reset_mmx(); sprintf_iwrite (v, "f"); }
void ostringstream::iwrite (double v)		{ simd::reset_mmx(); sprintf_iwrite (v, "lf"); }
#if HAVE_LONG_LONG
void ostringstream::iwrite (long long v)	{ sprintf_iwrite (v, "lld"); }
void ostringstream::iwrite (unsigned long long v) { sprintf_iwrite (v, "llu"); }
#endif

/// Writes \p v into the stream as utf8
void ostringstream::iwrite (wchar_t v)
{
    char buffer [9];
    *utf8out(buffer) = v;
    write_buffer (buffer, Utf8Bytes(v));
}

/// Writes value \p v into the stream as text.
void ostringstream::iwrite (bool v)
{
    static const char* c_Names[2] = { "false", "true" };
    write_buffer (c_Names[v], 5 - v);
}

/// Writes string \p s into the stream.
void ostringstream::iwrite (const char* s)
{
    write_buffer (s, strlen(s));
}

/// Writes string \p v into the stream.
void ostringstream::iwrite (const string& v)
{
    write_buffer (v.begin(), v.size());
}

/// Equivalent to a vsprintf on the string.
int ostringstream::vformat (const char* fmt, va_list args)
{
#if HAVE_VA_COPY
    va_list args2;
    __va_copy (args2, args);    // Because vsnprintf will iterate over args, changing them.
#else
    #define args2 args
#endif
    const bool bIsString (m_pResizable);
    int rv = vsnprintf (ipos(), remaining() + bIsString, fmt, args);
    if (uoff_t(rv) >= remaining() + bIsString)
	rv = vsnprintf (ipos(), overflow(rv) + bIsString, fmt, args2);
    skip (min (uoff_t(rv), remaining()));
    return (rv);
}

/// Equivalent to a sprintf on the string.
int ostringstream::format (const char* fmt, ...)
{
    simd::reset_mmx();
    va_list args;
    va_start (args, fmt);
    const int rv = vformat (fmt, args);
    va_end (args);
    return (rv);
}

/// Sets the flag \p f in the stream.
void ostringstream::iwrite (ios::fmtflags f)
{
    switch (f) {
	case ios::oct:		set_base (8); break;
	case ios::dec:		set_base (10); break;
	case ios::hex:		set_base (16); break;
	case ios::left:
	    m_Flags |= ios::left;
	    m_Flags &= ~ios::right;
	    break;
	case ios::right:
	    m_Flags |= ios::right;
	    m_Flags &= ~ios::left;
	    break;
	default:
	    m_Flags |= f;
	    break;
    }
}

/// Links to string \p l as resizable.
void ostringstream::link (string& l)
{
    if (l.is_linked())
	l.reserve (l.capacity());
    assert (l.data() && "The output string buffer must not be read-only");
    ostream::link (l);
    m_pResizable = &l;
}

/// Unlinks the stream from its bound buffer.
void ostringstream::unlink (void)
{
    ostream::unlink();
    m_pResizable = NULL;
}

/// Writes the contents of \p buffer of \p size into the stream.
void ostringstream::write (const void* buffer, size_type sz)
{
    if (remaining() < sz && overflow(sz) < sz)
	return;
    ostream::write (buffer, sz);
}

/// Writes the contents of \p buf into the stream.
void ostringstream::write (const cmemlink& buf)
{
    if (remaining() < buf.size() && overflow(buf.size()) < buf.size())
	return;
    ostream::write (buf);
}

/// Attempts to create more output space. Returns remaining().
ostringstream::size_type ostringstream::overflow (size_type n)
{
    if (m_pResizable && n > remaining()) {
	const uoff_t oldPos (pos());
	m_pResizable->reserve (oldPos + n, false);
	m_pResizable->resize (oldPos + n);
	link (*m_pResizable);
	seek (oldPos);
    }
    if (n > remaining())
#ifdef WANT_STREAM_BOUNDS_CHECKING
	throw stream_bounds_exception ("write", "text", pos(), n, remaining());
#else
	assert (remaining() >= n && "Buffer overrun. Check your stream size calculations.");
#endif
    return (remaining());
}

} // namespace ustl

<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- *
 *                     OpenSim:  OptimizationExample.cpp                      *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Samuel R. Hamner, Ajay Seth                                     *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

/*
 *  Example of an OpenSim program that optimizes the performance of a model.
 *  The main() loads the arm26 model and maximizes the forward velocity of
 *  the hand during a muscle-driven forward simulation by finding the set
 *  of (constant) controls.
 */

//==============================================================================
//==============================================================================
#include <OpenSim/OpenSim.h>
#include "OpenSim/Common/STOFileAdapter.h"
#include <ctime>  // clock(), clock_t, CLOCKS_PER_SEC

using namespace OpenSim;
using namespace SimTK;
using namespace std;

// Global variables to define integration time window, optimizer step count,
// the best solution.
int stepCount = 0;
double initialTime = 0.0;
double finalTime = 0.25;
double bestSoFar = Infinity;

class ExampleOptimizationSystem : public OptimizerSystem {
   public:

       /* Constructor class. Parameters passed are accessed in the objectiveFunc() class. */
       ExampleOptimizationSystem(int numParameters, State& s, Model& aModel):
           OptimizerSystem(numParameters),
           numControls(numParameters),
           si(s),
           osimModel(aModel)
       {
           // Create the integrator for the simulation.
           p_integrator = new RungeKuttaMersonIntegrator(osimModel.getMultibodySystem());
           p_integrator->setAccuracy(1.0e-7);
       }

    int objectiveFunc(  const Vector &newControls, bool new_coefficients, Real& f ) const override {

        // make a copy of the initial states
        State s = si;

        // Update the control values
        osimModel.updDefaultControls() = newControls;

        // Integrate from initial time to final time
        Manager manager(osimModel, *p_integrator);
        s.setTime(initialTime);

        osimModel.getMultibodySystem().realize(s, Stage::Acceleration);

        manager.integrate(s, finalTime);

        /* Calculate the scalar quantity we want to minimize or maximize.
        *  In this case, were maximizing forward velocity of the
        *  forearm/hand mass center, so to maximize, compute velocity
        *  and multiply it by -1.
        */
        const auto& hand = osimModel.getComponent<OpenSim::Body>("r_ulna_radius_hand");
        osimModel.getMultibodySystem().realize(s, Stage::Velocity);
        Vec3 massCenter = hand.getMassCenter();
        Vec3 velocity = hand.findStationVelocityInGround(s, massCenter);
        f = -velocity[0];
        stepCount++;

        // Use an if statement to only store and print the results of an
        //  optimization step if it is better than a previous result.
        if(f < bestSoFar) {
            bestSoFar = f;
            cout << "\nobjective evaluation #: " << stepCount << "  controls = "
                 << newControls <<  " bestSoFar = " << f << std::endl;
        }

      return 0;

   }

private:
    int numControls;
    State& si;
    Model& osimModel;
    SimTK::ReferencePtr<RungeKuttaMersonIntegrator> p_integrator;

 };

//______________________________________________________________________________
/**
 * Define an optimization problem that finds a set of muscle controls to maximize
 * the forward velocity of the forearm/hand segment mass center.
 */
int main()
{
    try {
        std::clock_t startTime = std::clock();

        // Use Millard2012Equilibrium muscles with rigid tendons for better
        // performance.
        Object::renameType("Thelen2003Muscle", "Millard2012EquilibriumMuscle");

        // Create a new OpenSim model. This model is similar to the arm26 model,
        // but without wrapping surfaces for better performance.
        Model osimModel("Arm26_Optimize.osim");

        // Initialize the system and get the state representing the state system
        State& si = osimModel.initSystem();

        // Initialize the starting shoulder angle.
        const CoordinateSet& coords = osimModel.getCoordinateSet();
        coords.get("r_shoulder_elev").setValue(si, -1.57079633);

        // Set the initial muscle activations and make all tendons rigid.
        const Set<Muscle> &muscleSet = osimModel.getMuscles();
        for(int i=0; i<muscleSet.getSize(); ++i) {
            muscleSet[i].setActivation(si, 0.01);
            muscleSet[i].setIgnoreTendonCompliance(si, true);
        }

        // Make sure the muscles states are in equilibrium
        osimModel.equilibrateMuscles(si);

        // The number of controls will equal the number of muscles in the model!
        int numControls = osimModel.getNumControls();

        // Initialize the optimizer system we've defined.
        ExampleOptimizationSystem sys(numControls, si, osimModel);
        Real f = NaN;

        /* Define initial values and bounds for the controls to optimize */
        Vector controls(numControls, 0.01);
        controls[3] = 0.99;
        controls[4] = 0.99;
        controls[5] = 0.99;

        Vector lower_bounds(numControls, 0.01);
        Vector upper_bounds(numControls, 0.99);

        sys.setParameterLimits( lower_bounds, upper_bounds );

        // Create an optimizer. Pass in our OptimizerSystem
        // and the name of the optimization algorithm.
        Optimizer opt(sys, SimTK::LBFGSB);
        //Optimizer opt(sys, InteriorPoint);

        // Specify settings for the optimizer
        opt.setConvergenceTolerance(0.1);
        opt.useNumericalGradient(true, 1e-5);
        opt.setMaxIterations(100);
        opt.setLimitedMemoryHistory(500);

        // Optimize it!
        f = opt.optimize(controls);

        cout << "Elapsed time = " << (std::clock()-startTime)/CLOCKS_PER_SEC << "s" << endl;

        const Set<Actuator>& actuators = osimModel.getActuators();
        for(int i=0; i<actuators.getSize(); ++i){
            cout << actuators[i].getName() << " control value = " << controls[i] << endl;
        }

        cout << "\nMaximum hand velocity = " << -f << "m/s" << endl;

        cout << "OpenSim example completed successfully." << endl;

        // Dump out optimization results to a text file for testing
        ofstream ofile;
        ofile.open("Arm26_optimization_result");
        for(int i=0; i<actuators.getSize(); ++i)
            ofile << controls[i] << endl;
        ofile << -f <<endl;
        ofile.close();

        // Re-run simulation with optimal controls.
        RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(1.0e-7);
        Manager manager(osimModel, integrator);
        osimModel.updDefaultControls() = controls;

        // Integrate from initial time to final time.
        si.setTime(initialTime);
        osimModel.getMultibodySystem().realize(si, Stage::Acceleration);
        manager.integrate(si, finalTime);

        auto statesTable = manager.getStatesTable();
        STOFileAdapter_<double>::write(statesTable,
                                      "Arm26_optimized_states.sto");
    }
    catch (const std::exception& ex)
    {
        std::cout << ex.what() << std::endl;
        return 1;
    }

    // End of main() routine.
    return 0;
}
<commit_msg>Speedup optimization example loosening the accuracy from 1e-7 to 1e-5 for numerical integration and for the estimate of the objective function accuracy. Also greatly reduce the maximum number of iterations so if the tests is to fail it should do some more quickly.<commit_after>/* -------------------------------------------------------------------------- *
 *                     OpenSim:  OptimizationExample.cpp                      *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Samuel R. Hamner, Ajay Seth                                     *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

/*
 *  Example of an OpenSim program that optimizes the performance of a model.
 *  The main() loads the arm26 model and maximizes the forward velocity of
 *  the hand during a muscle-driven forward simulation by finding the set
 *  of (constant) controls.
 */

//==============================================================================
//==============================================================================
#include <OpenSim/OpenSim.h>
#include "OpenSim/Common/STOFileAdapter.h"
#include <ctime>  // clock(), clock_t, CLOCKS_PER_SEC

using namespace OpenSim;
using namespace SimTK;
using namespace std;

// Global variables to define integration time window, optimizer step count,
// the best solution.
int stepCount = 0;
const double initialTime = 0.0;
const double finalTime = 0.25;
const double desired_accuracy = 1.0e-5;
double bestSoFar = Infinity;

class ExampleOptimizationSystem : public OptimizerSystem {
public:
    /* Constructor class. Parameters passed are accessed in the objectiveFunc() class. */
    ExampleOptimizationSystem(int numParameters, State& s, Model& aModel):
        OptimizerSystem(numParameters),
        numControls(numParameters),
        si(s),
        osimModel(aModel)
    {
        // Create the integrator for the simulation.
        p_integrator = new RungeKuttaMersonIntegrator(osimModel.getMultibodySystem());
        p_integrator->setAccuracy(desired_accuracy);
    }

    int objectiveFunc(const Vector &newControls,
        bool new_coefficients, Real& f) const override {

        // make a copy of the initial states
        State s = si;

        // Update the control values
        osimModel.updDefaultControls() = newControls;

        // Integrate from initial time to final time
        Manager manager(osimModel, *p_integrator);
        s.setTime(initialTime);

        osimModel.getMultibodySystem().realize(s, Stage::Acceleration);

        manager.integrate(s, finalTime);

        /* Calculate the scalar quantity we want to minimize or maximize.
        *  In this case, were maximizing forward velocity of the
        *  forearm/hand mass center, so to maximize, compute velocity
        *  and multiply it by -1.
        */
        const auto& hand = osimModel.getComponent<OpenSim::Body>("r_ulna_radius_hand");
        osimModel.getMultibodySystem().realize(s, Stage::Velocity);
        Vec3 massCenter = hand.getMassCenter();
        Vec3 velocity = hand.findStationVelocityInGround(s, massCenter);
        f = -velocity[0];
        stepCount++;

        // Use an if statement to only store and print the results of an
        //  optimization step if it is better than a previous result.
        if(f < bestSoFar) {
            bestSoFar = f;
            cout << "\nobjective evaluation #: " << stepCount << "  controls = "
                 << newControls <<  " bestSoFar = " << f << std::endl;
        }

      return 0;

   }

private:
    int numControls;
    State& si;
    Model& osimModel;
    SimTK::ReferencePtr<RungeKuttaMersonIntegrator> p_integrator;

 };

//______________________________________________________________________________
/**
 * Define an optimization problem that finds a set of muscle controls to maximize
 * the forward velocity of the forearm/hand segment mass center.
 */
int main()
{
    try {
        std::clock_t startTime = std::clock();

        // Use Millard2012Equilibrium muscles with rigid tendons for better
        // performance.
        Object::renameType("Thelen2003Muscle", "Millard2012EquilibriumMuscle");

        // Create a new OpenSim model. This model is similar to the arm26 model,
        // but without wrapping surfaces for better performance.
        Model osimModel("Arm26_Optimize.osim");

        // Initialize the system and get the state representing the state system
        State& si = osimModel.initSystem();

        // Initialize the starting shoulder angle.
        const CoordinateSet& coords = osimModel.getCoordinateSet();
        coords.get("r_shoulder_elev").setValue(si, -1.57079633);

        // Set the initial muscle activations and make all tendons rigid.
        const Set<Muscle> &muscleSet = osimModel.getMuscles();
        for(int i=0; i<muscleSet.getSize(); ++i) {
            muscleSet[i].setActivation(si, 0.01);
            muscleSet[i].setIgnoreTendonCompliance(si, true);
        }

        // Make sure the muscles states are in equilibrium
        osimModel.equilibrateMuscles(si);

        // The number of controls will equal the number of muscles in the model!
        int numControls = osimModel.getNumControls();

        // Initialize the optimizer system we've defined.
        ExampleOptimizationSystem sys(numControls, si, osimModel);
        Real f = NaN;

        /* Define initial values and bounds for the controls to optimize */
        Vector controls(numControls, 0.02);
        controls[3] = 0.99;
        controls[4] = 0.99;
        controls[5] = 0.99;

        Vector lower_bounds(numControls, 0.01);
        Vector upper_bounds(numControls, 0.99);

        sys.setParameterLimits( lower_bounds, upper_bounds );

        // Create an optimizer. Pass in our OptimizerSystem
        // and the name of the optimization algorithm.
        Optimizer opt(sys, SimTK::LBFGSB);
        //Optimizer opt(sys, InteriorPoint);

        // Specify settings for the optimizer
        opt.setConvergenceTolerance(0.1);
        opt.useNumericalGradient(true, desired_accuracy);
        opt.setMaxIterations(2);
        opt.setLimitedMemoryHistory(500);

        // Optimize it!
        f = opt.optimize(controls);

        cout << "Elapsed time = " << (std::clock()-startTime)/CLOCKS_PER_SEC << "s" << endl;

        const Set<Actuator>& actuators = osimModel.getActuators();
        for(int i=0; i<actuators.getSize(); ++i){
            cout << actuators[i].getName() << " control value = " << controls[i] << endl;
        }

        cout << "\nMaximum hand velocity = " << -f << "m/s" << endl;

        cout << "OpenSim example completed successfully." << endl;

        // Dump out optimization results to a text file for testing
        ofstream ofile;
        ofile.open("Arm26_optimization_result");
        for(int i=0; i<actuators.getSize(); ++i)
            ofile << controls[i] << endl;
        ofile << -f <<endl;
        ofile.close();

        // Re-run simulation with optimal controls.
        RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(desired_accuracy);
        Manager manager(osimModel, integrator);
        osimModel.updDefaultControls() = controls;

        // Integrate from initial time to final time.
        si.setTime(initialTime);
        osimModel.getMultibodySystem().realize(si, Stage::Acceleration);
        manager.integrate(si, finalTime);

        auto statesTable = manager.getStatesTable();
        STOFileAdapter_<double>::write(statesTable,
                                      "Arm26_optimized_states.sto");
    }
    catch (const std::exception& ex)
    {
        std::cout << ex.what() << std::endl;
        return 1;
    }

    // End of main() routine.
    return 0;
}
<|endoftext|>


<commit_before>// Copyright (c) 2011 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ppapi/tests/test_fullscreen.h"

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

#include "ppapi/c/dev/ppb_testing_dev.h"
#include "ppapi/c/dev/ppb_fullscreen_dev.h"
#include "ppapi/cpp/input_event.h"
#include "ppapi/cpp/instance.h"
#include "ppapi/cpp/module.h"
#include "ppapi/cpp/point.h"
#include "ppapi/tests/test_utils.h"
#include "ppapi/tests/testing_instance.h"

REGISTER_TEST_CASE(Fullscreen);

namespace {

bool HasMidScreen(const pp::Rect& position, const pp::Size& screen_size) {
  static int32_t mid_x = screen_size.width() / 2;
  static int32_t mid_y = screen_size.height() / 2;
  return (position.Contains(mid_x, mid_y));
}

}  // namespace

TestFullscreen::TestFullscreen(TestingInstance* instance)
    : TestCase(instance),
      error_(),
      screen_mode_(instance),
      fullscreen_pending_(false),
      normal_pending_(false),
      saw_first_fullscreen_didchangeview(false),
      graphics2d_fullscreen_(instance, pp::Size(10, 10), false),
      graphics2d_normal_(instance, pp::Size(15, 15), false),
      set_fullscreen_true_callback_(instance->pp_instance()),
      fullscreen_callback_(instance->pp_instance()),
      normal_callback_(instance->pp_instance()) {
  screen_mode_.GetScreenSize(&screen_size_);
}

bool TestFullscreen::Init() {
  if (graphics2d_fullscreen_.is_null() && graphics2d_normal_.is_null())
    return false;
  return InitTestingInterface();
}

void TestFullscreen::RunTest() {
  RUN_TEST(GetScreenSize);
  RUN_TEST(NormalToFullscreenToNormal);
}

bool TestFullscreen::GotError() {
  return !error_.empty();
}

std::string TestFullscreen::Error() {
  std::string last_error = error_;
  error_.clear();
  return last_error;
}

std::string TestFullscreen::TestGetScreenSize() {
  if (screen_size_.width() < 320 || screen_size_.width() > 2560)
    return ReportError("screen_size.width()", screen_size_.width());
  if (screen_size_.height() < 200 || screen_size_.height() > 2048)
    return ReportError("screen_size.height()", screen_size_.height());
  PASS();
}

std::string TestFullscreen::TestNormalToFullscreenToNormal() {
  // 0. Start in normal mode.
  if (screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() at start", true);

  // 1. Switch to fullscreen.
  // This is only allowed within a context of a user gesture (e.g. mouse click).
  if (screen_mode_.SetFullscreen(true))
    return ReportError("SetFullscreen(true) outside of user gesture", true);
  // The transition is asynchronous and ends at the next DidChangeView().
  // No graphics devices can be bound while in transition.
  instance_->RequestInputEvents(PP_INPUTEVENT_CLASS_MOUSE);
  // HandleInputEvent() will call SetFullscreen(true).
  // DidChangeView() will call the callback once in fullscreen mode.
  fullscreen_callback_.WaitForResult();
  if (GotError())
    return Error();
  if (fullscreen_pending_)
    return "fullscreen_pending_ has not been reset";
  if (!screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in fullscreen", false);
  if (!instance_->BindGraphics(graphics2d_fullscreen_))
    return ReportError("BindGraphics() in fullscreen", false);

  // 2. Stay in fullscreen. No change.
  if (screen_mode_.SetFullscreen(true))
    return ReportError("SetFullscreen(true) in fullscreen", true);
  if (!screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in fullscreen^2", false);

  // 3. Switch to normal.
  // The transition is asynchronous and ends at DidChangeView().
  // No graphics devices can be bound while in transition.
  normal_pending_ = true;
  if (!screen_mode_.SetFullscreen(false))
    return ReportError("SetFullscreen(false) in fullscreen", false);
  // Normal is now pending, so additional requests should fail.
  if (screen_mode_.SetFullscreen(false))
    return ReportError("SetFullscreen(false) in normal pending", true);
  if (screen_mode_.SetFullscreen(true))
    return ReportError("SetFullscreen(true) in normal pending", true);
  if (instance_->BindGraphics(graphics2d_normal_))
    return ReportError("BindGraphics() in normal transition", true);
  if (!screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in normal transition", false);
  // DidChangeView() will call the callback once out of fullscreen mode.
  normal_callback_.WaitForResult();
  if (normal_pending_)
    return "normal_pending_ has not been reset";
  if (screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in normal", true);
  if (!instance_->BindGraphics(graphics2d_fullscreen_))
    return ReportError("BindGraphics() in normal", false);

  // 4. Stay in normal. No change.
  if (screen_mode_.SetFullscreen(false))
    return ReportError("SetFullscreen(false) in normal", true);
  if (screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in normal^2", true);

  PASS();
}

void TestFullscreen::FailFullscreenTest(const std::string& error) {
  screen_mode_.SetFullscreen(false);
  fullscreen_pending_ = false;
  error_ = error;
  pp::Module::Get()->core()->CallOnMainThread(0, fullscreen_callback_);
}

// Transition to fullscreen can only happen when processing a user gesture.
bool TestFullscreen::HandleInputEvent(const pp::InputEvent& event) {
  // We only let mouse events through and only mouse clicks count.
  if (event.GetType() != PP_INPUTEVENT_TYPE_MOUSEDOWN &&
      event.GetType() != PP_INPUTEVENT_TYPE_MOUSEUP)
    return false;
  // We got the gesture. No need to handle any more events.
  instance_->ClearInputEventRequest(PP_INPUTEVENT_CLASS_MOUSE);
  fullscreen_pending_ = true;
  if (!screen_mode_.SetFullscreen(true)) {
    FailFullscreenTest(ReportError("SetFullscreen(true) in normal", false));
    return false;
  }
  // Fullscreen is now pending, so additional requests should fail.
  if (screen_mode_.SetFullscreen(true)) {
    FailFullscreenTest(
        ReportError("SetFullscreen(true) in fullscreen pending", true));
    return false;
  }
  if (screen_mode_.SetFullscreen(false)) {
    FailFullscreenTest(
        ReportError("SetFullscreen(false) in fullscreen pending", true));
  }
  // No graphics devices can be bound while in transition.
  if (instance_->BindGraphics(graphics2d_fullscreen_)) {
    FailFullscreenTest(
        ReportError("BindGraphics() in fullscreen transition", true));
    return false;
  }
  if (screen_mode_.IsFullscreen()) {
    FailFullscreenTest(
        ReportError("IsFullscreen() in fullscreen transtion", true));
    return false;
  }
  // DidChangeView() will complete the transition to fullscreen.
  return false;
}

// Transitions to/from fullscreen is asynchornous ending at DidChangeView.
// When going to fullscreen, two DidChangeView calls are generated:
// one for moving the plugin to the middle of window and one for stretching
// the window and placing the plugin in the middle of the screen.
// WebKit does not change the plugin size, but Pepper does explicitly set
// it to screen width and height when SetFullscreen(true) is called and
// resets it back when ViewChanged is received indicating that we exited
// fullscreen.
// NOTE: The number of DidChangeView calls for <object> might be different.
void TestFullscreen::DidChangeView(const pp::Rect& position,
                                   const pp::Rect& clip) {
  if (normal_position_.IsEmpty())
    normal_position_ = position;

  if (fullscreen_pending_ && !saw_first_fullscreen_didchangeview) {
    saw_first_fullscreen_didchangeview = true;
    if (!screen_mode_.IsFullscreen())
      FailFullscreenTest("DidChangeView1 is not in fullscreen");
    if (position.size() != screen_size_)
      FailFullscreenTest("DidChangeView1 does not have screen size");
    // Wait for the 2nd DidChangeView.
  } else if (fullscreen_pending_) {
    fullscreen_pending_ = false;
    saw_first_fullscreen_didchangeview = false;
    if (!screen_mode_.IsFullscreen())
      FailFullscreenTest("DidChangeView2 is not in fullscreen");
    else if (!HasMidScreen(position, screen_size_))
      FailFullscreenTest("DidChangeView2 is not in the middle of the screen");
    else if (position.size() != screen_size_)
      FailFullscreenTest("DidChangeView2 does not have screen size");
    // NOTE: we cannot reliably test for clip size being equal to the screen
    // because it might be affected by JS console, info bars, etc.
    else
      pp::Module::Get()->core()->CallOnMainThread(0, fullscreen_callback_);
  } else if (normal_pending_) {
    normal_pending_ = false;
    if (screen_mode_.IsFullscreen())
      error_ = "DidChangeview is in fullscreen";
    else if (position != normal_position_)
      error_ = "DidChangeView position is not normal";
    pp::Module::Get()->core()->CallOnMainThread(0, normal_callback_);
  }
}
<commit_msg>PPAPI Fullscreen: update ppapi_test to match the behavior after RenderWidget::is_fullscreen (used in DidChangeView to detect fullscreen mode) was implemented. Review URL: http://codereview.chromium.org/8301010<commit_after>// Copyright (c) 2011 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ppapi/tests/test_fullscreen.h"

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

#include "ppapi/c/dev/ppb_testing_dev.h"
#include "ppapi/c/dev/ppb_fullscreen_dev.h"
#include "ppapi/cpp/input_event.h"
#include "ppapi/cpp/instance.h"
#include "ppapi/cpp/module.h"
#include "ppapi/cpp/point.h"
#include "ppapi/tests/test_utils.h"
#include "ppapi/tests/testing_instance.h"

REGISTER_TEST_CASE(Fullscreen);

namespace {

bool HasMidScreen(const pp::Rect& position, const pp::Size& screen_size) {
  static int32_t mid_x = screen_size.width() / 2;
  static int32_t mid_y = screen_size.height() / 2;
  return (position.Contains(mid_x, mid_y));
}

}  // namespace

TestFullscreen::TestFullscreen(TestingInstance* instance)
    : TestCase(instance),
      error_(),
      screen_mode_(instance),
      fullscreen_pending_(false),
      normal_pending_(false),
      saw_first_fullscreen_didchangeview(false),
      graphics2d_fullscreen_(instance, pp::Size(10, 10), false),
      graphics2d_normal_(instance, pp::Size(15, 15), false),
      set_fullscreen_true_callback_(instance->pp_instance()),
      fullscreen_callback_(instance->pp_instance()),
      normal_callback_(instance->pp_instance()) {
  screen_mode_.GetScreenSize(&screen_size_);
}

bool TestFullscreen::Init() {
  if (graphics2d_fullscreen_.is_null() && graphics2d_normal_.is_null())
    return false;
  return InitTestingInterface();
}

void TestFullscreen::RunTest() {
  RUN_TEST(GetScreenSize);
  RUN_TEST(NormalToFullscreenToNormal);
}

bool TestFullscreen::GotError() {
  return !error_.empty();
}

std::string TestFullscreen::Error() {
  std::string last_error = error_;
  error_.clear();
  return last_error;
}

std::string TestFullscreen::TestGetScreenSize() {
  if (screen_size_.width() < 320 || screen_size_.width() > 2560)
    return ReportError("screen_size.width()", screen_size_.width());
  if (screen_size_.height() < 200 || screen_size_.height() > 2048)
    return ReportError("screen_size.height()", screen_size_.height());
  PASS();
}

std::string TestFullscreen::TestNormalToFullscreenToNormal() {
  // 0. Start in normal mode.
  if (screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() at start", true);

  // 1. Switch to fullscreen.
  // This is only allowed within a context of a user gesture (e.g. mouse click).
  if (screen_mode_.SetFullscreen(true))
    return ReportError("SetFullscreen(true) outside of user gesture", true);
  // The transition is asynchronous and ends at the next DidChangeView().
  // No graphics devices can be bound while in transition.
  instance_->RequestInputEvents(PP_INPUTEVENT_CLASS_MOUSE);
  // HandleInputEvent() will call SetFullscreen(true).
  // DidChangeView() will call the callback once in fullscreen mode.
  fullscreen_callback_.WaitForResult();
  if (GotError())
    return Error();
  if (fullscreen_pending_)
    return "fullscreen_pending_ has not been reset";
  if (!screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in fullscreen", false);
  if (!instance_->BindGraphics(graphics2d_fullscreen_))
    return ReportError("BindGraphics() in fullscreen", false);

  // 2. Stay in fullscreen. No change.
  if (screen_mode_.SetFullscreen(true))
    return ReportError("SetFullscreen(true) in fullscreen", true);
  if (!screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in fullscreen^2", false);

  // 3. Switch to normal.
  // The transition is asynchronous and ends at DidChangeView().
  // No graphics devices can be bound while in transition.
  normal_pending_ = true;
  if (!screen_mode_.SetFullscreen(false))
    return ReportError("SetFullscreen(false) in fullscreen", false);
  // Normal is now pending, so additional requests should fail.
  if (screen_mode_.SetFullscreen(false))
    return ReportError("SetFullscreen(false) in normal pending", true);
  if (screen_mode_.SetFullscreen(true))
    return ReportError("SetFullscreen(true) in normal pending", true);
  if (instance_->BindGraphics(graphics2d_normal_))
    return ReportError("BindGraphics() in normal transition", true);
  if (!screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in normal transition", false);
  // DidChangeView() will call the callback once out of fullscreen mode.
  normal_callback_.WaitForResult();
  if (normal_pending_)
    return "normal_pending_ has not been reset";
  if (screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in normal", true);
  if (!instance_->BindGraphics(graphics2d_fullscreen_))
    return ReportError("BindGraphics() in normal", false);

  // 4. Stay in normal. No change.
  if (screen_mode_.SetFullscreen(false))
    return ReportError("SetFullscreen(false) in normal", true);
  if (screen_mode_.IsFullscreen())
    return ReportError("IsFullscreen() in normal^2", true);

  PASS();
}

void TestFullscreen::FailFullscreenTest(const std::string& error) {
  screen_mode_.SetFullscreen(false);
  fullscreen_pending_ = false;
  error_ = error;
  pp::Module::Get()->core()->CallOnMainThread(0, fullscreen_callback_);
}

// Transition to fullscreen can only happen when processing a user gesture.
bool TestFullscreen::HandleInputEvent(const pp::InputEvent& event) {
  // We only let mouse events through and only mouse clicks count.
  if (event.GetType() != PP_INPUTEVENT_TYPE_MOUSEDOWN &&
      event.GetType() != PP_INPUTEVENT_TYPE_MOUSEUP)
    return false;
  // We got the gesture. No need to handle any more events.
  instance_->ClearInputEventRequest(PP_INPUTEVENT_CLASS_MOUSE);
  fullscreen_pending_ = true;
  if (!screen_mode_.SetFullscreen(true)) {
    FailFullscreenTest(ReportError("SetFullscreen(true) in normal", false));
    return false;
  }
  // Fullscreen is now pending, so additional requests should fail.
  if (screen_mode_.SetFullscreen(true)) {
    FailFullscreenTest(
        ReportError("SetFullscreen(true) in fullscreen pending", true));
    return false;
  }
  if (screen_mode_.SetFullscreen(false)) {
    FailFullscreenTest(
        ReportError("SetFullscreen(false) in fullscreen pending", true));
  }
  // No graphics devices can be bound while in transition.
  if (instance_->BindGraphics(graphics2d_fullscreen_)) {
    FailFullscreenTest(
        ReportError("BindGraphics() in fullscreen transition", true));
    return false;
  }
  if (screen_mode_.IsFullscreen()) {
    FailFullscreenTest(
        ReportError("IsFullscreen() in fullscreen transtion", true));
    return false;
  }
  // DidChangeView() will complete the transition to fullscreen.
  return false;
}

// Transitions to/from fullscreen is asynchornous ending at DidChangeView.
// When going to fullscreen, two DidChangeView calls are generated:
// one for moving the plugin to the middle of window and one for stretching
// the window and placing the plugin in the middle of the screen.
// WebKit does not change the plugin size, but Pepper does explicitly set
// it to screen width and height when SetFullscreen(true) is called and
// resets it back when ViewChanged is received indicating that we exited
// fullscreen.
// NOTE: The number of DidChangeView calls for <object> might be different.
void TestFullscreen::DidChangeView(const pp::Rect& position,
                                   const pp::Rect& clip) {
  if (normal_position_.IsEmpty())
    normal_position_ = position;

  if (fullscreen_pending_ && !saw_first_fullscreen_didchangeview) {
    saw_first_fullscreen_didchangeview = true;
    if (screen_mode_.IsFullscreen())
      FailFullscreenTest("DidChangeView1 is in fullscreen");
    if (position.size() != screen_size_)
      FailFullscreenTest("DidChangeView1 does not have screen size");
    // Wait for the 2nd DidChangeView.
  } else if (fullscreen_pending_) {
    fullscreen_pending_ = false;
    saw_first_fullscreen_didchangeview = false;
    if (!screen_mode_.IsFullscreen())
      FailFullscreenTest("DidChangeView2 is not in fullscreen");
    else if (!HasMidScreen(position, screen_size_))
      FailFullscreenTest("DidChangeView2 is not in the middle of the screen");
    else if (position.size() != screen_size_)
      FailFullscreenTest("DidChangeView2 does not have screen size");
    // NOTE: we cannot reliably test for clip size being equal to the screen
    // because it might be affected by JS console, info bars, etc.
    else
      pp::Module::Get()->core()->CallOnMainThread(0, fullscreen_callback_);
  } else if (normal_pending_) {
    normal_pending_ = false;
    if (screen_mode_.IsFullscreen())
      error_ = "DidChangeview is in fullscreen";
    else if (position != normal_position_)
      error_ = "DidChangeView position is not normal";
    pp::Module::Get()->core()->CallOnMainThread(0, normal_callback_);
  }
}
<|endoftext|>


<commit_before>#include <mex.h>
#include <stdio.h>
#include <zmq.h>
#include <string.h>
#include <assert.h>
#include "protocol.h"

////////////////////////////////////////////////////////////
// Global variables
uint16_T socket_port;
uint32_T socket_timeout;
void *context;
void *zsocket;
zmq_pollitem_t poll_items [1];

////////////////////////////////////////////////////////////
// Prototypes
static void close_socket();
bool server_init(const mxArray *config);
bool server_recv(const mxArray *config, mxArray *msg_array);

////////////////////////////////////////////////////////////
// Initialise the server
bool server_init(const mxArray *config)
{
    if (context != NULL) {
        // We have already been initialised
        printf("Server: INIT called again\n");
        close_socket();
    }

    // Load config settings
    socket_port = getConfigField<uint16_T>(config, "port", "uint16");
    socket_timeout = getConfigField<uint32_T>(config, "timeout", "uint32");

    // Initialise ZMQ
    context = zmq_ctx_new();
    zsocket = zmq_socket(context, ZMQ_REP);
    if (zsocket == NULL) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:failure",
                           "Failed to create ZMQ socket:\n%s", zmq_strerror(errno));
    }
    mexAtExit(close_socket);
    int linger = 0;
    zmq_setsockopt(zsocket, ZMQ_LINGER, &linger, sizeof(linger));

    // Bind the socket
    char endpoint [30];
#ifdef _WIN32
    // Silly windows defines its own version of this function
    _snprintf_s(endpoint, 30, "tcp://*:%i", socket_port);
#else
    snprintf(endpoint, 30, "tcp://*:%i", socket_port);
#endif
    int rc = zmq_bind(zsocket, endpoint);
    if (rc != 0) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:failure",
                           "Failed to bind ZMQ socket:\n%s", zmq_strerror(errno));
    }

    // Prepare the poll items (for the RECV call)
    poll_items[0].socket = zsocket;
    poll_items[0].events = ZMQ_POLLIN;

    // Done
    printf("Server initialised\n");
    return true;
}

////////////////////////////////////////////////////////////
// Receive on the socket, or timeout
bool server_recv(const mxArray *config, mxArray *msg_array)
{
    int rc;

    if (context == NULL) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:failure",
                           "RECV called before INIT");
    }

    // Initialise a zmq_msg_t structure to receive the message
    zmq_msg_t zmsg;
    rc = zmq_msg_init(&zmsg);
    assert(rc == 0);

    // Attempt to receive a message
    errno = EAGAIN; // On windows, zmq_msg_recv does not seem to set this!
    int num_bytes = zmq_msg_recv(&zmsg, zsocket, ZMQ_DONTWAIT);
    if (num_bytes == -1) {
        // Did not receive a message ...
        if (errno == EAGAIN) {
            // ... because none were available.
            // Wait for a message to be available.
            int nevents = zmq_poll(poll_items, 1, socket_timeout);
            if (nevents == -1) {
                int poll_errno = errno;
                zmq_msg_close(&zmsg);
                mexErrMsgIdAndTxt( "MATLAB:zmq_communicate:error",
                                   "Failed to poll socket: %s", zmq_strerror(poll_errno));
            } else if (nevents == 0) {
                // Timeout.
                zmq_msg_close(&zmsg);
                return false;
            } else {
                // A message is available
                num_bytes = zmq_msg_recv(&zmsg, zsocket, 0);
            }
        } else {
            // ... because receiving failed
            int poll_errno = errno;
            zmq_msg_close(&zmsg);
            mexErrMsgIdAndTxt( "MATLAB:zmq_communicate:error",
                               "Failed to receive from socket: %s", zmq_strerror(poll_errno));
        }
    }
    //printf("Server received %i bytes\n", num_bytes);
    uint8_T *msg_zmq_data = (uint8_T *)zmq_msg_data(&zmsg);

    // Allocate memory on the MATLAB side
    uint8_T *msg_matlab_data = (uint8_T *)mxCalloc(num_bytes, sizeof(uint8_T));

    // Copy the memory
    memcpy(msg_matlab_data, msg_zmq_data, num_bytes);

    // Link the MATLAB memory to the return value
    mxSetData(msg_array, msg_matlab_data);
    mxSetM(msg_array, 1);
    mxSetN(msg_array, num_bytes);

    // Free the message
    zmq_msg_close(&zmsg);

return true;
}

////////////////////////////////////////////////////////////
// Send on the socket
bool server_send(const mxArray *data)
{
    int rc;

    uint8_T *req_data = (uint8_T*) mxGetData(data);
    size_t req_size = mxGetNumberOfElements(data);

    rc = zmq_send(zsocket, req_data, req_size*sizeof(uint8_T), 0);
    //printf("Sent %i bytes.\n", rc);
    if (rc != req_size*sizeof(uint8_T)) {
        mexErrMsgIdAndTxt( "MATLAB:zmq_communicate:error",
                           "Failed to send message: %s", zmq_strerror(errno));
    }

    return true;
}

////////////////////////////////////////////////////////////
// Close the socket
static void close_socket()
{
    printf("server: closing ZMQ socket\n");

    if (zsocket != NULL) {
        zmq_close(zsocket);
        zsocket = NULL;
    }

    if (context != NULL) {
        zmq_ctx_destroy(context);
        context = NULL;
    }
}

////////////////////////////////////////////////////////////
// MATLAB mex function wrapper
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
    // Check for the proper number of arguments
    if (nrhs != 2) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:invalidNumInputs",
                           "Usage: [has_msg, msg] = server_communicate(mode, data)");
    }
    if (nlhs != 2) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:invalidNumOutputs",
                           "Need two return values.\n"
                           "Usage: [has_msg, msg] = server_communicate(mode, data)");
    }

    // Check the mode
    if ( (mxGetM(prhs[0]) != 1)
         || (mxGetN(prhs[0]) != 1)
         || !mxIsClass(prhs[0], "uint32") ) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:invalidInputs",
                           "Usage: server_communicate(mode, data)\n"
                           "Mode argument must be uint32");
    }

    // Initialise the data return value
    plhs[1] = mxCreateNumericMatrix(0, 0, mxUINT8_CLASS, mxREAL);

    // Check the mode, and run the appropriate function
    uint32_T *mode = (uint32_T *)mxGetData(prhs[0]);
    bool success;

    switch (*mode) {
    case SERVER_INIT:
        success = server_init(prhs[1]);
        break;
    case SERVER_RECV:
        success = server_recv(prhs[1], plhs[1]);
        break;
    case SERVER_SEND:
        success = server_send(prhs[1]);
        break;
    default:
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:invalidInputs",
                           "Usage: server_communicate(mode, data)\n"
                           "Invalid mode argument");

    }

    // Set the success flag
    mxLogical *successData = (mxLogical *)mxCalloc(1, sizeof(mxLogical));
    *successData = success;
    plhs[0] = mxCreateNumericMatrix(0, 0, mxLOGICAL_CLASS, mxREAL);
    mxSetData(plhs[0], successData);
    mxSetM(plhs[0], 1);
    mxSetN(plhs[0], 1);

}
<commit_msg>Squashed '+netsrv/' changes from c497090..ab56df2<commit_after>#include <mex.h>
#include <stdio.h>
#include <zmq.h>
#include <string.h>
#include <assert.h>
#include "protocol.h"

////////////////////////////////////////////////////////////
// Global variables
uint16_T socket_port;
uint32_T socket_timeout;
void *context;
void *zsocket;
zmq_pollitem_t poll_items [1];

////////////////////////////////////////////////////////////
// Prototypes
static void close_socket();
bool server_init(const mxArray *config);
bool server_recv(const mxArray *config, mxArray *msg_array);

////////////////////////////////////////////////////////////
// Initialise the server
bool server_init(const mxArray *config)
{
    if (context != NULL) {
        // We have already been initialised
        printf("Server: INIT called again\n");
        close_socket();
    }

    // Load config settings
    socket_port = getConfigField<uint16_T>(config, "port", "uint16");
    socket_timeout = getConfigField<uint32_T>(config, "timeout", "uint32");

    // Initialise ZMQ
    context = zmq_ctx_new();
    zsocket = zmq_socket(context, ZMQ_REP);
    if (zsocket == NULL) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:failure",
                           "Failed to create ZMQ socket:\n%s", zmq_strerror(errno));
    }
    mexAtExit(close_socket);
    int linger = 0;
    zmq_setsockopt(zsocket, ZMQ_LINGER, &linger, sizeof(linger));

    // Bind the socket
    char endpoint [30];
#ifdef _WIN32
    // Silly windows defines its own version of this function
    _snprintf_s(endpoint, 30, "tcp://*:%i", socket_port);
#else
    snprintf(endpoint, 30, "tcp://*:%i", socket_port);
#endif
    int rc = zmq_bind(zsocket, endpoint);
    if (rc != 0) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:failure",
                           "Failed to bind ZMQ socket:\n%s", zmq_strerror(errno));
    }

    // Prepare the poll items (for the RECV call)
    poll_items[0].socket = zsocket;
    poll_items[0].events = ZMQ_POLLIN;

    // Done
    printf("Server initialised\n");
    return true;
}

////////////////////////////////////////////////////////////
// Receive on the socket, or timeout
bool server_recv(const mxArray *config, mxArray *msg_array)
{
    int rc;

    if (context == NULL) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:failure",
                           "RECV called before INIT");
    }

    // Initialise a zmq_msg_t structure to receive the message
    zmq_msg_t zmsg;
    rc = zmq_msg_init(&zmsg);
    assert(rc == 0);

    // Attempt to receive a message
    errno = EAGAIN; // On windows, zmq_msg_recv does not seem to set this!
    int num_bytes = zmq_msg_recv(&zmsg, zsocket, ZMQ_DONTWAIT);
    if (num_bytes == -1) {
        // Did not receive a message ...
        if (errno == EAGAIN) {
            // ... because none were available.
            // Wait for a message to be available.
            int nevents = zmq_poll(poll_items, 1, socket_timeout);
            if (nevents == -1) {
                if (errno == EINTR) {
                    // System call was interrupted. Treat this like a timeout.
                    zmq_msg_close(&zmsg);
                    return false;
                }
                int poll_errno = errno;
                zmq_msg_close(&zmsg);
                mexErrMsgIdAndTxt( "MATLAB:zmq_communicate:error",
                                   "Failed to poll socket: %s", zmq_strerror(poll_errno));
            } else if (nevents == 0) {
                // Timeout.
                zmq_msg_close(&zmsg);
                return false;
            } else {
                // A message is available
                num_bytes = zmq_msg_recv(&zmsg, zsocket, 0);
            }
        } else {
            // ... because receiving failed
            int poll_errno = errno;
            zmq_msg_close(&zmsg);
            mexErrMsgIdAndTxt( "MATLAB:zmq_communicate:error",
                               "Failed to receive from socket: %s", zmq_strerror(poll_errno));
        }
    }
    //printf("Server received %i bytes\n", num_bytes);
    uint8_T *msg_zmq_data = (uint8_T *)zmq_msg_data(&zmsg);

    // Allocate memory on the MATLAB side
    uint8_T *msg_matlab_data = (uint8_T *)mxCalloc(num_bytes, sizeof(uint8_T));

    // Copy the memory
    memcpy(msg_matlab_data, msg_zmq_data, num_bytes);

    // Link the MATLAB memory to the return value
    mxSetData(msg_array, msg_matlab_data);
    mxSetM(msg_array, 1);
    mxSetN(msg_array, num_bytes);

    // Free the message
    zmq_msg_close(&zmsg);

return true;
}

////////////////////////////////////////////////////////////
// Send on the socket
bool server_send(const mxArray *data)
{
    int rc;

    uint8_T *req_data = (uint8_T*) mxGetData(data);
    size_t req_size = mxGetNumberOfElements(data);

    rc = zmq_send(zsocket, req_data, req_size*sizeof(uint8_T), 0);
    //printf("Sent %i bytes.\n", rc);
    if (rc != req_size*sizeof(uint8_T)) {
        mexErrMsgIdAndTxt( "MATLAB:zmq_communicate:error",
                           "Failed to send message: %s", zmq_strerror(errno));
    }

    return true;
}

////////////////////////////////////////////////////////////
// Close the socket
static void close_socket()
{
    printf("server: closing ZMQ socket\n");

    if (zsocket != NULL) {
        zmq_close(zsocket);
        zsocket = NULL;
    }

    if (context != NULL) {
        zmq_ctx_destroy(context);
        context = NULL;
    }
}

////////////////////////////////////////////////////////////
// MATLAB mex function wrapper
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
    // Check for the proper number of arguments
    if (nrhs != 2) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:invalidNumInputs",
                           "Usage: [has_msg, msg] = server_communicate(mode, data)");
    }
    if (nlhs != 2) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:invalidNumOutputs",
                           "Need two return values.\n"
                           "Usage: [has_msg, msg] = server_communicate(mode, data)");
    }

    // Check the mode
    if ( (mxGetM(prhs[0]) != 1)
         || (mxGetN(prhs[0]) != 1)
         || !mxIsClass(prhs[0], "uint32") ) {
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:invalidInputs",
                           "Usage: server_communicate(mode, data)\n"
                           "Mode argument must be uint32");
    }

    // Initialise the data return value
    plhs[1] = mxCreateNumericMatrix(0, 0, mxUINT8_CLASS, mxREAL);

    // Check the mode, and run the appropriate function
    uint32_T *mode = (uint32_T *)mxGetData(prhs[0]);
    bool success;

    switch (*mode) {
    case SERVER_INIT:
        success = server_init(prhs[1]);
        break;
    case SERVER_RECV:
        success = server_recv(prhs[1], plhs[1]);
        break;
    case SERVER_SEND:
        success = server_send(prhs[1]);
        break;
    default:
        mexErrMsgIdAndTxt( "MATLAB:server_communicate:invalidInputs",
                           "Usage: server_communicate(mode, data)\n"
                           "Invalid mode argument");

    }

    // Set the success flag
    mxLogical *successData = (mxLogical *)mxCalloc(1, sizeof(mxLogical));
    *successData = success;
    plhs[0] = mxCreateNumericMatrix(0, 0, mxLOGICAL_CLASS, mxREAL);
    mxSetData(plhs[0], successData);
    mxSetM(plhs[0], 1);
    mxSetN(plhs[0], 1);

}
<|endoftext|>