<commit_before>#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <vector>
#include <algorithm>
#include <chrono>
#include <thread>
#include <xmmintrin.h>

#include "engine.hpp"
#include "util.hpp"

namespace rack {

float sampleRate;
float sampleTime;
bool gPaused = false;

static bool running = false;

static std::mutex mutex;
static std::thread thread;
static VIPMutex vipMutex;

static std::vector<Module*> modules;
static std::vector<Wire*> wires;

// Parameter interpolation
static Module *smoothModule = NULL;
static int smoothParamId;
static float smoothValue;

float Light::getBrightness() {
	return sqrtf(fmaxf(0.0, value));
}

void Light::setBrightnessSmooth(float brightness) {
	// lambda = 3 * framerate
	value += (brightness * brightness - value) * sampleTime * (60.0 * 3.0);
}

void Wire::step() {
	float value = outputModule->outputs[outputId].value;
	inputModule->inputs[inputId].value = value;
}

void engineInit() {
	engineSetSampleRate(44100.0);
}

void engineDestroy() {
	// Make sure there are no wires or modules in the rack on destruction. This suggests that a module failed to remove itself before the GUI was destroyed.
	assert(wires.empty());
	assert(modules.empty());
}

static void engineStep() {
	// Param interpolation
	if (smoothModule) {
		float value = smoothModule->params[smoothParamId].value;
		const float lambda = 60.0; // decay rate is 1 graphics frame
		const float snap = 0.0001;
		float delta = smoothValue - value;
		if (fabsf(delta) < snap) {
			smoothModule->params[smoothParamId].value = smoothValue;
			smoothModule = NULL;
		}
		else {
			value += delta * lambda * sampleTime;
			smoothModule->params[smoothParamId].value = value;
		}
	}

	// Step modules
	for (Module *module : modules) {
		module->step();
	}

	// Step cables by moving their output values to inputs
	for (Wire *wire : wires) {
		wire->step();
	}
}

static void engineRun() {
	// Set CPU to denormals-are-zero mode
	// http://carlh.net/plugins/denormals.php
	_MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);

	// Every time the engine waits and locks a mutex, it steps this many frames
	const int mutexSteps = 64;
	// Time in seconds that the engine is rushing ahead of the estimated clock time
	double ahead = 0.0;
	auto lastTime = std::chrono::high_resolution_clock::now();

	while (running) {
		vipMutex.wait();

		if (!gPaused) {
			std::lock_guard<std::mutex> lock(mutex);
			for (int i = 0; i < mutexSteps; i++) {
				engineStep();
			}
		}

		double stepTime = mutexSteps * sampleTime;
		ahead += stepTime;
		auto currTime = std::chrono::high_resolution_clock::now();
		const double aheadFactor = 2.0;
		ahead -= aheadFactor * std::chrono::duration<double>(currTime - lastTime).count();
		lastTime = currTime;
		ahead = fmaxf(ahead, 0.0);

		// Avoid pegging the CPU at 100% when there are no "blocking" modules like AudioInterface, but still step audio at a reasonable rate
		// The number of steps to wait before possibly sleeping
		const double aheadMax = 1.0; // seconds
		if (ahead > aheadMax) {
			std::this_thread::sleep_for(std::chrono::duration<double>(stepTime));
		}
	}
}

void engineStart() {
	running = true;
	thread = std::thread(engineRun);
}

void engineStop() {
	running = false;
	thread.join();
}

void engineAddModule(Module *module) {
	assert(module);
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// Check that the module is not already added
	auto it = std::find(modules.begin(), modules.end(), module);
	assert(it == modules.end());
	modules.push_back(module);
}

void engineRemoveModule(Module *module) {
	assert(module);
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// If a param is being smoothed on this module, stop smoothing it immediately
	if (module == smoothModule) {
		smoothModule = NULL;
	}
	// Check that all wires are disconnected
	for (Wire *wire : wires) {
		assert(wire->outputModule != module);
		assert(wire->inputModule != module);
	}
	// Check that the module actually exists
	auto it = std::find(modules.begin(), modules.end(), module);
	assert(it != modules.end());
	// Remove it
	modules.erase(it);
}

static void updateActive() {
	// Set everything to inactive
	for (Module *module : modules) {
		for (Input &input : module->inputs) {
			input.active = false;
		}
		for (Output &output : module->outputs) {
			output.active = false;
		}
	}
	// Set inputs/outputs to active
	for (Wire *wire : wires) {
		wire->outputModule->outputs[wire->outputId].active = true;
		wire->inputModule->inputs[wire->inputId].active = true;
	}
}

void engineAddWire(Wire *wire) {
	assert(wire);
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// Check wire properties
	assert(wire->outputModule);
	assert(wire->inputModule);
	// Check that the wire is not already added, and that the input is not already used by another cable
	for (Wire *wire2 : wires) {
		assert(wire2 != wire);
		assert(!(wire2->inputModule == wire->inputModule && wire2->inputId == wire->inputId));
	}
	// Add the wire
	wires.push_back(wire);
	updateActive();
}

void engineRemoveWire(Wire *wire) {
	assert(wire);
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// Check that the wire is already added
	auto it = std::find(wires.begin(), wires.end(), wire);
	assert(it != wires.end());
	// Set input to 0V
	wire->inputModule->inputs[wire->inputId].value = 0.0;
	// Remove the wire
	wires.erase(it);
	updateActive();
}

void engineSetParam(Module *module, int paramId, float value) {
	module->params[paramId].value = value;
}

void engineSetParamSmooth(Module *module, int paramId, float value) {
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// Since only one param can be smoothed at a time, if another param is currently being smoothed, skip to its final state
	if (smoothModule && !(smoothModule == module && smoothParamId == paramId)) {
		smoothModule->params[smoothParamId].value = smoothValue;
	}
	smoothModule = module;
	smoothParamId = paramId;
	smoothValue = value;
}

void engineSetSampleRate(float newSampleRate) {
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	sampleRate = newSampleRate;
	sampleTime = 1.0 / sampleRate;
	// onSampleRateChange
	for (Module *module : modules) {
		module->onSampleRateChange();
	}
}

float engineGetSampleRate() {
	return sampleRate;
}

float engineGetSampleTime() {
	return sampleTime;
}

} // namespace rack
<commit_msg>Light::setBrightnessSmooth() turns on high values immediately, fades out low values<commit_after>#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <vector>
#include <algorithm>
#include <chrono>
#include <thread>
#include <xmmintrin.h>

#include "engine.hpp"
#include "util.hpp"

namespace rack {

float sampleRate;
float sampleTime;
bool gPaused = false;

static bool running = false;

static std::mutex mutex;
static std::thread thread;
static VIPMutex vipMutex;

static std::vector<Module*> modules;
static std::vector<Wire*> wires;

// Parameter interpolation
static Module *smoothModule = NULL;
static int smoothParamId;
static float smoothValue;

float Light::getBrightness() {
	return sqrtf(fmaxf(0.0, value));
}

void Light::setBrightnessSmooth(float brightness) {
	float v = brightness * brightness;
	if (v < value) {
		// Fade out light with lambda = 3 * framerate
		value += (v - value) * sampleTime * (60.0 * 3.0);
	}
	else {
		// Immediately illuminate light
		value = v;
	}
}

void Wire::step() {
	float value = outputModule->outputs[outputId].value;
	inputModule->inputs[inputId].value = value;
}

void engineInit() {
	engineSetSampleRate(44100.0);
}

void engineDestroy() {
	// Make sure there are no wires or modules in the rack on destruction. This suggests that a module failed to remove itself before the GUI was destroyed.
	assert(wires.empty());
	assert(modules.empty());
}

static void engineStep() {
	// Param interpolation
	if (smoothModule) {
		float value = smoothModule->params[smoothParamId].value;
		const float lambda = 60.0; // decay rate is 1 graphics frame
		const float snap = 0.0001;
		float delta = smoothValue - value;
		if (fabsf(delta) < snap) {
			smoothModule->params[smoothParamId].value = smoothValue;
			smoothModule = NULL;
		}
		else {
			value += delta * lambda * sampleTime;
			smoothModule->params[smoothParamId].value = value;
		}
	}

	// Step modules
	for (Module *module : modules) {
		module->step();
	}

	// Step cables by moving their output values to inputs
	for (Wire *wire : wires) {
		wire->step();
	}
}

static void engineRun() {
	// Set CPU to denormals-are-zero mode
	// http://carlh.net/plugins/denormals.php
	_MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);

	// Every time the engine waits and locks a mutex, it steps this many frames
	const int mutexSteps = 64;
	// Time in seconds that the engine is rushing ahead of the estimated clock time
	double ahead = 0.0;
	auto lastTime = std::chrono::high_resolution_clock::now();

	while (running) {
		vipMutex.wait();

		if (!gPaused) {
			std::lock_guard<std::mutex> lock(mutex);
			for (int i = 0; i < mutexSteps; i++) {
				engineStep();
			}
		}

		double stepTime = mutexSteps * sampleTime;
		ahead += stepTime;
		auto currTime = std::chrono::high_resolution_clock::now();
		const double aheadFactor = 2.0;
		ahead -= aheadFactor * std::chrono::duration<double>(currTime - lastTime).count();
		lastTime = currTime;
		ahead = fmaxf(ahead, 0.0);

		// Avoid pegging the CPU at 100% when there are no "blocking" modules like AudioInterface, but still step audio at a reasonable rate
		// The number of steps to wait before possibly sleeping
		const double aheadMax = 1.0; // seconds
		if (ahead > aheadMax) {
			std::this_thread::sleep_for(std::chrono::duration<double>(stepTime));
		}
	}
}

void engineStart() {
	running = true;
	thread = std::thread(engineRun);
}

void engineStop() {
	running = false;
	thread.join();
}

void engineAddModule(Module *module) {
	assert(module);
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// Check that the module is not already added
	auto it = std::find(modules.begin(), modules.end(), module);
	assert(it == modules.end());
	modules.push_back(module);
}

void engineRemoveModule(Module *module) {
	assert(module);
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// If a param is being smoothed on this module, stop smoothing it immediately
	if (module == smoothModule) {
		smoothModule = NULL;
	}
	// Check that all wires are disconnected
	for (Wire *wire : wires) {
		assert(wire->outputModule != module);
		assert(wire->inputModule != module);
	}
	// Check that the module actually exists
	auto it = std::find(modules.begin(), modules.end(), module);
	assert(it != modules.end());
	// Remove it
	modules.erase(it);
}

static void updateActive() {
	// Set everything to inactive
	for (Module *module : modules) {
		for (Input &input : module->inputs) {
			input.active = false;
		}
		for (Output &output : module->outputs) {
			output.active = false;
		}
	}
	// Set inputs/outputs to active
	for (Wire *wire : wires) {
		wire->outputModule->outputs[wire->outputId].active = true;
		wire->inputModule->inputs[wire->inputId].active = true;
	}
}

void engineAddWire(Wire *wire) {
	assert(wire);
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// Check wire properties
	assert(wire->outputModule);
	assert(wire->inputModule);
	// Check that the wire is not already added, and that the input is not already used by another cable
	for (Wire *wire2 : wires) {
		assert(wire2 != wire);
		assert(!(wire2->inputModule == wire->inputModule && wire2->inputId == wire->inputId));
	}
	// Add the wire
	wires.push_back(wire);
	updateActive();
}

void engineRemoveWire(Wire *wire) {
	assert(wire);
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// Check that the wire is already added
	auto it = std::find(wires.begin(), wires.end(), wire);
	assert(it != wires.end());
	// Set input to 0V
	wire->inputModule->inputs[wire->inputId].value = 0.0;
	// Remove the wire
	wires.erase(it);
	updateActive();
}

void engineSetParam(Module *module, int paramId, float value) {
	module->params[paramId].value = value;
}

void engineSetParamSmooth(Module *module, int paramId, float value) {
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	// Since only one param can be smoothed at a time, if another param is currently being smoothed, skip to its final state
	if (smoothModule && !(smoothModule == module && smoothParamId == paramId)) {
		smoothModule->params[smoothParamId].value = smoothValue;
	}
	smoothModule = module;
	smoothParamId = paramId;
	smoothValue = value;
}

void engineSetSampleRate(float newSampleRate) {
	VIPLock vipLock(vipMutex);
	std::lock_guard<std::mutex> lock(mutex);
	sampleRate = newSampleRate;
	sampleTime = 1.0 / sampleRate;
	// onSampleRateChange
	for (Module *module : modules) {
		module->onSampleRateChange();
	}
}

float engineGetSampleRate() {
	return sampleRate;
}

float engineGetSampleTime() {
	return sampleTime;
}

} // namespace rack
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: e3ditem.cxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: hr $ $Date: 2006-06-19 16:11:15 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _COM_SUN_STAR_DRAWING_DIRECTION3D_HPP_
#include <com/sun/star/drawing/Direction3D.hpp>
#endif
#ifndef _STREAM_HXX
#include <tools/stream.hxx>
#endif

#include "e3ditem.hxx"

using namespace ::rtl;
using namespace ::com::sun::star;

// STATIC DATA -----------------------------------------------------------

DBG_NAMEEX(SvxVector3DItem);
DBG_NAME(SvxVector3DItem);

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

TYPEINIT1_AUTOFACTORY(SvxVector3DItem, SfxPoolItem);

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

SvxVector3DItem::SvxVector3DItem()
{
    DBG_CTOR(SvxVector3DItem, 0);
}

SvxVector3DItem::~SvxVector3DItem()
{
    DBG_DTOR(SvxVector3DItem, 0);
}

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

SvxVector3DItem::SvxVector3DItem( USHORT _nWhich, const Vector3D& rVal ) :
    SfxPoolItem( _nWhich ),
    aVal( rVal )
{
    DBG_CTOR(SvxVector3DItem, 0);
}

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

SvxVector3DItem::SvxVector3DItem( USHORT _nWhich, SvStream& rStream ) :
    SfxPoolItem( _nWhich )
{
    DBG_CTOR(SvxVector3DItem, 0);
    rStream >> aVal;
}

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

SvxVector3DItem::SvxVector3DItem( const SvxVector3DItem& rItem ) :
    SfxPoolItem( rItem ),
    aVal( rItem.aVal )
{
    DBG_CTOR(SvxVector3DItem, 0);
}

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

int SvxVector3DItem::operator==( const SfxPoolItem &rItem ) const
{
    DBG_CHKTHIS(SvxVector3DItem, 0);
    DBG_ASSERT( SfxPoolItem::operator==( rItem ), "unequal type" );
    return ((SvxVector3DItem&)rItem).aVal == aVal;
}

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

SfxPoolItem* SvxVector3DItem::Clone( SfxItemPool * ) const
{
    DBG_CHKTHIS(SvxVector3DItem, 0);
    return new SvxVector3DItem( *this );
}

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

SfxPoolItem* SvxVector3DItem::Create(SvStream &rStream, USHORT /*nVersion*/) const
{
    DBG_CHKTHIS(SvxVector3DItem, 0);
    Vector3D aStr;
    rStream >> aStr;
    return new SvxVector3DItem(Which(), aStr);
}

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

SvStream& SvxVector3DItem::Store(SvStream &rStream, USHORT /*nItemVersion*/) const
{
    DBG_CHKTHIS(SvxVector3DItem, 0);

    // ## if (nItemVersion)
    rStream << aVal;

    return rStream;
}

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

sal_Bool SvxVector3DItem::QueryValue( uno::Any& rVal, BYTE /*nMemberId*/ ) const
{
    drawing::Direction3D aDirection;

    // Werte eintragen
    aDirection.DirectionX = aVal.X();
    aDirection.DirectionY = aVal.Y();
    aDirection.DirectionZ = aVal.Z();

    rVal <<= aDirection;
    return( sal_True );
}

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

sal_Bool SvxVector3DItem::PutValue( const uno::Any& rVal, BYTE /*nMemberId*/ )
{
    drawing::Direction3D aDirection;
    if(!(rVal >>= aDirection))
        return sal_False;

    aVal.X() = aDirection.DirectionX;
    aVal.Y() = aDirection.DirectionY;
    aVal.Z() = aDirection.DirectionZ;
    return sal_True;
}

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

USHORT SvxVector3DItem::GetVersion (USHORT nFileFormatVersion) const
{
    return (nFileFormatVersion == SOFFICE_FILEFORMAT_31) ? USHRT_MAX : 0;
}

<commit_msg>INTEGRATION: CWS pchfix02 (1.5.114); FILE MERGED 2006/09/01 17:46:59 kaib 1.5.114.1: #i68856# Added header markers and pch files<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: e3ditem.cxx,v $
 *
 *  $Revision: 1.6 $
 *
 *  last change: $Author: obo $ $Date: 2006-09-17 05:19:29 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_svx.hxx"

#ifndef _COM_SUN_STAR_DRAWING_DIRECTION3D_HPP_
#include <com/sun/star/drawing/Direction3D.hpp>
#endif
#ifndef _STREAM_HXX
#include <tools/stream.hxx>
#endif

#include "e3ditem.hxx"

using namespace ::rtl;
using namespace ::com::sun::star;

// STATIC DATA -----------------------------------------------------------

DBG_NAMEEX(SvxVector3DItem);
DBG_NAME(SvxVector3DItem);

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

TYPEINIT1_AUTOFACTORY(SvxVector3DItem, SfxPoolItem);

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

SvxVector3DItem::SvxVector3DItem()
{
    DBG_CTOR(SvxVector3DItem, 0);
}

SvxVector3DItem::~SvxVector3DItem()
{
    DBG_DTOR(SvxVector3DItem, 0);
}

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

SvxVector3DItem::SvxVector3DItem( USHORT _nWhich, const Vector3D& rVal ) :
    SfxPoolItem( _nWhich ),
    aVal( rVal )
{
    DBG_CTOR(SvxVector3DItem, 0);
}

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

SvxVector3DItem::SvxVector3DItem( USHORT _nWhich, SvStream& rStream ) :
    SfxPoolItem( _nWhich )
{
    DBG_CTOR(SvxVector3DItem, 0);
    rStream >> aVal;
}

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

SvxVector3DItem::SvxVector3DItem( const SvxVector3DItem& rItem ) :
    SfxPoolItem( rItem ),
    aVal( rItem.aVal )
{
    DBG_CTOR(SvxVector3DItem, 0);
}

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

int SvxVector3DItem::operator==( const SfxPoolItem &rItem ) const
{
    DBG_CHKTHIS(SvxVector3DItem, 0);
    DBG_ASSERT( SfxPoolItem::operator==( rItem ), "unequal type" );
    return ((SvxVector3DItem&)rItem).aVal == aVal;
}

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

SfxPoolItem* SvxVector3DItem::Clone( SfxItemPool * ) const
{
    DBG_CHKTHIS(SvxVector3DItem, 0);
    return new SvxVector3DItem( *this );
}

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

SfxPoolItem* SvxVector3DItem::Create(SvStream &rStream, USHORT /*nVersion*/) const
{
    DBG_CHKTHIS(SvxVector3DItem, 0);
    Vector3D aStr;
    rStream >> aStr;
    return new SvxVector3DItem(Which(), aStr);
}

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

SvStream& SvxVector3DItem::Store(SvStream &rStream, USHORT /*nItemVersion*/) const
{
    DBG_CHKTHIS(SvxVector3DItem, 0);

    // ## if (nItemVersion)
    rStream << aVal;

    return rStream;
}

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

sal_Bool SvxVector3DItem::QueryValue( uno::Any& rVal, BYTE /*nMemberId*/ ) const
{
    drawing::Direction3D aDirection;

    // Werte eintragen
    aDirection.DirectionX = aVal.X();
    aDirection.DirectionY = aVal.Y();
    aDirection.DirectionZ = aVal.Z();

    rVal <<= aDirection;
    return( sal_True );
}

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

sal_Bool SvxVector3DItem::PutValue( const uno::Any& rVal, BYTE /*nMemberId*/ )
{
    drawing::Direction3D aDirection;
    if(!(rVal >>= aDirection))
        return sal_False;

    aVal.X() = aDirection.DirectionX;
    aVal.Y() = aDirection.DirectionY;
    aVal.Z() = aDirection.DirectionZ;
    return sal_True;
}

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

USHORT SvxVector3DItem::GetVersion (USHORT nFileFormatVersion) const
{
    return (nFileFormatVersion == SOFFICE_FILEFORMAT_31) ? USHRT_MAX : 0;
}

<|endoftext|>


<commit_before>#include "rune/engine.h"

rune::Engine::Engine()
{
    _basePath = "./";
}

rune::Engine::~Engine()
{
    close();
}

void rune::Engine::init()
{
    _init_entities();
    _open = true;
    _glThread = NULL;
}

void rune::Engine::setBasePath(QString basePath)
{
    if(!basePath.endsWith("/"))
    {
        _basePath += "/";
    }

    _basePath = basePath;
}

QString rune::Engine::basePath()
{
    return _basePath;
}

void rune::Engine::close()
{
    if(!_open)
        return;

    _close_entities();

    if(_loadedMaps.size() > 0)
    {
        // unload all loaded maps
        for(QMap<QString, WorldMap*>::iterator it = _loadedMaps.begin();
            it != _loadedMaps.end();
            ++it)
        {
            qDebug() << "free map " << it.key() << " at " << it.value();
            delete it.value();
        }
    }
    _open = false;
}

bool rune::Engine::loadEntity(QString path)
{
    if(path.isEmpty())
    {
        return false;
    }

    QString loadPath = path;

    if(!loadPath.endsWith(".yml"))
    {
        loadPath += ".yml";
    }else
    {
        path = path.left(path.length()-4);
    }

    if(loadPath.startsWith("/")) {
        loadPath = loadPath.right(loadPath.length()-1);
    }

    // build complete path
    loadPath = _basePath + "entity/" + loadPath;

    QFile yFile(loadPath);
    if(!yFile.exists()  || yFile.size() <= 0)
    {
        qDebug() << "error: entity definition '" << loadPath << "' does not exist" << endl;
        return false;
    }

    // create entity for blueprint base
    Entity* entity = new Entity(this);

    YAML::Node yEntity = YAML::LoadFile(loadPath.toUtf8().constData());

    // inherit from base
    if(yEntity[rune::PROP_BASE.toStdString()])
    {
        QString baseEntity = QString::fromStdString(yEntity[rune::PROP_BASE.toStdString()].as<std::string>());
        if(!g_blueprintRegister->contains(baseEntity)) // load base entity
            if(!loadEntity(baseEntity))
            {
                delete entity;
                return false;
            }
        entity->copyFrom(*getBlueprint(baseEntity));
    }

    // set entity properties from yaml
    for(YAML::iterator it=yEntity.begin(); it != yEntity.end(); ++it){
        entity->setProperty(QString::fromStdString(it->first.as<std::string>()), QString::fromStdString(it->second.as<std::string>()));
    }

    // set entity property
    entity->setProperty(rune::PROP_ENTITY, path);

    if(g_blueprintRegister->contains(path))
    {
        unloadEntity(path);
    }

    // add to blueprint register
    g_blueprintRegister->insert(path, entity);

    return true;
}

bool rune::Engine::unloadEntity(QString path)
{
    if(path.isEmpty())
    {
        return false;
    }

    if(path.endsWith(".yml"))
    {
        path = path.left(path.length()-4);
    }

    if(!g_blueprintRegister->contains(path))
    {
        return false;
    }

    // delete entity
    Entity* e = g_blueprintRegister->value(path);
    g_blueprintRegister->remove(path);

    // TODO remove existing clones?
    delete e;

    return true;
}

QString rune::Engine::cloneEntity(QString path)
{
    if(!g_blueprintRegister->contains(path))
    {
        // blueprint was not loaded yet -> try to load
        if(!loadEntity(path))
            return ""; // blueprint could not be loaded -> error
    }

    // copy entity
    Entity* clone = new Entity(this);
    clone->copyFrom(*(g_blueprintRegister->value(path)));

    /* check if clone register is already available
    if(g_activeEntities == NULL)
    {
        // create if not
        g_activeEntities = new QMap<QString, rune::Entity*>();
    }*/

    // generate entity id
    QUuid uid = QUuid::createUuid();
    clone->setProperty(rune::PROP_UID, uid.toString());

    g_activeEntities->insert(uid.toString(), clone);

    // init interpreter
    ScriptInterpreter* si = new ScriptInterpreter(clone);
    si->bind(clone);

    return clone->getProperty(PROP_UID);
}

bool rune::Engine::modifyEntityProperty(QString uid, QString prop, QString value)
{
    // TODO modification stack
    Entity* e = getClone(uid);
    if(e == NULL)
        return false; // TODO error handling
    e->setProperty(prop, value);
    return true;
}

void rune::Engine::startGameLoop()
{
    if(_glThread != NULL)
        return; // game loop already in progress
    _glThread = new GameLoopThread(this);
    _glThread->start();
    return;
}

void rune::Engine::stopGameLoop()
{
    if(_glThread == NULL)
        return; // no game loop running

    _glThread->terminate();
    return;
}

void rune::Engine::callAction(QString uid, QString action, uint timestamp)
{
    QMutexLocker mlock(&_actionQueueMutex); // thread safe

    rune_action_queue_item i;
    i.uid       = uid;
    i.action    = action;
    i.timestamp = timestamp;

    _actionQueue.enqueue(i);
    return;
}

void rune::Engine::glFinished()
{
    emit(gameStateChanged());
}

rune::Entity *rune::Engine::getBlueprint(QString path)
{
    if(!g_blueprintRegister->contains(path))
    {
        return NULL;
    }

    return (g_blueprintRegister->value(path));
}

QQueue<rune_action_queue_item> rune::Engine::getReadyActions()
{
    QMutexLocker mlock(&_actionQueueMutex);

    uint now = QDateTime().toTime_t();
    QQueue<rune_action_queue_item> ready;
    QQueue<rune_action_queue_item> notReady;

    while(!_actionQueue.isEmpty())
    {
        rune_action_queue_item i = _actionQueue.dequeue();
        if(i.timestamp <= now)
            ready.enqueue(i);
        else
            notReady.enqueue(i);
    }

    _actionQueue = notReady;
    return ready;
}

rune::Entity *rune::Engine::getClone(QString uid)
{
    if(!g_activeEntities->contains(uid))
        return NULL;

    return g_activeEntities->value(uid);
}

rune::WorldMap *rune::Engine::loadMap(QString path)
{
    QString absPath = path;

    if(!path.startsWith("/"))
        path = "/" + path;

    if(_loadedMaps.contains(path))
    {
        if(_loadedMaps[path] != NULL)
        {
            rune::setError(RUNE_ERR_MAP_LOADED_BEFORE);
            return NULL;
        }
    }

    absPath = path.right(path.length() - 1);
    absPath = _basePath + "map/" + absPath + ".yml";

    WorldMap* map = new WorldMap(this);
    if(!map->loadMap(absPath))
        return NULL;

    map->setName(path);

    _loadedMaps[path] = map;
    return map;
}

void rune::Engine::unloadMap(QString path)
{
    if(!path.startsWith("/"))
        path = "/" + path;

    if(!_loadedMaps.contains(path))
        return;

    WorldMap* map = _loadedMaps[path];
    if(map == NULL)
        return;

    delete map;
    _loadedMaps[path] = NULL;
    return;
}

rune::WorldMap *rune::Engine::getMap(QString path)
{
    if(!path.startsWith("/"))
        path = "/" + path;

    if(!_loadedMaps.contains(path))
        return NULL;

    return _loadedMaps[path];
}

void rune::Engine::_init_entities()
{
    g_blueprintRegister = new QMap<QString, Entity*>();
    g_activeEntities = new QMap<QString, rune::Entity*>();
}

void rune::Engine::_close_entities()
{
    if(g_blueprintRegister == NULL){
        //qWarn("rune entity environment is not initialised");
        return;
    }

    // remove all registered entity blueprints
    // TODO not thread safe! (use mutex or something)
    QList<Entity*> entities = g_blueprintRegister->values();
    for(int i=0; i<entities.size(); ++i){
        delete entities[i];
    }

    delete g_blueprintRegister;
}
<commit_msg>improved game loop handling<commit_after>#include "rune/engine.h"

rune::Engine::Engine()
{
    _basePath = "./";
}

rune::Engine::~Engine()
{
    close();
}

void rune::Engine::init()
{
    _init_entities();
    _open = true;
    _glThread = NULL;
}

void rune::Engine::setBasePath(QString basePath)
{
    if(!basePath.endsWith("/"))
    {
        _basePath += "/";
    }

    _basePath = basePath;
}

QString rune::Engine::basePath()
{
    return _basePath;
}

void rune::Engine::close()
{
    if(!_open)
        return;

    _close_entities();

    if(_loadedMaps.size() > 0)
    {
        // unload all loaded maps
        for(QMap<QString, WorldMap*>::iterator it = _loadedMaps.begin();
            it != _loadedMaps.end();
            ++it)
        {
            qDebug() << "free map " << it.key() << " at " << it.value();
            delete it.value();
        }
    }

    if(_glThread != NULL)
    {
        stopGameLoop();
    }

    _open = false;
}

bool rune::Engine::loadEntity(QString path)
{
    if(path.isEmpty())
    {
        return false;
    }

    QString loadPath = path;

    if(!loadPath.endsWith(".yml"))
    {
        loadPath += ".yml";
    }else
    {
        path = path.left(path.length()-4);
    }

    if(loadPath.startsWith("/")) {
        loadPath = loadPath.right(loadPath.length()-1);
    }

    // build complete path
    loadPath = _basePath + "entity/" + loadPath;

    QFile yFile(loadPath);
    if(!yFile.exists()  || yFile.size() <= 0)
    {
        qDebug() << "error: entity definition '" << loadPath << "' does not exist" << endl;
        return false;
    }

    // create entity for blueprint base
    Entity* entity = new Entity(this);

    YAML::Node yEntity = YAML::LoadFile(loadPath.toUtf8().constData());

    // inherit from base
    if(yEntity[rune::PROP_BASE.toStdString()])
    {
        QString baseEntity = QString::fromStdString(yEntity[rune::PROP_BASE.toStdString()].as<std::string>());
        if(!g_blueprintRegister->contains(baseEntity)) // load base entity
            if(!loadEntity(baseEntity))
            {
                delete entity;
                return false;
            }
        entity->copyFrom(*getBlueprint(baseEntity));
    }

    // set entity properties from yaml
    for(YAML::iterator it=yEntity.begin(); it != yEntity.end(); ++it){
        entity->setProperty(QString::fromStdString(it->first.as<std::string>()), QString::fromStdString(it->second.as<std::string>()));
    }

    // set entity property
    entity->setProperty(rune::PROP_ENTITY, path);

    if(g_blueprintRegister->contains(path))
    {
        unloadEntity(path);
    }

    // add to blueprint register
    g_blueprintRegister->insert(path, entity);

    return true;
}

bool rune::Engine::unloadEntity(QString path)
{
    if(path.isEmpty())
    {
        return false;
    }

    if(path.endsWith(".yml"))
    {
        path = path.left(path.length()-4);
    }

    if(!g_blueprintRegister->contains(path))
    {
        return false;
    }

    // delete entity
    Entity* e = g_blueprintRegister->value(path);
    g_blueprintRegister->remove(path);

    // TODO remove existing clones?
    delete e;

    return true;
}

QString rune::Engine::cloneEntity(QString path)
{
    if(!g_blueprintRegister->contains(path))
    {
        // blueprint was not loaded yet -> try to load
        if(!loadEntity(path))
            return ""; // blueprint could not be loaded -> error
    }

    // copy entity
    Entity* clone = new Entity(this);
    clone->copyFrom(*(g_blueprintRegister->value(path)));

    /* check if clone register is already available
    if(g_activeEntities == NULL)
    {
        // create if not
        g_activeEntities = new QMap<QString, rune::Entity*>();
    }*/

    // generate entity id
    QUuid uid = QUuid::createUuid();
    clone->setProperty(rune::PROP_UID, uid.toString());

    g_activeEntities->insert(uid.toString(), clone);

    // init interpreter
    ScriptInterpreter* si = new ScriptInterpreter(clone);
    si->bind(clone);

    return clone->getProperty(PROP_UID);
}

bool rune::Engine::modifyEntityProperty(QString uid, QString prop, QString value)
{
    // TODO modification stack
    Entity* e = getClone(uid);
    if(e == NULL)
        return false; // TODO error handling
    e->setProperty(prop, value);
    return true;
}

void rune::Engine::startGameLoop()
{
    if(_glThread != NULL && _glThread->isRunning())
        return; // game loop already in progress

    if(_glThread == NULL)
        _glThread = new GameLoopThread(this);

    connect(_glThread, SIGNAL(gameLoopFinished()), this, SLOT(glFinished()));
    _glThread->start();
    return;
}

void rune::Engine::stopGameLoop()
{
    if(_glThread == NULL)
        return; // no game loop running

    if(!_glThread->isRunning())
        return;

    _glThread->invalidateEngine();
    return;
}

void rune::Engine::callAction(QString uid, QString action, uint timestamp)
{
    QMutexLocker mlock(&_actionQueueMutex); // thread safe

    rune_action_queue_item i;
    i.uid       = uid;
    i.action    = action;
    i.timestamp = timestamp;

    _actionQueue.enqueue(i);
    return;
}

void rune::Engine::glFinished()
{
    emit(gameStateChanged());
}

rune::Entity *rune::Engine::getBlueprint(QString path)
{
    if(!g_blueprintRegister->contains(path))
    {
        return NULL;
    }

    return (g_blueprintRegister->value(path));
}

QQueue<rune_action_queue_item> rune::Engine::getReadyActions()
{
    QMutexLocker mlock(&_actionQueueMutex);

    uint now = QDateTime().toTime_t();
    QQueue<rune_action_queue_item> ready;
    QQueue<rune_action_queue_item> notReady;

    while(!_actionQueue.isEmpty())
    {
        rune_action_queue_item i = _actionQueue.dequeue();
        if(i.timestamp <= now)
            ready.enqueue(i);
        else
            notReady.enqueue(i);
    }

    _actionQueue = notReady;
    return ready;
}

rune::Entity *rune::Engine::getClone(QString uid)
{
    if(!g_activeEntities->contains(uid))
        return NULL;

    return g_activeEntities->value(uid);
}

rune::WorldMap *rune::Engine::loadMap(QString path)
{
    QString absPath = path;

    if(!path.startsWith("/"))
        path = "/" + path;

    if(_loadedMaps.contains(path))
    {
        if(_loadedMaps[path] != NULL)
        {
            rune::setError(RUNE_ERR_MAP_LOADED_BEFORE);
            return NULL;
        }
    }

    absPath = path.right(path.length() - 1);
    absPath = _basePath + "map/" + absPath + ".yml";

    WorldMap* map = new WorldMap(this);
    if(!map->loadMap(absPath))
        return NULL;

    map->setName(path);

    _loadedMaps[path] = map;
    return map;
}

void rune::Engine::unloadMap(QString path)
{
    if(!path.startsWith("/"))
        path = "/" + path;

    if(!_loadedMaps.contains(path))
        return;

    WorldMap* map = _loadedMaps[path];
    if(map == NULL)
        return;

    delete map;
    _loadedMaps[path] = NULL;
    return;
}

rune::WorldMap *rune::Engine::getMap(QString path)
{
    if(!path.startsWith("/"))
        path = "/" + path;

    if(!_loadedMaps.contains(path))
        return NULL;

    return _loadedMaps[path];
}

void rune::Engine::_init_entities()
{
    g_blueprintRegister = new QMap<QString, Entity*>();
    g_activeEntities = new QMap<QString, rune::Entity*>();
}

void rune::Engine::_close_entities()
{
    if(g_blueprintRegister == NULL){
        //qWarn("rune entity environment is not initialised");
        return;
    }

    // remove all registered entity blueprints
    // TODO not thread safe! (use mutex or something)
    QList<Entity*> entities = g_blueprintRegister->values();
    for(int i=0; i<entities.size(); ++i){
        delete entities[i];
    }

    delete g_blueprintRegister;
}
<|endoftext|>


<commit_before>/*******************************************************************************
 * Copyright (c) 2015 Wojciech Migda
 * All rights reserved
 * Distributed under the terms of the GNU LGPL v3
 *******************************************************************************
 *
 * Filename: fmincg.hpp
 *
 * Description:
 *      description
 *
 * Authors:
 *          Wojciech Migda (wm)
 *
 *******************************************************************************
 * History:
 * --------
 * Date         Who  Ticket     Description
 * ----------   ---  ---------  ------------------------------------------------
 * 2015-02-04   wm              Initial version
 *
 ******************************************************************************/

/*
 * Minimize a continuous differentialble multivariate function. Starting point

 * is given by "X" (D by 1), and the function named in the string "f", must

 * return a function value and a vector of partial derivatives. The Polack-

 * Ribiere flavour of conjugate gradients is used to compute search directions,

 * and a line search using quadratic and cubic polynomial approximations and the

 * Wolfe-Powell stopping criteria is used together with the slope ratio method

 * for guessing initial step sizes. Additionally a bunch of checks are made to

 * make sure that exploration is taking place and that extrapolation will not

 * be unboundedly large. The "length" gives the length of the run: if it is

 * positive, it gives the maximum number of line searches, if negative its

 * absolute gives the maximum allowed number of function evaluations. You can

 * (optionally) give "length" a second component, which will indicate the

 * reduction in function value to be expected in the first line-search (defaults

 * to 1.0). The function returns when either its length is up, or if no further

 * progress can be made (ie, we are at a minimum, or so close that due to

 * numerical problems, we cannot get any closer). If the function terminates

 * within a few iterations, it could be an indication that the function value

 * and derivatives are not consistent (ie, there may be a bug in the

 * implementation of your "f" function). The function returns the found

 * solution "X", a vector of function values "fX" indicating the progress made

 * and "i" the number of iterations (line searches or function evaluations,

 * depending on the sign of "length") used.

 *

 * Usage: [X, fX, i] = fmincg(f, X, options, P1, P2, P3, P4, P5)

 *

 * See also: checkgrad

 *

 * Copyright (C) 2001 and 2002 by Carl Edward Rasmussen. Date 2002-02-13

 *

 *

 * (C) Copyright 1999, 2000 & 2001, Carl Edward Rasmussen

 * Permission is granted for anyone to copy, use, or modify these

 * programs and accompanying documents for purposes of research or

 * education, provided this copyright notice is retained, and note is

 * made of any changes that have been made.

 *

 * These programs and documents are distributed without any warranty,

 * express or implied. As the programs were written for research

 * purposes only, they have not been tested to the degree that would be

 * advisable in any important application. All use of these programs is

 * entirely at the user's own risk.

 *

 */

#ifndef FMINCG_HPP_
#define FMINCG_HPP_

#include <utility>
#include <valarray>
#include <cmath>
#include <algorithm>

namespace num
{

// based on:
// https://github.com/thomasjungblut/tjungblut-math-cpp/blob/master/tjungblut-math%2B%2B/source/src/Fmincg.cpp
template<typename _ValueType>
std::valarray<_ValueType>
fmincg(
    std::function<std::pair<_ValueType, std::valarray<_ValueType>> (const std::valarray<_ValueType>)> cost_gradient_fn,
    std::valarray<_ValueType> theta,
    int maxiter,
    bool verbose=false
)
{
    typedef _ValueType value_type;
    typedef std::valarray<value_type> vector;

    // number of extrapolation runs, set to a higher value for smaller ravine landscapes
    constexpr value_type EXT = 3.0;
    // a bunch of constants for line searches
    constexpr value_type RHO = 0.01;
    // RHO and SIG are the constants in the Wolfe-Powell conditions
    constexpr value_type SIG = 0.5;
    // don't reevaluate within 0.1 of the limit of the current bracket
    constexpr value_type INT = 0.1;
    // max 20 function evaluations per line search
    constexpr int MAX = 20;
    // maximum allowed slope ratio
    constexpr value_type RATIO = 100.0;

    // we start by setting up all memory that we will need in terms of vectors,
    // while calculating we will just fill this memory (overloaded << uses memcpy)

    // input will be the pointer to our current active parameter set
    vector & input(theta);
    vector X0 = vector(input);

    // search directions
    vector s = vector(input.size());
    // gradients
    vector df0 = vector(input.size());
    vector df1 = vector(input.size());
    vector df2 = vector(input.size());

    // define some integers for bookkeeping and then start
    int M = 0;
    int i = 0; // zero the run length counter
    constexpr int red = 1; // starting point
    int ls_failed = 0; // no previous line search has failed

    const std::pair<value_type, vector> cost_gradient = cost_gradient_fn(input);
    value_type f1 = cost_gradient.first;
    df1 = std::move(cost_gradient.second);

    i = i + (maxiter < 0 ? 1 : 0);
    // search direction is steepest
    s = -df1;

    value_type d1 = -(s * s).sum(); // this is the slope
    value_type z1 = red / (1.0 - d1); // initial step is red/(|s|+1)

    while (i < std::abs(maxiter)) // while not finished
    {
        i = i + (maxiter > 0 ? 1 : 0); // count iterations?!
        // make a copy of current values
        X0 = input;
        value_type f0 = f1;
        df0 = df1;

        // begin line search
        // fill our new line searched parameters
        input = input + (s * z1);
        const std::pair<value_type, vector> evaluateCost2 = cost_gradient_fn(input);
        value_type f2 = evaluateCost2.first;
        df2 = std::move(evaluateCost2.second);
        i = i + (maxiter < 0 ? 1 : 0); // count epochs
        value_type d2 = (df2 * s).sum();

        // initialize point 3 equal to point 1
        value_type f3 = f1;
        value_type d3 = d1;
        value_type z3 = -z1;
        if (maxiter > 0)
        {
            M = MAX;
        }
        else
        {
            M = std::min(MAX, - maxiter - i);
        }
        // initialize quantities
        int success = 0;
        value_type limit = -1;

        while (true)
        {
            while (((f2 > f1 + z1 * RHO * d1) | (d2 > -SIG * d1)) && (M > 0))
            {
                // tighten the bracket
                limit = z1;
                value_type z2 = 0.0;
                if (f2 > f1)
                {
                    // quadratic fit
                    z2 = z3 - (0.5 * d3 * z3 * z3) / (d3 * z3 + f2 - f3);
                }
                else
                {
                    // cubic fit
                    const value_type A = 6 * (f2 - f3) / z3 + 3 * (d2 + d3);
                    const value_type B = 3 * (f3 - f2) - z3 * (d3 + 2 * d2);
                    // numerical error possible - ok!
                    z2 = (std::sqrt(B * B - A * d2 * z3 * z3) - B) / A;
                }
                if (std::isnan(z2) || !std::isfinite(z2))
                {
                    // if we had a numerical problem then bisect
                    z2 = z3 / 2.0;
                }
                // don't accept too close to limits
                z2 = std::max(std::min(z2, INT * z3), (1 - INT) * z3);
                // update the step
                z1 = z1 + z2;
                input += (s * z2);
                std::pair<value_type, vector> evaluateCost3 = cost_gradient_fn(input);
                f2 = evaluateCost3.first;
                df2 = std::move(evaluateCost3.second);
                M = M - 1;
                i = i + (maxiter < 0 ? 1 : 0); // count epochs
                d2 = (df2 * s).sum();
                // z3 is now relative to the location of z2
                z3 = z3 - z2;
            }

            if (f2 > f1 + z1 * RHO * d1 || d2 > -SIG * d1)
            {
                break; // this is a failure
            }
            else if (d2 > SIG * d1)
            {
                success = 1;
                break; // success
            }
            else if (M == 0)
            {
                break; // failure
            }
            // make cubic extrapolation
            const value_type A = 6 * (f2 - f3) / z3 + 3 * (d2 + d3);
            const value_type B = 3 * (f3 - f2) - z3 * (d3 + 2 * d2);
            value_type z2 = -d2 * z3 * z3 / (B + std::sqrt(B * B - A * d2 * z3 * z3));
            // num prob or wrong sign?
            if (std::isnan(z2) || !std::isfinite(z2) || z2 < 0)
            {
                // if we have no upper limit
                if (limit < -0.5)
                {
                    // the extrapolate the maximum amount
                    z2 = z1 * (EXT - 1);
                }
                else
                {
                    // otherwise bisect
                    z2 = (limit - z1) / 2;
                }
            }
            else if ((limit > -0.5) && (z2 + z1 > limit))
            {
                // extraplation beyond max?
                z2 = (limit - z1) / 2; // bisect
            }
            else if ((limit < -0.5) && (z2 + z1 > z1 * EXT))
            {
                // extrapolationbeyond limit
                z2 = z1 * (EXT - 1.0); // set to extrapolation limit
            }
            else if (z2 < -z3 * INT)
            {
                z2 = -z3 * INT;
            }
            else if ((limit > -0.5) && (z2 < (limit - z1) * (1.0 - INT)))
            {
                // too close to the limit
                z2 = (limit - z1) * (1.0 - INT);
            }
            // set point 3 equal to point 2
            f3 = f2;
            d3 = d2;
            z3 = -z2;
            z1 = z1 + z2;
            // update current estimates
            input += (s * z2);
            const std::pair<value_type, vector> evaluateCost3 = cost_gradient_fn(input);
            f2 = evaluateCost3.first;
            df2 = std::move(evaluateCost3.second);
            M = M - 1;
            i = i + (maxiter < 0 ? 1 : 0); // count epochs?!
            d2 = (df2 * s).sum();
        } // end of line search

        if (success == 1) // if line search succeeded
        {
            f1 = f2;
            if (verbose)
            {
                std::cout << "Iteration " << i << " | Cost: " << f1 << std::endl;
            }
            // Polack-Ribiere direction: s =
            // (df2'*df2-df1'*df2)/(df1'*df1)*s - df2;
            const value_type df2len = (df2 * df2).sum();
            const value_type df12len = (df1* df2).sum();
            const value_type df1len = (df1 * df1).sum();
            const value_type numerator = (df2len - df12len) / df1len;
            s = (s * numerator) - df2;
            std::swap(df1, df2); // swap derivatives
            d2 = (df1 * s).sum();
            // new slope must be negative
            if (d2 > 0)
            {
                // otherwise use steepest direction
                s = -df1;
                d2 = -(s * s).sum();
            }
            // realmin in octave = 2.2251e-308
            // slope ratio but max RATIO
            const value_type thres = d1 / (d2 - std::numeric_limits<value_type>::min());
            z1 = z1 * std::min(RATIO, thres);
            d1 = d2;
            ls_failed = 0; // this line search did not fail
        }
        else
        {
            // restore data from the beginning of the iteration
            input = X0;
            f1 = f0;
            df1 = df0; // restore point from before failed line search
            // line search failed twice in a row?
            if (ls_failed == 1 || i > std::abs(maxiter))
            {
                break; // or we ran out of time, so we give up
            }
            // swap derivatives
            std::swap(df1, df2);
            // try steepest
            s = -df1;
            d1 = -(s * s).sum();
            z1 = 1.0 / (1.0 - d1);
            ls_failed = 1; // this line search failed
        }
    }

    return theta;
}

}

#endif /* FMINCG_HPP_ */
<commit_msg>missing #include <iostream><commit_after>/*******************************************************************************
 * Copyright (c) 2015 Wojciech Migda
 * All rights reserved
 * Distributed under the terms of the GNU LGPL v3
 *******************************************************************************
 *
 * Filename: fmincg.hpp
 *
 * Description:
 *      description
 *
 * Authors:
 *          Wojciech Migda (wm)
 *
 *******************************************************************************
 * History:
 * --------
 * Date         Who  Ticket     Description
 * ----------   ---  ---------  ------------------------------------------------
 * 2015-02-04   wm              Initial version
 *
 ******************************************************************************/

/*
 * Minimize a continuous differentialble multivariate function. Starting point

 * is given by "X" (D by 1), and the function named in the string "f", must

 * return a function value and a vector of partial derivatives. The Polack-

 * Ribiere flavour of conjugate gradients is used to compute search directions,

 * and a line search using quadratic and cubic polynomial approximations and the

 * Wolfe-Powell stopping criteria is used together with the slope ratio method

 * for guessing initial step sizes. Additionally a bunch of checks are made to

 * make sure that exploration is taking place and that extrapolation will not

 * be unboundedly large. The "length" gives the length of the run: if it is

 * positive, it gives the maximum number of line searches, if negative its

 * absolute gives the maximum allowed number of function evaluations. You can

 * (optionally) give "length" a second component, which will indicate the

 * reduction in function value to be expected in the first line-search (defaults

 * to 1.0). The function returns when either its length is up, or if no further

 * progress can be made (ie, we are at a minimum, or so close that due to

 * numerical problems, we cannot get any closer). If the function terminates

 * within a few iterations, it could be an indication that the function value

 * and derivatives are not consistent (ie, there may be a bug in the

 * implementation of your "f" function). The function returns the found

 * solution "X", a vector of function values "fX" indicating the progress made

 * and "i" the number of iterations (line searches or function evaluations,

 * depending on the sign of "length") used.

 *

 * Usage: [X, fX, i] = fmincg(f, X, options, P1, P2, P3, P4, P5)

 *

 * See also: checkgrad

 *

 * Copyright (C) 2001 and 2002 by Carl Edward Rasmussen. Date 2002-02-13

 *

 *

 * (C) Copyright 1999, 2000 & 2001, Carl Edward Rasmussen

 * Permission is granted for anyone to copy, use, or modify these

 * programs and accompanying documents for purposes of research or

 * education, provided this copyright notice is retained, and note is

 * made of any changes that have been made.

 *

 * These programs and documents are distributed without any warranty,

 * express or implied. As the programs were written for research

 * purposes only, they have not been tested to the degree that would be

 * advisable in any important application. All use of these programs is

 * entirely at the user's own risk.

 *

 */

#ifndef FMINCG_HPP_
#define FMINCG_HPP_

#include <utility>
#include <valarray>
#include <cmath>
#include <algorithm>
#include <iostream>

namespace num
{

// based on:
// https://github.com/thomasjungblut/tjungblut-math-cpp/blob/master/tjungblut-math%2B%2B/source/src/Fmincg.cpp
template<typename _ValueType>
std::valarray<_ValueType>
fmincg(
    std::function<std::pair<_ValueType, std::valarray<_ValueType>> (const std::valarray<_ValueType>)> cost_gradient_fn,
    std::valarray<_ValueType> theta,
    int maxiter,
    bool verbose=false
)
{
    typedef _ValueType value_type;
    typedef std::valarray<value_type> vector;

    // number of extrapolation runs, set to a higher value for smaller ravine landscapes
    constexpr value_type EXT = 3.0;
    // a bunch of constants for line searches
    constexpr value_type RHO = 0.01;
    // RHO and SIG are the constants in the Wolfe-Powell conditions
    constexpr value_type SIG = 0.5;
    // don't reevaluate within 0.1 of the limit of the current bracket
    constexpr value_type INT = 0.1;
    // max 20 function evaluations per line search
    constexpr int MAX = 20;
    // maximum allowed slope ratio
    constexpr value_type RATIO = 100.0;

    // we start by setting up all memory that we will need in terms of vectors,
    // while calculating we will just fill this memory (overloaded << uses memcpy)

    // input will be the pointer to our current active parameter set
    vector & input(theta);
    vector X0 = vector(input);

    // search directions
    vector s = vector(input.size());
    // gradients
    vector df0 = vector(input.size());
    vector df1 = vector(input.size());
    vector df2 = vector(input.size());

    // define some integers for bookkeeping and then start
    int M = 0;
    int i = 0; // zero the run length counter
    constexpr int red = 1; // starting point
    int ls_failed = 0; // no previous line search has failed

    const std::pair<value_type, vector> cost_gradient = cost_gradient_fn(input);
    value_type f1 = cost_gradient.first;
    df1 = std::move(cost_gradient.second);

    i = i + (maxiter < 0 ? 1 : 0);
    // search direction is steepest
    s = -df1;

    value_type d1 = -(s * s).sum(); // this is the slope
    value_type z1 = red / (1.0 - d1); // initial step is red/(|s|+1)

    while (i < std::abs(maxiter)) // while not finished
    {
        i = i + (maxiter > 0 ? 1 : 0); // count iterations?!
        // make a copy of current values
        X0 = input;
        value_type f0 = f1;
        df0 = df1;

        // begin line search
        // fill our new line searched parameters
        input = input + (s * z1);
        const std::pair<value_type, vector> evaluateCost2 = cost_gradient_fn(input);
        value_type f2 = evaluateCost2.first;
        df2 = std::move(evaluateCost2.second);
        i = i + (maxiter < 0 ? 1 : 0); // count epochs
        value_type d2 = (df2 * s).sum();

        // initialize point 3 equal to point 1
        value_type f3 = f1;
        value_type d3 = d1;
        value_type z3 = -z1;
        if (maxiter > 0)
        {
            M = MAX;
        }
        else
        {
            M = std::min(MAX, - maxiter - i);
        }
        // initialize quantities
        int success = 0;
        value_type limit = -1;

        while (true)
        {
            while (((f2 > f1 + z1 * RHO * d1) | (d2 > -SIG * d1)) && (M > 0))
            {
                // tighten the bracket
                limit = z1;
                value_type z2 = 0.0;
                if (f2 > f1)
                {
                    // quadratic fit
                    z2 = z3 - (0.5 * d3 * z3 * z3) / (d3 * z3 + f2 - f3);
                }
                else
                {
                    // cubic fit
                    const value_type A = 6 * (f2 - f3) / z3 + 3 * (d2 + d3);
                    const value_type B = 3 * (f3 - f2) - z3 * (d3 + 2 * d2);
                    // numerical error possible - ok!
                    z2 = (std::sqrt(B * B - A * d2 * z3 * z3) - B) / A;
                }
                if (std::isnan(z2) || !std::isfinite(z2))
                {
                    // if we had a numerical problem then bisect
                    z2 = z3 / 2.0;
                }
                // don't accept too close to limits
                z2 = std::max(std::min(z2, INT * z3), (1 - INT) * z3);
                // update the step
                z1 = z1 + z2;
                input += (s * z2);
                std::pair<value_type, vector> evaluateCost3 = cost_gradient_fn(input);
                f2 = evaluateCost3.first;
                df2 = std::move(evaluateCost3.second);
                M = M - 1;
                i = i + (maxiter < 0 ? 1 : 0); // count epochs
                d2 = (df2 * s).sum();
                // z3 is now relative to the location of z2
                z3 = z3 - z2;
            }

            if (f2 > f1 + z1 * RHO * d1 || d2 > -SIG * d1)
            {
                break; // this is a failure
            }
            else if (d2 > SIG * d1)
            {
                success = 1;
                break; // success
            }
            else if (M == 0)
            {
                break; // failure
            }
            // make cubic extrapolation
            const value_type A = 6 * (f2 - f3) / z3 + 3 * (d2 + d3);
            const value_type B = 3 * (f3 - f2) - z3 * (d3 + 2 * d2);
            value_type z2 = -d2 * z3 * z3 / (B + std::sqrt(B * B - A * d2 * z3 * z3));
            // num prob or wrong sign?
            if (std::isnan(z2) || !std::isfinite(z2) || z2 < 0)
            {
                // if we have no upper limit
                if (limit < -0.5)
                {
                    // the extrapolate the maximum amount
                    z2 = z1 * (EXT - 1);
                }
                else
                {
                    // otherwise bisect
                    z2 = (limit - z1) / 2;
                }
            }
            else if ((limit > -0.5) && (z2 + z1 > limit))
            {
                // extraplation beyond max?
                z2 = (limit - z1) / 2; // bisect
            }
            else if ((limit < -0.5) && (z2 + z1 > z1 * EXT))
            {
                // extrapolationbeyond limit
                z2 = z1 * (EXT - 1.0); // set to extrapolation limit
            }
            else if (z2 < -z3 * INT)
            {
                z2 = -z3 * INT;
            }
            else if ((limit > -0.5) && (z2 < (limit - z1) * (1.0 - INT)))
            {
                // too close to the limit
                z2 = (limit - z1) * (1.0 - INT);
            }
            // set point 3 equal to point 2
            f3 = f2;
            d3 = d2;
            z3 = -z2;
            z1 = z1 + z2;
            // update current estimates
            input += (s * z2);
            const std::pair<value_type, vector> evaluateCost3 = cost_gradient_fn(input);
            f2 = evaluateCost3.first;
            df2 = std::move(evaluateCost3.second);
            M = M - 1;
            i = i + (maxiter < 0 ? 1 : 0); // count epochs?!
            d2 = (df2 * s).sum();
        } // end of line search

        if (success == 1) // if line search succeeded
        {
            f1 = f2;
            if (verbose)
            {
                std::cout << "Iteration " << i << " | Cost: " << f1 << std::endl;
            }
            // Polack-Ribiere direction: s =
            // (df2'*df2-df1'*df2)/(df1'*df1)*s - df2;
            const value_type df2len = (df2 * df2).sum();
            const value_type df12len = (df1* df2).sum();
            const value_type df1len = (df1 * df1).sum();
            const value_type numerator = (df2len - df12len) / df1len;
            s = (s * numerator) - df2;
            std::swap(df1, df2); // swap derivatives
            d2 = (df1 * s).sum();
            // new slope must be negative
            if (d2 > 0)
            {
                // otherwise use steepest direction
                s = -df1;
                d2 = -(s * s).sum();
            }
            // realmin in octave = 2.2251e-308
            // slope ratio but max RATIO
            const value_type thres = d1 / (d2 - std::numeric_limits<value_type>::min());
            z1 = z1 * std::min(RATIO, thres);
            d1 = d2;
            ls_failed = 0; // this line search did not fail
        }
        else
        {
            // restore data from the beginning of the iteration
            input = X0;
            f1 = f0;
            df1 = df0; // restore point from before failed line search
            // line search failed twice in a row?
            if (ls_failed == 1 || i > std::abs(maxiter))
            {
                break; // or we ran out of time, so we give up
            }
            // swap derivatives
            std::swap(df1, df2);
            // try steepest
            s = -df1;
            d1 = -(s * s).sum();
            z1 = 1.0 / (1.0 - d1);
            ls_failed = 1; // this line search failed
        }
    }

    return theta;
}

}

#endif /* FMINCG_HPP_ */
<|endoftext|>


<commit_before>/******************************************************************************
* Copyright (c) 2014, Pete Gadomski (pete.gadomski@gmail.com)
*
* 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 Hobu, Inc. or Flaxen Geo Consulting 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 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; 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 <pdal/pdal_test_main.hpp>

#include <pdal/Writer.hpp>
#include <pdal/StageFactory.hpp>
#include <pdal/util/Algorithm.hpp>

#include "Support.hpp"
#include "Pgtest-Support.hpp"
#include "../io/PgCommon.hpp"

using namespace pdal;

namespace { // anonymous

std::string getTestConnBase()
{
    std::string s;
    if ( ! testDbPort.empty() )
        s += " port='" + testDbPort + "'";
    if ( ! testDbHost.empty() )
        s += " host='" + testDbHost + "'";
    if ( ! testDbUser.empty() )
        s += " user='" + testDbUser + "'";
    return s;
}

std::string getConnectionString(const std::string& dbname)
{
    return getTestConnBase() + " dbname='" + dbname + "'";
}

std::string getTestDBTempConn()
{
    return getConnectionString(testDbTempname);
}

std::string getMasterDBConn()
{
    return getConnectionString(testDbName);
}

} // anonymous namespace

Options getDbOptions()
{
    Options options;

    options.add(Option("connection", getTestDBTempConn()));
    options.add(Option("table", "pdal-\"test\"-table")); // intentional quotes
    options.add(Option("column", "p\"a")); // intentional quotes
    options.add(Option("srid", "4326"));
    options.add(Option("capacity", "10000"));

    return options;
}

class PgpointcloudWriterTest : public testing::Test
{
public:
    PgpointcloudWriterTest() : m_masterConnection(0), m_testConnection(0),
                               m_bSkipTests(false) {};
protected:
    virtual void SetUp()
    {
        std::string connstr = getMasterDBConn();
        m_masterConnection = pg_connect( connstr );
        m_testConnection = NULL;

        // Silence those pesky notices
        executeOnMasterDb("SET client_min_messages TO WARNING");

        dropTestDb();

        std::stringstream createDbSql;
        createDbSql << "CREATE DATABASE " <<
            testDbTempname << " TEMPLATE template0";
        try
        {
            executeOnMasterDb(createDbSql.str());
        }
        catch( const pdal_error& error )
        {
            m_bSkipTests = true;
            return;
        }

        m_testConnection = pg_connect( getTestDBTempConn() );

        try
        {
            executeOnTestDb("CREATE EXTENSION pointcloud");
        }
        catch( const pdal_error& error )
        {
            m_bSkipTests = true;
            return;
        }
    }

    void executeOnTestDb(const std::string& sql)
    {
        pg_execute(m_testConnection, sql);
    }

    virtual void TearDown()
    {
        if (!m_testConnection || !m_masterConnection) return;
        if (m_testConnection)
        {
            PQfinish(m_testConnection);
        }
        dropTestDb();
        if (m_masterConnection)
        {
            PQfinish(m_masterConnection);
        }
    }

    bool shouldSkipTests() const { return m_bSkipTests; }

private:

    void executeOnMasterDb(const std::string& sql)
    {
        pg_execute(m_masterConnection, sql);
    }

    void execute(PGconn* connection, const std::string& sql)
    {
        pg_execute(connection, sql);
    }

    void dropTestDb()
    {
        std::stringstream dropDbSql;
        dropDbSql << "DROP DATABASE IF EXISTS " << testDbTempname;
        executeOnMasterDb(dropDbSql.str());
    }

    PGconn* m_masterConnection;
    PGconn* m_testConnection;
    bool m_bSkipTests;
};

namespace
{

void optionsWrite(const Options& writerOps)
{
    StageFactory f;
    std::unique_ptr<Stage> writer(f.createStage("writers.pgpointcloud"));
    std::unique_ptr<Stage> reader(f.createStage("readers.las"));

    EXPECT_TRUE(writer.get());
    EXPECT_TRUE(reader.get());
    if (!writer.get() || !reader.get())
        return;

    const std::string file(Support::datapath("las/1.2-with-color.las"));
    Options options;
    options.add("filename", file);
    reader->setOptions(options);
    writer->setOptions(writerOps);
    writer->setInput(*reader);

    PointTable table;
    writer->prepare(table);

    PointViewSet written = writer->execute(table);

    point_count_t count(0);
    for(auto i = written.begin(); i != written.end(); ++i)
	    count += (*i)->size();
    EXPECT_EQ(written.size(), 1U);
    EXPECT_EQ(count, 1065U);
}

} // unnamed namespace

TEST_F(PgpointcloudWriterTest, write)
{
    if (shouldSkipTests())
    {
        return;
    }

    optionsWrite(getDbOptions());
}

TEST_F(PgpointcloudWriterTest, writeScaled)
{
    if (shouldSkipTests())
    {
        return;
    }

    Options ops = getDbOptions();
    ops.add("scale_x", .01);
    ops.add("scale_y", .01);
    ops.add("scale_z", .01);

    optionsWrite(ops);
}

TEST_F(PgpointcloudWriterTest, writeXYZ)
{
    if (shouldSkipTests())
    {
        return;
    }

    Options ops = getDbOptions();
    ops.add("output_dims", "X,Y,Z");

    optionsWrite(ops);

    PointTable table;
    std::unique_ptr<Stage> reader(
        StageFactory().createStage("readers.pgpointcloud"));
    reader->setOptions(getDbOptions());

    reader->prepare(table);
    Dimension::IdList dims = table.layout()->dims();
    EXPECT_EQ(dims.size(), (size_t)3);
    EXPECT_TRUE(Utils::contains(dims, Dimension::Id::X));
    EXPECT_TRUE(Utils::contains(dims, Dimension::Id::Y));
    EXPECT_TRUE(Utils::contains(dims, Dimension::Id::Z));
}

TEST_F(PgpointcloudWriterTest, writetNoPointcloudExtension)
{
    if (shouldSkipTests())
    {
        return;
    }

    StageFactory f;
    std::unique_ptr<Stage> writer(f.createStage("writers.pgpointcloud"));
    EXPECT_TRUE(writer.get());

    executeOnTestDb("DROP EXTENSION pointcloud");

    const std::string file(Support::datapath("las/1.2-with-color.las"));

    const Option opt_filename("filename", file);

    std::unique_ptr<Stage> reader(f.createStage("readers.las"));
    EXPECT_TRUE(reader.get());
    Options options;
    options.add(opt_filename);
    reader->setOptions(options);
    writer->setOptions(getDbOptions());
    writer->setInput(*reader);

    PointTable table;
    writer->prepare(table);

    EXPECT_THROW(writer->execute(table), pdal_error);
}
<commit_msg>make test table name start with a number to simulate #1098<commit_after>/******************************************************************************
* Copyright (c) 2014, Pete Gadomski (pete.gadomski@gmail.com)
*
* 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 Hobu, Inc. or Flaxen Geo Consulting 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 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; 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 <pdal/pdal_test_main.hpp>

#include <pdal/Writer.hpp>
#include <pdal/StageFactory.hpp>
#include <pdal/util/Algorithm.hpp>

#include "Support.hpp"
#include "Pgtest-Support.hpp"
#include "../io/PgCommon.hpp"

using namespace pdal;

namespace { // anonymous

std::string getTestConnBase()
{
    std::string s;
    if ( ! testDbPort.empty() )
        s += " port='" + testDbPort + "'";
    if ( ! testDbHost.empty() )
        s += " host='" + testDbHost + "'";
    if ( ! testDbUser.empty() )
        s += " user='" + testDbUser + "'";
    return s;
}

std::string getConnectionString(const std::string& dbname)
{
    return getTestConnBase() + " dbname='" + dbname + "'";
}

std::string getTestDBTempConn()
{
    return getConnectionString(testDbTempname);
}

std::string getMasterDBConn()
{
    return getConnectionString(testDbName);
}

} // anonymous namespace

Options getDbOptions()
{
    Options options;

    options.add(Option("connection", getTestDBTempConn()));
    options.add(Option("table", "4dal-\"test\"-table")); // intentional quotes
    options.add(Option("column", "p\"a")); // intentional quotes
    options.add(Option("srid", "4326"));
    options.add(Option("capacity", "10000"));

    return options;
}

class PgpointcloudWriterTest : public testing::Test
{
public:
    PgpointcloudWriterTest() : m_masterConnection(0), m_testConnection(0),
                               m_bSkipTests(false) {};
protected:
    virtual void SetUp()
    {
        std::string connstr = getMasterDBConn();
        m_masterConnection = pg_connect( connstr );
        m_testConnection = NULL;

        // Silence those pesky notices
        executeOnMasterDb("SET client_min_messages TO WARNING");

        dropTestDb();

        std::stringstream createDbSql;
        createDbSql << "CREATE DATABASE " <<
            testDbTempname << " TEMPLATE template0";
        try
        {
            executeOnMasterDb(createDbSql.str());
        }
        catch( const pdal_error& error )
        {
            m_bSkipTests = true;
            return;
        }

        m_testConnection = pg_connect( getTestDBTempConn() );

        try
        {
            executeOnTestDb("CREATE EXTENSION pointcloud");
        }
        catch( const pdal_error& error )
        {
            m_bSkipTests = true;
            return;
        }
    }

    void executeOnTestDb(const std::string& sql)
    {
        pg_execute(m_testConnection, sql);
    }

    virtual void TearDown()
    {
        if (!m_testConnection || !m_masterConnection) return;
        if (m_testConnection)
        {
            PQfinish(m_testConnection);
        }
        dropTestDb();
        if (m_masterConnection)
        {
            PQfinish(m_masterConnection);
        }
    }

    bool shouldSkipTests() const { return m_bSkipTests; }

private:

    void executeOnMasterDb(const std::string& sql)
    {
        pg_execute(m_masterConnection, sql);
    }

    void execute(PGconn* connection, const std::string& sql)
    {
        pg_execute(connection, sql);
    }

    void dropTestDb()
    {
        std::stringstream dropDbSql;
        dropDbSql << "DROP DATABASE IF EXISTS " << testDbTempname;
        executeOnMasterDb(dropDbSql.str());
    }

    PGconn* m_masterConnection;
    PGconn* m_testConnection;
    bool m_bSkipTests;
};

namespace
{

void optionsWrite(const Options& writerOps)
{
    StageFactory f;
    std::unique_ptr<Stage> writer(f.createStage("writers.pgpointcloud"));
    std::unique_ptr<Stage> reader(f.createStage("readers.las"));

    EXPECT_TRUE(writer.get());
    EXPECT_TRUE(reader.get());
    if (!writer.get() || !reader.get())
        return;

    const std::string file(Support::datapath("las/1.2-with-color.las"));
    Options options;
    options.add("filename", file);
    reader->setOptions(options);
    writer->setOptions(writerOps);
    writer->setInput(*reader);

    PointTable table;
    writer->prepare(table);

    PointViewSet written = writer->execute(table);

    point_count_t count(0);
    for(auto i = written.begin(); i != written.end(); ++i)
	    count += (*i)->size();
    EXPECT_EQ(written.size(), 1U);
    EXPECT_EQ(count, 1065U);
}

} // unnamed namespace

TEST_F(PgpointcloudWriterTest, write)
{
    if (shouldSkipTests())
    {
        return;
    }

    optionsWrite(getDbOptions());
}

TEST_F(PgpointcloudWriterTest, writeScaled)
{
    if (shouldSkipTests())
    {
        return;
    }

    Options ops = getDbOptions();
    ops.add("scale_x", .01);
    ops.add("scale_y", .01);
    ops.add("scale_z", .01);

    optionsWrite(ops);
}

TEST_F(PgpointcloudWriterTest, writeXYZ)
{
    if (shouldSkipTests())
    {
        return;
    }

    Options ops = getDbOptions();
    ops.add("output_dims", "X,Y,Z");

    optionsWrite(ops);

    PointTable table;
    std::unique_ptr<Stage> reader(
        StageFactory().createStage("readers.pgpointcloud"));
    reader->setOptions(getDbOptions());

    reader->prepare(table);
    Dimension::IdList dims = table.layout()->dims();
    EXPECT_EQ(dims.size(), (size_t)3);
    EXPECT_TRUE(Utils::contains(dims, Dimension::Id::X));
    EXPECT_TRUE(Utils::contains(dims, Dimension::Id::Y));
    EXPECT_TRUE(Utils::contains(dims, Dimension::Id::Z));
}

TEST_F(PgpointcloudWriterTest, writetNoPointcloudExtension)
{
    if (shouldSkipTests())
    {
        return;
    }

    StageFactory f;
    std::unique_ptr<Stage> writer(f.createStage("writers.pgpointcloud"));
    EXPECT_TRUE(writer.get());

    executeOnTestDb("DROP EXTENSION pointcloud");

    const std::string file(Support::datapath("las/1.2-with-color.las"));

    const Option opt_filename("filename", file);

    std::unique_ptr<Stage> reader(f.createStage("readers.las"));
    EXPECT_TRUE(reader.get());
    Options options;
    options.add(opt_filename);
    reader->setOptions(options);
    writer->setOptions(getDbOptions());
    writer->setInput(*reader);

    PointTable table;
    writer->prepare(table);

    EXPECT_THROW(writer->execute(table), pdal_error);
}
<|endoftext|>


<commit_before>#ifndef CONTROLLER_HPP
#define CONTROLLER_HPP
#include "Position6DOF.hpp"
#include "MovementQuadruple.hpp"
#include "Formation.hpp"
#include "ros/ros.h"
//Ros messages/services
#include "api_application/MoveFormation.h"	// ? (D)
#include "../matlab/Vector.h"
#include "../matlab/TrackingArea.h"
#include "control_application/quadcopter_movement.h"		// ? (D)
#include "api_application/SetFormation.h"
#include "api_application/Message.h"
#include "api_application/Announce.h"
#include "api_application/System.h"
#include "quadcopter_application/find_all.h"
#include "quadcopter_application/blink.h"
#include "quadcopter_application/quadcopter_status.h"
#include "control_application/BuildFormation.h"
#include "control_application/Shutdown.h"
//#include "control_application/SetQuadcopters.h"
#include "../position/IPositionReceiver.hpp"
#include <time.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <string>
#include <vector>
#include <pthread.h>
#include <list>
#include "Mutex.hpp"
#include <cmath>
#include <time.h>
#include <sstream>
#include <boost/bind.hpp>

#define THRUST_MIN 10001
#define THRUST_STAND_STILL 18001
#define THRUST_START 22000
#define THRUST_DECLINE 20000
#define THRUST_STEP 50
#define ROLL_STEP 2
#define PITCH_STEP 2
#define INVALID -1
//TODO 100% = 1?
#define LOW_BATTERY 0.05
//In seconds
#define TIME_UPDATED 1

/* For calculateMovement */
#define CALCULATE_NONE 0 // Unset
#define CALCULATE_START 1
#define CALCULATE_STABILIZE 2
#define CALCULATE_HOLD 3
#define CALCULATE_MOVE 4
#define CALCULATE_ACTIVATED 6 // QC ready to receive data from quadcoptermodul

/* Used for lists */
#define MAX_NUMBER_QUADCOPTER 10

#define POS_CHECK (current[0] != target[0]) || (current[1] != target[1]) || (current[2] != target[2])

class Controller : public IPositionReceiver {
//class Controller {
public:
	Controller();

	/* Initializing */
	void initialize();

	/* Movement and Positioning */
	void convertMovement(double* const vector);
	Position6DOF* getTargetPosition();
	void setTargetPosition();
	void updatePositions(std::vector<Vector> positions, std::vector<int> ids, std::vector<int> updates);
	void sendMovement();
	void sendMovementAll();
	void calculateMovement();
	void reachTrackedArea(std::vector<int> ids);

	/* Formation also services*/
	bool buildFormation(control_application::BuildFormation::Request  &req, control_application::BuildFormation::Response &res);
	void shutdownFormation();

	/* Service to set Quadcopter IDs*/
	bool setQuadcopters(control_application::SetQuadcopters::Request  &req, control_application::SetQuadcopters::Response &res);

	bool shutdown(control_application::Shutdown::Request &req, control_application::Shutdown::Response &res);

	bool checkInput();
	void emergencyRoutine(std::string message);

	void moveUp(std::vector<int> ids);
	void moveUpNoArg();

protected:
	//Callbacks for Ros subscriber
	void MoveFormationCallback(const api_application::MoveFormation::ConstPtr& msg);
	void SetFormationCallback(const api_application::SetFormation::ConstPtr& msg);
	void QuadStatusCallback(const quadcopter_application::quadcopter_status::ConstPtr& msg, int topicNr);
	void SystemCallback(const api_application::System::ConstPtr& msg);

	void stopReachTrackedArea();

private:
	/*  */

	/* Position */
	//std::vector<Position6DOF> targetPosition;
	//std::vector<Position6DOF> currentPosition;
	//std::vector<rpy-values> sentMovement;
	//TODO Where are those lists filled/ updated? FIFO? Latest element last?
	//TODO Include time somehow or how do we check if there hasn't been any input?
	std::list<std::vector<Position6DOF> > listPositions;
	std::list<std::vector<Position6DOF> > listTargets;
	std::list<std::vector<Position6DOF> > listSendTargets;

	/* Identification of Quadcopters */
	//Receive data over ROS
	Formation formation;
	//TODO needs to be with service find all
	int totalAmount;
	int amount;
	std::list<float[3]> formationMovement;
	time_t lastFormationMovement;
	time_t lastCurrent;
	unsigned int senderID;
	//TODO Set area
	TrackingArea trackingArea;

	//Mapping of int id to string id/ hardware id   qc[id][uri/hardware id]
	//std::vector<std::string> quadcopters;
	//Mapping of quadcopter global id
	std::vector<unsigned int> quadcopters;

	/* Set data */
	int thrust;
	float pitch, roll, yawrate;
	std::vector<MovementQuadruple> movementAll;

	/* Received data */
	int id;
	std::vector<float> pitch_stab;
	std::vector<float> roll_stab;
	std::vector<float> yaw_stab;
	std::vector<unsigned int> thrust_stab;
	std::vector<float> battery_status;
	int startProcess;
	std::vector<std::string> idString;
	std::vector<int> idsToGetTracked;

	//Control variables
	//Array of tracked quadcopters
	std::vector<bool> tracked;
	//Set when we are in the shutdown process
	bool shutdownStarted;
	bool getTracked;
	bool receiveQuadcopters;

	/* Mutex */
	Mutex curPosMutex;
	Mutex tarPosMutex;
	Mutex shutdownMutex;
	Mutex formMovMutex;
	Mutex listPositionsMutex;
	Mutex getTrackedMutex;

	/* Threads */
	pthread_t tCalc;
	pthread_t tSend;
	pthread_t tGetTracked;

	/**
  	 * NodeHandle is the main access point to communications with the ROS system.
	 * The first NodeHandle constructed will fully initialize this node, and the last
     * NodeHandle destructed will close down the node.
     */
    ros::NodeHandle n;

	//Subscriber
	//Subscriber for the MoveFormation data
	ros::Subscriber MoveFormation_sub;
	//Subscriber for Formation data from API
	ros::Subscriber SetFormation_sub;
	//Subscriber for Quadcopter data from QuadcopterModul
	//ros::Subscriber QuadStatus_sub;
	std::vector<ros::Subscriber> QuadStatus_sub;
	//Subscriber to System topic (Sends the start and end of the system)
	ros::Subscriber System_sub;

	/* Publisher */
	//Publisher for the Movement data of the Quadcopts (1000 is the max. buffered messages)
	//ros::Publisher Movement_pub;
	std::vector<ros::Publisher> Movement_pub;
	//Publisher for Message to API
	ros::Publisher Message_pub;

	/* Services */
	//Service for building formation
	ros::ServiceServer BuildForm_srv;
	//Service for shutingdown formation
	ros::ServiceServer Shutdown_srv;
	//Service for setting the quadcopter ids
	ros::ServiceServer QuadID_srv;

	//Clients
	ros::ServiceClient FindAll_client;
	ros::ServiceClient Blink_client;
	ros::ServiceClient Announce_client;

};

#endif // CONTROLLER_HPP
<commit_msg>Adding variable for calculateMovement-switch<commit_after>#ifndef CONTROLLER_HPP
#define CONTROLLER_HPP
#include "Position6DOF.hpp"
#include "MovementQuadruple.hpp"
#include "Formation.hpp"
#include "ros/ros.h"
//Ros messages/services
#include "api_application/MoveFormation.h"	// ? (D)
#include "../matlab/Vector.h"
#include "../matlab/TrackingArea.h"
#include "control_application/quadcopter_movement.h"		// ? (D)
#include "api_application/SetFormation.h"
#include "api_application/Message.h"
#include "api_application/Announce.h"
#include "api_application/System.h"
#include "quadcopter_application/find_all.h"
#include "quadcopter_application/blink.h"
#include "quadcopter_application/quadcopter_status.h"
#include "control_application/BuildFormation.h"
#include "control_application/Shutdown.h"
//#include "control_application/SetQuadcopters.h"
#include "../position/IPositionReceiver.hpp"
#include <time.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <string>
#include <vector>
#include <pthread.h>
#include <list>
#include "Mutex.hpp"
#include <cmath>
#include <time.h>
#include <sstream>
#include <boost/bind.hpp>

#define THRUST_MIN 10001
#define THRUST_STAND_STILL 18001
#define THRUST_START 22000
#define THRUST_DECLINE 20000
#define THRUST_STEP 50
#define ROLL_STEP 2
#define PITCH_STEP 2
#define INVALID -1
//TODO 100% = 1?
#define LOW_BATTERY 0.05
//In seconds
#define TIME_UPDATED 1

/* For calculateMovement */
#define CALCULATE_NONE 0 // Unset
#define CALCULATE_START 1
#define CALCULATE_STABILIZE 2
#define CALCULATE_HOLD 3
#define CALCULATE_MOVE 4
#define CALCULATE_ACTIVATED 6 // QC ready to receive data from quadcoptermodul

/* Used for lists */
#define MAX_NUMBER_QUADCOPTER 10

#define POS_CHECK (current[0] != target[0]) || (current[1] != target[1]) || (current[2] != target[2])

class Controller : public IPositionReceiver {
//class Controller {
public:
	Controller();

	/* Initializing */
	void initialize();

	/* Movement and Positioning */
	void convertMovement(double* const vector);
	Position6DOF* getTargetPosition();
	void setTargetPosition();
	void updatePositions(std::vector<Vector> positions, std::vector<int> ids, std::vector<int> updates);
	void sendMovement();
	void sendMovementAll();
	void calculateMovement();
	void reachTrackedArea(std::vector<int> ids);

	/* Formation also services*/
	bool buildFormation(control_application::BuildFormation::Request  &req, control_application::BuildFormation::Response &res);
	void shutdownFormation();

	/* Service to set Quadcopter IDs*/
	bool setQuadcopters(control_application::SetQuadcopters::Request  &req, control_application::SetQuadcopters::Response &res);

	bool shutdown(control_application::Shutdown::Request &req, control_application::Shutdown::Response &res);

	bool checkInput();
	void emergencyRoutine(std::string message);

	void moveUp(std::vector<int> ids);
	void moveUpNoArg();

protected:
	//Callbacks for Ros subscriber
	void MoveFormationCallback(const api_application::MoveFormation::ConstPtr& msg);
	void SetFormationCallback(const api_application::SetFormation::ConstPtr& msg);
	void QuadStatusCallback(const quadcopter_application::quadcopter_status::ConstPtr& msg, int topicNr);
	void SystemCallback(const api_application::System::ConstPtr& msg);

	void stopReachTrackedArea();

private:
	/*  */

	/* Position */
	//std::vector<Position6DOF> targetPosition;
	//std::vector<Position6DOF> currentPosition;
	//std::vector<rpy-values> sentMovement;
	//TODO Where are those lists filled/ updated? FIFO? Latest element last?
	//TODO Include time somehow or how do we check if there hasn't been any input?
	std::list<std::vector<Position6DOF> > listPositions;
	std::list<std::vector<Position6DOF> > listTargets;
	std::list<std::vector<Position6DOF> > listSendTargets;

	/* Identification of Quadcopters */
	//Receive data over ROS
	Formation formation;
	//TODO needs to be with service find all
	int totalAmount;
	int amount;
	std::list<float[3]> formationMovement;
	time_t lastFormationMovement;
	time_t lastCurrent;
	unsigned int senderID;
	//TODO Set area
	TrackingArea trackingArea;

	//Mapping of int id to string id/ hardware id   qc[id][uri/hardware id]
	//std::vector<std::string> quadcopters;
	//Mapping of quadcopter global id
	std::vector<unsigned int> quadcopters;
	/* For calculateMovement, using local id from mapping before. */
	std::vector<unsigned int> quadcopterMovementStatus;

	/* Set data */
	int thrust;
	float pitch, roll, yawrate;
	std::vector<MovementQuadruple> movementAll;

	/* Received data */
	int id;
	std::vector<float> pitch_stab;
	std::vector<float> roll_stab;
	std::vector<float> yaw_stab;
	std::vector<unsigned int> thrust_stab;
	std::vector<float> battery_status;
	int startProcess;
	std::vector<std::string> idString;
	std::vector<int> idsToGetTracked;

	//Control variables
	//Array of tracked quadcopters
	std::vector<bool> tracked;
	//Set when we are in the shutdown process
	bool shutdownStarted;
	bool getTracked;
	bool receiveQuadcopters;

	/* Mutex */
	Mutex curPosMutex;
	Mutex tarPosMutex;
	Mutex shutdownMutex;
	Mutex formMovMutex;
	Mutex listPositionsMutex;
	Mutex getTrackedMutex;

	/* Threads */
	pthread_t tCalc;
	pthread_t tSend;
	pthread_t tGetTracked;

	/**
  	 * NodeHandle is the main access point to communications with the ROS system.
	 * The first NodeHandle constructed will fully initialize this node, and the last
     * NodeHandle destructed will close down the node.
     */
    ros::NodeHandle n;

	//Subscriber
	//Subscriber for the MoveFormation data
	ros::Subscriber MoveFormation_sub;
	//Subscriber for Formation data from API
	ros::Subscriber SetFormation_sub;
	//Subscriber for Quadcopter data from QuadcopterModul
	//ros::Subscriber QuadStatus_sub;
	std::vector<ros::Subscriber> QuadStatus_sub;
	//Subscriber to System topic (Sends the start and end of the system)
	ros::Subscriber System_sub;

	/* Publisher */
	//Publisher for the Movement data of the Quadcopts (1000 is the max. buffered messages)
	//ros::Publisher Movement_pub;
	std::vector<ros::Publisher> Movement_pub;
	//Publisher for Message to API
	ros::Publisher Message_pub;

	/* Services */
	//Service for building formation
	ros::ServiceServer BuildForm_srv;
	//Service for shutingdown formation
	ros::ServiceServer Shutdown_srv;
	//Service for setting the quadcopter ids
	ros::ServiceServer QuadID_srv;

	//Clients
	ros::ServiceClient FindAll_client;
	ros::ServiceClient Blink_client;
	ros::ServiceClient Announce_client;

};

#endif // CONTROLLER_HPP
<|endoftext|>