<commit_before>#include "../headers.h"

unsigned char KeyboardController::ascii_table_[0x80] = {
	0,   0,   '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '-', '^', 0/*0x08*/, 0,
	'q', 'w', 'e', 'r', 't', 'y', 'u', 'i', 'o', 'p', '@', '[', 0/*0x0a*/, 0, 'a', 's',
	'd', 'f', 'g', 'h', 'j', 'k', 'l', ';', ':', 0,   0,   ']', 'z', 'x', 'c', 'v',
	'b', 'n', 'm', ',', '.', '/', 0,   '*', 0,   ' ', 0,   0,   0,   0,   0,   0,
	0,   0,   0,   0,   0,   0,   0,   '7', '8', '9', '-', '4', '5', '6', '+', '1',
	'2', '3', '0', '.', 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	0,   0,   0,   0x5c, 0,  0,   0,   0,   0,   0,   0,   0,   0,   0x5c, 0,  0
};
unsigned char KeyboardController::ascii_shift_table_[0x80] = {
	0,   0,   '!', 0x22, '#', '$', '%', '&', 0x27, '(', ')', '~', '=', '~', 0/*0x08*/, 0,
	'Q', 'W', 'E', 'R', 'T', 'Y', 'U', 'I', 'O', 'P', '`', '{', 0/*0x0a*/, 0, 'A', 'S',
	'D', 'F', 'G', 'H', 'J', 'K', 'L', '+', '*', 0,   0,   '}', 'Z', 'X', 'C', 'V',
	'B', 'N', 'M', '<', '>', '?', 0,   '*', 0,   ' ', 0,   0,   0,   0,   0,   0,
	0,   0,   0,   0,   0,   0,   0,   '7', '8', '9', '-', '4', '5', '6', '+', '1',
	'2', '3', '0', '.', 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	0,   0,   0,   '_', 0,   0,   0,   0,   0,   0,   0,   0,   0,   '|', 0,   0
};
int KeyboardController::shift_ = 0;
int KeyboardController::cmd_wait_ = -1;
int KeyboardController::leds_ = 0;
Queue *KeyboardController::cmd_ = nullptr;
Queue *KeyboardController::queue_ = nullptr;

void KeyboardController::init() {
	BootInfo *binfo = (BootInfo *)ADDRESS_BOOTINFO;
	leds_ = (binfo->leds >> 4) & 7;

	wait();
	Output8(kPortKeyCmd, kKeyCmdWriteMode);
	wait();
	Output8(kPortKeyData, kKBCMode);

	Task *task = new Task((char *)kKeyboardTaskName, 2, 1, []() {
		Task *task = TaskController::getNowTask();

		// キャレットの表示とタイマー設定
		SheetCtl::drawLine(SheetCtl::back_, SheetCtl::tbox_col_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
		SheetCtl::refresh(SheetCtl::back_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 3, SheetCtl::back_->bysize - 20 - 2);
		SheetCtl::tbox_col_ = Rgb(255, 255, 255);
		SheetCtl::tbox_timer_ = new Timer(task->queue_, 256);
		SheetCtl::tbox_timer_->set(50);

		for (;;) {
			if (!cmd_->isempty() && cmd_wait_ < 0) {
				cmd_wait_ = cmd_->pop();
				wait();
				Output8(kPortKeyData, cmd_wait_);
			}
			Cli();
			if (task->queue_->isempty()) {
				task->sleep();
				Sti();
			} else {
				int code = task->queue_->pop();
				Sti();
				if (code == SheetCtl::tbox_timer_->data()) {
					SheetCtl::drawLine(SheetCtl::back_, SheetCtl::tbox_col_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
					SheetCtl::refresh(SheetCtl::back_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 3, SheetCtl::back_->bysize - 20 - 2);
					SheetCtl::tbox_col_ ^= Rgb(255, 255, 255);
					SheetCtl::tbox_timer_->set(50);
				} else if (code < 256) {
					decode(static_cast<unsigned char>(code));
				}
			}
		}
	}, new Queue(128));
	queue_ = task->queue_;
	cmd_ = new Queue(32);
}

void KeyboardController::decode(unsigned char code) {
	if (code < 0x80 && ascii_table_[code]) {
		char s[2];
		s[0] = (shift_ && !(leds_ & 4) || !shift_ && leds_ & 4) ? ascii_shift_table_[code] : ascii_table_[code];
		s[1] = 0;
		SheetCtl::drawLine(SheetCtl::back_, Rgb(255, 255, 255), SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
		SheetCtl::drawString(SheetCtl::back_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 3, 0, s);
		SheetCtl::tbox_cpos_ += 8;
		SheetCtl::tbox_col_ = 0;
		SheetCtl::drawLine(SheetCtl::back_, SheetCtl::tbox_col_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
		SheetCtl::refresh(SheetCtl::back_, SheetCtl::tbox_cpos_ - 8 + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ - 8 + 2 + 8 + 1, SheetCtl::back_->bysize - 20 - 2);
		//SheetCtl::tbox_timer_->set(50); タイマーキャンセルしたい
	}
	switch (code) {
		case 0x57:
			SheetCtl::upDown(SheetCtl::sheets_[1], SheetCtl::top_ - 1);
			break;

		case 0x3a: // Caps Lock
			leds_ ^= 4;
			cmd_->push(kKeyCmdLED);
			cmd_->push(leds_);
			break;

		case 0x45: // Num Lock
			leds_ ^= 2;
			cmd_->push(kKeyCmdLED);
			cmd_->push(leds_);
			break;

		case 0x46: // Scroll Lock
			leds_ ^= 1;
			cmd_->push(kKeyCmdLED);
			cmd_->push(leds_);
			break;

		case 0xfa:
			cmd_wait_= -1;
			break;

		case 0xfe:
			wait();
			Output8(kPortKeyData, cmd_wait_);
			break;

		case 0x2a:
			shift_ |= 1;
			break;

		case 0x36:
			shift_ |= 2;
			break;

		case 0xaa:
			shift_ &= ~1;
			break;

		case 0xb6:
			shift_ &= ~2;
			break;

		case 0x0e:
			if (SheetCtl::tbox_cpos_ > 2) {
				SheetCtl::tbox_cpos_ -= 8;
				SheetCtl::fillRect(SheetCtl::back_, Rgb(255, 255, 255), SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2 + 8 + 1, SheetCtl::back_->bysize - 20 - 2);
				SheetCtl::tbox_col_ = 0;
				SheetCtl::drawLine(SheetCtl::back_, SheetCtl::tbox_col_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
				SheetCtl::refresh(SheetCtl::back_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2 + 8 + 1, SheetCtl::back_->bysize - 20 - 2);
				//SheetCtl::tbox_timer_->set(50); タイマーキャンセルしたい
			}
			break;
	}
}

void KeyboardController::wait() {
	while (Input8(kPortKeyStatus) & kKeyStatusSendNotReady) {}
}
<commit_msg>バックスペースを押した際の，テキストボックスのキャレットの表示の修正<commit_after>#include "../headers.h"

unsigned char KeyboardController::ascii_table_[0x80] = {
	0,   0,   '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '-', '^', 0/*0x08*/, 0,
	'q', 'w', 'e', 'r', 't', 'y', 'u', 'i', 'o', 'p', '@', '[', 0/*0x0a*/, 0, 'a', 's',
	'd', 'f', 'g', 'h', 'j', 'k', 'l', ';', ':', 0,   0,   ']', 'z', 'x', 'c', 'v',
	'b', 'n', 'm', ',', '.', '/', 0,   '*', 0,   ' ', 0,   0,   0,   0,   0,   0,
	0,   0,   0,   0,   0,   0,   0,   '7', '8', '9', '-', '4', '5', '6', '+', '1',
	'2', '3', '0', '.', 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	0,   0,   0,   0x5c, 0,  0,   0,   0,   0,   0,   0,   0,   0,   0x5c, 0,  0
};
unsigned char KeyboardController::ascii_shift_table_[0x80] = {
	0,   0,   '!', 0x22, '#', '$', '%', '&', 0x27, '(', ')', '~', '=', '~', 0/*0x08*/, 0,
	'Q', 'W', 'E', 'R', 'T', 'Y', 'U', 'I', 'O', 'P', '`', '{', 0/*0x0a*/, 0, 'A', 'S',
	'D', 'F', 'G', 'H', 'J', 'K', 'L', '+', '*', 0,   0,   '}', 'Z', 'X', 'C', 'V',
	'B', 'N', 'M', '<', '>', '?', 0,   '*', 0,   ' ', 0,   0,   0,   0,   0,   0,
	0,   0,   0,   0,   0,   0,   0,   '7', '8', '9', '-', '4', '5', '6', '+', '1',
	'2', '3', '0', '.', 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	0,   0,   0,   '_', 0,   0,   0,   0,   0,   0,   0,   0,   0,   '|', 0,   0
};
int KeyboardController::shift_ = 0;
int KeyboardController::cmd_wait_ = -1;
int KeyboardController::leds_ = 0;
Queue *KeyboardController::cmd_ = nullptr;
Queue *KeyboardController::queue_ = nullptr;

void KeyboardController::init() {
	BootInfo *binfo = (BootInfo *)ADDRESS_BOOTINFO;
	leds_ = (binfo->leds >> 4) & 7;

	wait();
	Output8(kPortKeyCmd, kKeyCmdWriteMode);
	wait();
	Output8(kPortKeyData, kKBCMode);

	Task *task = new Task((char *)kKeyboardTaskName, 2, 1, []() {
		Task *task = TaskController::getNowTask();

		// キャレットの表示とタイマー設定
		SheetCtl::drawLine(SheetCtl::back_, SheetCtl::tbox_col_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
		SheetCtl::refresh(SheetCtl::back_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 3, SheetCtl::back_->bysize - 20 - 2);
		SheetCtl::tbox_col_ = Rgb(255, 255, 255);
		SheetCtl::tbox_timer_ = new Timer(task->queue_, 256);
		SheetCtl::tbox_timer_->set(50);

		for (;;) {
			if (!cmd_->isempty() && cmd_wait_ < 0) {
				cmd_wait_ = cmd_->pop();
				wait();
				Output8(kPortKeyData, cmd_wait_);
			}
			Cli();
			if (task->queue_->isempty()) {
				task->sleep();
				Sti();
			} else {
				int code = task->queue_->pop();
				Sti();
				if (code == SheetCtl::tbox_timer_->data()) {
					SheetCtl::drawLine(SheetCtl::back_, SheetCtl::tbox_col_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
					SheetCtl::refresh(SheetCtl::back_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 3, SheetCtl::back_->bysize - 20 - 2);
					SheetCtl::tbox_col_ ^= Rgb(255, 255, 255);
					SheetCtl::tbox_timer_->set(50);
				} else if (code < 256) {
					decode(static_cast<unsigned char>(code));
				}
			}
		}
	}, new Queue(128));
	queue_ = task->queue_;
	cmd_ = new Queue(32);
}

void KeyboardController::decode(unsigned char code) {
	if (code < 0x80 && ascii_table_[code]) {
		char s[2];
		s[0] = (shift_ && !(leds_ & 4) || !shift_ && leds_ & 4) ? ascii_shift_table_[code] : ascii_table_[code];
		s[1] = 0;
		SheetCtl::drawLine(SheetCtl::back_, Rgb(255, 255, 255), SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
		SheetCtl::drawString(SheetCtl::back_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 3, 0, s);
		SheetCtl::tbox_cpos_ += 8;
		SheetCtl::tbox_col_ = 0;
		SheetCtl::drawLine(SheetCtl::back_, SheetCtl::tbox_col_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
		SheetCtl::refresh(SheetCtl::back_, SheetCtl::tbox_cpos_ - 8 + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ - 8 + 2 + 8 + 1, SheetCtl::back_->bysize - 20 - 2);
		//SheetCtl::tbox_timer_->set(50); タイマーキャンセルしたい
	}
	switch (code) {
		case 0x57:
			SheetCtl::upDown(SheetCtl::sheets_[1], SheetCtl::top_ - 1);
			break;

		case 0x3a: // Caps Lock
			leds_ ^= 4;
			cmd_->push(kKeyCmdLED);
			cmd_->push(leds_);
			break;

		case 0x45: // Num Lock
			leds_ ^= 2;
			cmd_->push(kKeyCmdLED);
			cmd_->push(leds_);
			break;

		case 0x46: // Scroll Lock
			leds_ ^= 1;
			cmd_->push(kKeyCmdLED);
			cmd_->push(leds_);
			break;

		case 0xfa:
			cmd_wait_= -1;
			break;

		case 0xfe:
			wait();
			Output8(kPortKeyData, cmd_wait_);
			break;

		case 0x2a:
			shift_ |= 1;
			break;

		case 0x36:
			shift_ |= 2;
			break;

		case 0xaa:
			shift_ &= ~1;
			break;

		case 0xb6:
			shift_ &= ~2;
			break;

		case 0x0e:
			if (SheetCtl::tbox_cpos_ > 2) {
				SheetCtl::tbox_cpos_ -= 8;
				SheetCtl::fillRect(SheetCtl::back_, Rgb(255, 255, 255), SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2 + 8 + 1, SheetCtl::back_->bysize - 20 - 1);
				SheetCtl::tbox_col_ = 0;
				SheetCtl::drawLine(SheetCtl::back_, SheetCtl::tbox_col_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 2);
				SheetCtl::refresh(SheetCtl::back_, SheetCtl::tbox_cpos_ + 2, SheetCtl::back_->bysize - 20 - 22 + 2, SheetCtl::tbox_cpos_ + 2 + 8 + 1, SheetCtl::back_->bysize - 20 - 2);
				//SheetCtl::tbox_timer_->set(50); タイマーキャンセルしたい
			}
			break;
	}
}

void KeyboardController::wait() {
	while (Input8(kPortKeyStatus) & kKeyStatusSendNotReady) {}
}
<|endoftext|>


<commit_before>/*****************************************************************************
 * Media Library
 *****************************************************************************
 * Copyright (C) 2015 Hugo Beauzée-Luyssen, Videolabs
 *
 * Authors: Hugo Beauzée-Luyssen<hugo@beauzee.fr>
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * This program 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 program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
 *****************************************************************************/

#include "Directory.h"
#include "Media.h"
#include "Device.h"
#include "filesystem/unix/File.h"

#include <cstring>
#include <cstdlib>
#include <dirent.h>
#include <limits.h>
#include <sys/stat.h>
#include <unistd.h>

namespace fs
{

Directory::Directory( const std::string& path )
    : m_path( toAbsolute( path ) )
{
#ifndef NDEBUG
    struct stat s;
    lstat( m_path.c_str(), &s );
    if ( S_ISDIR( s.st_mode ) == false )
        throw std::runtime_error( "The provided path isn't a directory" );
#endif
    if ( *m_path.crbegin() != '/' )
        m_path += '/';
}

const std::string&Directory::path() const
{
    return m_path;
}

const std::vector<std::shared_ptr<IFile>>& Directory::files() const
{
    if ( m_dirs.size() == 0 && m_files.size() == 0 )
        read();
    return m_files;
}

const std::vector<std::shared_ptr<IDirectory>>& Directory::dirs() const
{
    if ( m_dirs.size() == 0 && m_files.size() == 0 )
        read();
    return m_dirs;
}

std::shared_ptr<IDevice> Directory::device() const
{
    auto lock = m_device.lock();
    if ( m_device.isCached() == false )
        m_device = Device::fromPath( m_path );
    return m_device.get();
}

std::string Directory::toAbsolute( const std::string& path )
{
    char abs[PATH_MAX];
    if ( realpath( path.c_str(), abs ) == nullptr )
    {
        std::string err( "Failed to convert to absolute path" );
        err += "(" + path + "): ";
        err += strerror(errno);
        throw std::runtime_error( err );
    }
    return std::string{ abs };
}

void Directory::read() const
{
    std::unique_ptr<DIR, int(*)(DIR*)> dir( opendir( m_path.c_str() ), closedir );
    if ( dir == nullptr )
    {
        std::string err( "Failed to open directory " );
        err += m_path;
        err += strerror(errno);
        throw std::runtime_error( err );
    }

    dirent* result = nullptr;

    while ( ( result = readdir( dir.get() ) ) != nullptr )
    {
        if ( strcmp( result->d_name, "." ) == 0 ||
             strcmp( result->d_name, ".." ) == 0 )
        {
            continue;
        }
        std::string path = m_path + "/" + result->d_name;

#if defined(_DIRENT_HAVE_D_TYPE) && defined(_BSD_SOURCE)
        if ( result->d_type == DT_DIR )
        {
#else
        struct stat s;
        if ( lstat( path.c_str(), &s ) != 0 )
        {
            if ( errno == EACCES )
                continue;
            std::string err( "Failed to get file info " );
            err += path + ": ";
            err += strerror(errno);
            throw std::runtime_error( err );
        }
        if ( S_ISDIR( s.st_mode ) )
        {
#endif
            auto dirPath = toAbsolute( path );
            if ( *dirPath.crbegin() != '/' )
                dirPath += '/';
            //FIXME: This will use toAbsolute again in the constructor.
            m_dirs.emplace_back( std::make_shared<Directory>( dirPath ) );
        }
        else
        {
            auto filePath = toAbsolute( path );
            m_files.emplace_back( std::make_shared<File>( filePath ) );
        }
    }
}

}
<commit_msg>Directory: Check for potential lstat failure<commit_after>/*****************************************************************************
 * Media Library
 *****************************************************************************
 * Copyright (C) 2015 Hugo Beauzée-Luyssen, Videolabs
 *
 * Authors: Hugo Beauzée-Luyssen<hugo@beauzee.fr>
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * This program 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 program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
 *****************************************************************************/

#include "Directory.h"
#include "Media.h"
#include "Device.h"
#include "filesystem/unix/File.h"

#include <cstring>
#include <cstdlib>
#include <dirent.h>
#include <limits.h>
#include <sys/stat.h>
#include <unistd.h>

namespace fs
{

Directory::Directory( const std::string& path )
    : m_path( toAbsolute( path ) )
{
#ifndef NDEBUG
    struct stat s;
    if ( lstat( m_path.c_str(), &s ) != 0 )
        throw std::runtime_error( "Failed to stat directory " + path + "(" +
                                  strerror( errno ) + ")" );
    if ( S_ISDIR( s.st_mode ) == false )
        throw std::runtime_error( "The provided path isn't a directory" );
#endif
    if ( *m_path.crbegin() != '/' )
        m_path += '/';
}

const std::string&Directory::path() const
{
    return m_path;
}

const std::vector<std::shared_ptr<IFile>>& Directory::files() const
{
    if ( m_dirs.size() == 0 && m_files.size() == 0 )
        read();
    return m_files;
}

const std::vector<std::shared_ptr<IDirectory>>& Directory::dirs() const
{
    if ( m_dirs.size() == 0 && m_files.size() == 0 )
        read();
    return m_dirs;
}

std::shared_ptr<IDevice> Directory::device() const
{
    auto lock = m_device.lock();
    if ( m_device.isCached() == false )
        m_device = Device::fromPath( m_path );
    return m_device.get();
}

std::string Directory::toAbsolute( const std::string& path )
{
    char abs[PATH_MAX];
    if ( realpath( path.c_str(), abs ) == nullptr )
    {
        std::string err( "Failed to convert to absolute path" );
        err += "(" + path + "): ";
        err += strerror(errno);
        throw std::runtime_error( err );
    }
    return std::string{ abs };
}

void Directory::read() const
{
    std::unique_ptr<DIR, int(*)(DIR*)> dir( opendir( m_path.c_str() ), closedir );
    if ( dir == nullptr )
    {
        std::string err( "Failed to open directory " );
        err += m_path;
        err += strerror(errno);
        throw std::runtime_error( err );
    }

    dirent* result = nullptr;

    while ( ( result = readdir( dir.get() ) ) != nullptr )
    {
        if ( strcmp( result->d_name, "." ) == 0 ||
             strcmp( result->d_name, ".." ) == 0 )
        {
            continue;
        }
        std::string path = m_path + "/" + result->d_name;

#if defined(_DIRENT_HAVE_D_TYPE) && defined(_BSD_SOURCE)
        if ( result->d_type == DT_DIR )
        {
#else
        struct stat s;
        if ( lstat( path.c_str(), &s ) != 0 )
        {
            if ( errno == EACCES )
                continue;
            std::string err( "Failed to get file info " );
            err += path + ": ";
            err += strerror(errno);
            throw std::runtime_error( err );
        }
        if ( S_ISDIR( s.st_mode ) )
        {
#endif
            auto dirPath = toAbsolute( path );
            if ( *dirPath.crbegin() != '/' )
                dirPath += '/';
            //FIXME: This will use toAbsolute again in the constructor.
            m_dirs.emplace_back( std::make_shared<Directory>( dirPath ) );
        }
        else
        {
            auto filePath = toAbsolute( path );
            m_files.emplace_back( std::make_shared<File>( filePath ) );
        }
    }
}

}
<|endoftext|>


<commit_before>//===- PassManager.h - Infrastructure for managing & running IR passes ----===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "llvm/ADT/STLExtras.h"
#include "llvm/IR/PassManager.h"
#include "llvm/Support/CommandLine.h"
#include "llvm/Support/Debug.h"

using namespace llvm;

static cl::opt<bool>
DebugPM("debug-pass-manager", cl::Hidden,
        cl::desc("Print pass management debugging information"));

PreservedAnalyses ModulePassManager::run(Module *M, ModuleAnalysisManager *AM) {
  PreservedAnalyses PA = PreservedAnalyses::all();

  if (DebugPM)
    dbgs() << "Starting module pass manager run.\n";

  for (unsigned Idx = 0, Size = Passes.size(); Idx != Size; ++Idx) {
    if (DebugPM)
      dbgs() << "Running module pass: " << Passes[Idx]->name() << "\n";

    PreservedAnalyses PassPA = Passes[Idx]->run(M, AM);
    if (AM)
      AM->invalidate(M, PassPA);
    PA.intersect(std::move(PassPA));
  }

  if (DebugPM)
    dbgs() << "Finished module pass manager run.\n";

  return PA;
}

ModuleAnalysisManager::ResultConceptT &
ModuleAnalysisManager::getResultImpl(void *PassID, Module *M) {
  ModuleAnalysisResultMapT::iterator RI;
  bool Inserted;
  std::tie(RI, Inserted) = ModuleAnalysisResults.insert(std::make_pair(
      PassID, std::unique_ptr<detail::AnalysisResultConcept<Module *>>()));

  // If we don't have a cached result for this module, look up the pass and run
  // it to produce a result, which we then add to the cache.
  if (Inserted)
    RI->second = std::move(lookupPass(PassID).run(M, this));

  return *RI->second;
}

ModuleAnalysisManager::ResultConceptT *
ModuleAnalysisManager::getCachedResultImpl(void *PassID, Module *M) const {
  ModuleAnalysisResultMapT::const_iterator RI =
      ModuleAnalysisResults.find(PassID);
  return RI == ModuleAnalysisResults.end() ? 0 : &*RI->second;
}

void ModuleAnalysisManager::invalidateImpl(void *PassID, Module *M) {
  ModuleAnalysisResults.erase(PassID);
}

void ModuleAnalysisManager::invalidateImpl(Module *M,
                                           const PreservedAnalyses &PA) {
  // FIXME: This is a total hack based on the fact that erasure doesn't
  // invalidate iteration for DenseMap.
  for (ModuleAnalysisResultMapT::iterator I = ModuleAnalysisResults.begin(),
                                          E = ModuleAnalysisResults.end();
       I != E; ++I)
    if (I->second->invalidate(M, PA))
      ModuleAnalysisResults.erase(I);
}

PreservedAnalyses FunctionPassManager::run(Function *F,
                                           FunctionAnalysisManager *AM) {
  PreservedAnalyses PA = PreservedAnalyses::all();

  if (DebugPM)
    dbgs() << "Starting function pass manager run.\n";

  for (unsigned Idx = 0, Size = Passes.size(); Idx != Size; ++Idx) {
    if (DebugPM)
      dbgs() << "Running function pass: " << Passes[Idx]->name() << "\n";

    PreservedAnalyses PassPA = Passes[Idx]->run(F, AM);
    if (AM)
      AM->invalidate(F, PassPA);
    PA.intersect(std::move(PassPA));
  }

  if (DebugPM)
    dbgs() << "Finished function pass manager run.\n";

  return PA;
}

bool FunctionAnalysisManager::empty() const {
  assert(FunctionAnalysisResults.empty() ==
             FunctionAnalysisResultLists.empty() &&
         "The storage and index of analysis results disagree on how many there "
         "are!");
  return FunctionAnalysisResults.empty();
}

void FunctionAnalysisManager::clear() {
  FunctionAnalysisResults.clear();
  FunctionAnalysisResultLists.clear();
}

FunctionAnalysisManager::ResultConceptT &
FunctionAnalysisManager::getResultImpl(void *PassID, Function *F) {
  FunctionAnalysisResultMapT::iterator RI;
  bool Inserted;
  std::tie(RI, Inserted) = FunctionAnalysisResults.insert(std::make_pair(
      std::make_pair(PassID, F), FunctionAnalysisResultListT::iterator()));

  // If we don't have a cached result for this function, look up the pass and
  // run it to produce a result, which we then add to the cache.
  if (Inserted) {
    FunctionAnalysisResultListT &ResultList = FunctionAnalysisResultLists[F];
    ResultList.emplace_back(PassID, lookupPass(PassID).run(F, this));
    RI->second = std::prev(ResultList.end());
  }

  return *RI->second->second;
}

FunctionAnalysisManager::ResultConceptT *
FunctionAnalysisManager::getCachedResultImpl(void *PassID, Function *F) const {
  FunctionAnalysisResultMapT::const_iterator RI =
      FunctionAnalysisResults.find(std::make_pair(PassID, F));
  return RI == FunctionAnalysisResults.end() ? 0 : &*RI->second->second;
}

void FunctionAnalysisManager::invalidateImpl(void *PassID, Function *F) {
  FunctionAnalysisResultMapT::iterator RI =
      FunctionAnalysisResults.find(std::make_pair(PassID, F));
  if (RI == FunctionAnalysisResults.end())
    return;

  FunctionAnalysisResultLists[F].erase(RI->second);
}

void FunctionAnalysisManager::invalidateImpl(Function *F,
                                             const PreservedAnalyses &PA) {
  // Clear all the invalidated results associated specifically with this
  // function.
  SmallVector<void *, 8> InvalidatedPassIDs;
  FunctionAnalysisResultListT &ResultsList = FunctionAnalysisResultLists[F];
  for (FunctionAnalysisResultListT::iterator I = ResultsList.begin(),
                                             E = ResultsList.end();
       I != E;)
    if (I->second->invalidate(F, PA)) {
      InvalidatedPassIDs.push_back(I->first);
      I = ResultsList.erase(I);
    } else {
      ++I;
    }
  while (!InvalidatedPassIDs.empty())
    FunctionAnalysisResults.erase(
        std::make_pair(InvalidatedPassIDs.pop_back_val(), F));
}

char FunctionAnalysisManagerModuleProxy::PassID;

FunctionAnalysisManagerModuleProxy::Result
FunctionAnalysisManagerModuleProxy::run(Module *M) {
  assert(FAM->empty() && "Function analyses ran prior to the module proxy!");
  return Result(*FAM);
}

FunctionAnalysisManagerModuleProxy::Result::~Result() {
  // Clear out the analysis manager if we're being destroyed -- it means we
  // didn't even see an invalidate call when we got invalidated.
  FAM->clear();
}

bool FunctionAnalysisManagerModuleProxy::Result::invalidate(
    Module *M, const PreservedAnalyses &PA) {
  // If this proxy isn't marked as preserved, then we can't even invalidate
  // individual function analyses, there may be an invalid set of Function
  // objects in the cache making it impossible to incrementally preserve them.
  // Just clear the entire manager.
  if (!PA.preserved(ID()))
    FAM->clear();

  // Return false to indicate that this result is still a valid proxy.
  return false;
}

char ModuleAnalysisManagerFunctionProxy::PassID;
<commit_msg>Fix comment (PR19188)<commit_after>//===- PassManager.cpp - Infrastructure for managing & running IR passes --===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "llvm/ADT/STLExtras.h"
#include "llvm/IR/PassManager.h"
#include "llvm/Support/CommandLine.h"
#include "llvm/Support/Debug.h"

using namespace llvm;

static cl::opt<bool>
DebugPM("debug-pass-manager", cl::Hidden,
        cl::desc("Print pass management debugging information"));

PreservedAnalyses ModulePassManager::run(Module *M, ModuleAnalysisManager *AM) {
  PreservedAnalyses PA = PreservedAnalyses::all();

  if (DebugPM)
    dbgs() << "Starting module pass manager run.\n";

  for (unsigned Idx = 0, Size = Passes.size(); Idx != Size; ++Idx) {
    if (DebugPM)
      dbgs() << "Running module pass: " << Passes[Idx]->name() << "\n";

    PreservedAnalyses PassPA = Passes[Idx]->run(M, AM);
    if (AM)
      AM->invalidate(M, PassPA);
    PA.intersect(std::move(PassPA));
  }

  if (DebugPM)
    dbgs() << "Finished module pass manager run.\n";

  return PA;
}

ModuleAnalysisManager::ResultConceptT &
ModuleAnalysisManager::getResultImpl(void *PassID, Module *M) {
  ModuleAnalysisResultMapT::iterator RI;
  bool Inserted;
  std::tie(RI, Inserted) = ModuleAnalysisResults.insert(std::make_pair(
      PassID, std::unique_ptr<detail::AnalysisResultConcept<Module *>>()));

  // If we don't have a cached result for this module, look up the pass and run
  // it to produce a result, which we then add to the cache.
  if (Inserted)
    RI->second = std::move(lookupPass(PassID).run(M, this));

  return *RI->second;
}

ModuleAnalysisManager::ResultConceptT *
ModuleAnalysisManager::getCachedResultImpl(void *PassID, Module *M) const {
  ModuleAnalysisResultMapT::const_iterator RI =
      ModuleAnalysisResults.find(PassID);
  return RI == ModuleAnalysisResults.end() ? 0 : &*RI->second;
}

void ModuleAnalysisManager::invalidateImpl(void *PassID, Module *M) {
  ModuleAnalysisResults.erase(PassID);
}

void ModuleAnalysisManager::invalidateImpl(Module *M,
                                           const PreservedAnalyses &PA) {
  // FIXME: This is a total hack based on the fact that erasure doesn't
  // invalidate iteration for DenseMap.
  for (ModuleAnalysisResultMapT::iterator I = ModuleAnalysisResults.begin(),
                                          E = ModuleAnalysisResults.end();
       I != E; ++I)
    if (I->second->invalidate(M, PA))
      ModuleAnalysisResults.erase(I);
}

PreservedAnalyses FunctionPassManager::run(Function *F,
                                           FunctionAnalysisManager *AM) {
  PreservedAnalyses PA = PreservedAnalyses::all();

  if (DebugPM)
    dbgs() << "Starting function pass manager run.\n";

  for (unsigned Idx = 0, Size = Passes.size(); Idx != Size; ++Idx) {
    if (DebugPM)
      dbgs() << "Running function pass: " << Passes[Idx]->name() << "\n";

    PreservedAnalyses PassPA = Passes[Idx]->run(F, AM);
    if (AM)
      AM->invalidate(F, PassPA);
    PA.intersect(std::move(PassPA));
  }

  if (DebugPM)
    dbgs() << "Finished function pass manager run.\n";

  return PA;
}

bool FunctionAnalysisManager::empty() const {
  assert(FunctionAnalysisResults.empty() ==
             FunctionAnalysisResultLists.empty() &&
         "The storage and index of analysis results disagree on how many there "
         "are!");
  return FunctionAnalysisResults.empty();
}

void FunctionAnalysisManager::clear() {
  FunctionAnalysisResults.clear();
  FunctionAnalysisResultLists.clear();
}

FunctionAnalysisManager::ResultConceptT &
FunctionAnalysisManager::getResultImpl(void *PassID, Function *F) {
  FunctionAnalysisResultMapT::iterator RI;
  bool Inserted;
  std::tie(RI, Inserted) = FunctionAnalysisResults.insert(std::make_pair(
      std::make_pair(PassID, F), FunctionAnalysisResultListT::iterator()));

  // If we don't have a cached result for this function, look up the pass and
  // run it to produce a result, which we then add to the cache.
  if (Inserted) {
    FunctionAnalysisResultListT &ResultList = FunctionAnalysisResultLists[F];
    ResultList.emplace_back(PassID, lookupPass(PassID).run(F, this));
    RI->second = std::prev(ResultList.end());
  }

  return *RI->second->second;
}

FunctionAnalysisManager::ResultConceptT *
FunctionAnalysisManager::getCachedResultImpl(void *PassID, Function *F) const {
  FunctionAnalysisResultMapT::const_iterator RI =
      FunctionAnalysisResults.find(std::make_pair(PassID, F));
  return RI == FunctionAnalysisResults.end() ? 0 : &*RI->second->second;
}

void FunctionAnalysisManager::invalidateImpl(void *PassID, Function *F) {
  FunctionAnalysisResultMapT::iterator RI =
      FunctionAnalysisResults.find(std::make_pair(PassID, F));
  if (RI == FunctionAnalysisResults.end())
    return;

  FunctionAnalysisResultLists[F].erase(RI->second);
}

void FunctionAnalysisManager::invalidateImpl(Function *F,
                                             const PreservedAnalyses &PA) {
  // Clear all the invalidated results associated specifically with this
  // function.
  SmallVector<void *, 8> InvalidatedPassIDs;
  FunctionAnalysisResultListT &ResultsList = FunctionAnalysisResultLists[F];
  for (FunctionAnalysisResultListT::iterator I = ResultsList.begin(),
                                             E = ResultsList.end();
       I != E;)
    if (I->second->invalidate(F, PA)) {
      InvalidatedPassIDs.push_back(I->first);
      I = ResultsList.erase(I);
    } else {
      ++I;
    }
  while (!InvalidatedPassIDs.empty())
    FunctionAnalysisResults.erase(
        std::make_pair(InvalidatedPassIDs.pop_back_val(), F));
}

char FunctionAnalysisManagerModuleProxy::PassID;

FunctionAnalysisManagerModuleProxy::Result
FunctionAnalysisManagerModuleProxy::run(Module *M) {
  assert(FAM->empty() && "Function analyses ran prior to the module proxy!");
  return Result(*FAM);
}

FunctionAnalysisManagerModuleProxy::Result::~Result() {
  // Clear out the analysis manager if we're being destroyed -- it means we
  // didn't even see an invalidate call when we got invalidated.
  FAM->clear();
}

bool FunctionAnalysisManagerModuleProxy::Result::invalidate(
    Module *M, const PreservedAnalyses &PA) {
  // If this proxy isn't marked as preserved, then we can't even invalidate
  // individual function analyses, there may be an invalid set of Function
  // objects in the cache making it impossible to incrementally preserve them.
  // Just clear the entire manager.
  if (!PA.preserved(ID()))
    FAM->clear();

  // Return false to indicate that this result is still a valid proxy.
  return false;
}

char ModuleAnalysisManagerFunctionProxy::PassID;
<|endoftext|>


<commit_before>#ifndef MUNCHAR_LEXEMES
#define MUNCHAR_LEXEMES

#include "munchar.hpp"
#include <cctype>

namespace Munchar {
  namespace Lexemes {

    constexpr auto null         = '\0'_lit;
    constexpr auto tab          = '\t'_lit;
    constexpr auto newline      = '\n'_lit;
    constexpr auto linefeed     = newline;
    constexpr auto cr           = '\r'_lit;
    constexpr auto crlf         = "\r\n"_lit;

    constexpr auto space        = ' '_lit;
    constexpr auto exclamation  = '!'_lit;
    constexpr auto d_quote      = '"'_lit;
    constexpr auto quote        = d_quote;
    constexpr auto pound        = '#'_lit;
    constexpr auto hash         = pound;
    constexpr auto octothorpe   = pound;
    constexpr auto dollar       = '$'_lit;
    constexpr auto percent      = '%'_lit;
    constexpr auto ampersand    = '&'_lit;
    constexpr auto apostrophe   = '\''_lit;
    constexpr auto s_quote      = apostrophe;
    constexpr auto l_paren      = '('_lit;
    constexpr auto r_paren      = ')'_lit;
    constexpr auto asterisk     = '*'_lit;
    constexpr auto plus         = '+'_lit;
    constexpr auto comma        = ','_lit;
    constexpr auto hyphen       = '-'_lit;
    constexpr auto minus        = hyphen;
    constexpr auto period       = '.'_lit;
    constexpr auto dot          = period;
    constexpr auto slash        = '/'_lit;
    constexpr auto divide       = slash;
    constexpr auto colon        = ':'_lit;
    constexpr auto semicolon    = ';'_lit;
    constexpr auto less_than    = '<'_lit;
    constexpr auto equals       = '='_lit;
    constexpr auto greater_than = '>'_lit;
    constexpr auto question     = '?'_lit;
    constexpr auto at           = '@'_lit;
    constexpr auto l_bracket    = '['_lit;
    constexpr auto backslash    = '\\';
    constexpr auto r_bracket    = ']'_lit;
    constexpr auto caret        = '^'_lit;
    constexpr auto circumflex   = caret;
    constexpr auto underscore   = '_'_lit;
    constexpr auto backquote    = '`'_lit;
    constexpr auto tilde        = '~'_lit;

    constexpr auto _            = Any_Char { };
    constexpr auto letter       = P(std::isalpha);
    constexpr auto alphanumeric = P(std::isalnum);
    constexpr auto digit        = P(std::isdigit);
    constexpr auto hex_digit    = P(std::isxdigit);
    constexpr auto whitespace   = P(std::isspace);
    constexpr auto sign         = "+-"_cls;
    // constexpr auto id_start = letter | underscore;
    // constexpr auto id_body = alphanumeric | underscore;
    // constexpr auto identifier = id_start & *id_body;
    // constexpr auto number = ~sign & ((*digit & '.'_lit_lit & +digit) | +digit);
  }
}

#endif<commit_msg>forgot some stuff<commit_after>#ifndef MUNCHAR_LEXEMES
#define MUNCHAR_LEXEMES

#include "munchar.hpp"
#include <cctype>

namespace Munchar {
  namespace Lexemes {

    constexpr auto null          = '\0'_lit;
    constexpr auto tab           = '\t'_lit;
    constexpr auto newline       = '\n'_lit;
    constexpr auto linefeed      = newline;
    constexpr auto cr            = '\r'_lit;
    constexpr auto crlf          = "\r\n"_lit;

    constexpr auto space         = ' '_lit;
    constexpr auto exclamation   = '!'_lit;
    constexpr auto double_quote  = '"'_lit;
    constexpr auto quote         = double_quote;
    constexpr auto pound         = '#'_lit;
    constexpr auto hash          = pound;
    constexpr auto octothorpe    = pound;
    constexpr auto dollar        = '$'_lit;
    constexpr auto percent       = '%'_lit;
    constexpr auto ampersand     = '&'_lit;
    constexpr auto apostrophe    = '\''_lit;
    constexpr auto single_quote  = apostrophe;
    constexpr auto left_paren    = '('_lit;
    constexpr auto right_paren   = ')'_lit;
    constexpr auto asterisk      = '*'_lit;
    constexpr auto plus          = '+'_lit;
    constexpr auto comma         = ','_lit;
    constexpr auto hyphen        = '-'_lit;
    constexpr auto minus         = hyphen;
    constexpr auto period        = '.'_lit;
    constexpr auto dot           = period;
    constexpr auto slash         = '/'_lit;
    constexpr auto divide        = slash;
    constexpr auto colon         = ':'_lit;
    constexpr auto semicolon     = ';'_lit;
    constexpr auto less_than     = '<'_lit;
    constexpr auto equals        = '='_lit;
    constexpr auto greater_than  = '>'_lit;
    constexpr auto question      = '?'_lit;
    constexpr auto at            = '@'_lit;
    constexpr auto left_bracket  = '['_lit;
    constexpr auto backslash     = '\\';
    constexpr auto right_bracket = ']'_lit;
    constexpr auto caret         = '^'_lit;
    constexpr auto circumflex    = caret;
    constexpr auto underscore    = '_'_lit;
    constexpr auto backquote     = '`'_lit;
    constexpr auto left_brace    = '{'_lit;
    constexpr auto vertical_bar  = '|'_lit;
    constexpr auto pipe          = vertical_bar;
    constexpr auto right_brace   = '}'_lit;
    constexpr auto tilde         = '~'_lit;

    constexpr auto _             = Any_Char { };
    constexpr auto letter        = P(std::isalpha);
    constexpr auto alphanumeric  = P(std::isalnum);
    constexpr auto digit         = P(std::isdigit);
    constexpr auto hex_digit     = P(std::isxdigit);
    constexpr auto whitespace    = P(std::isspace);
    constexpr auto sign          = "+-"_cls;
    // constexpr auto id_start = letter | underscore;
    // constexpr auto id_body = alphanumeric | underscore;
    // constexpr auto identifier = id_start & *id_body;
    // constexpr auto number = ~sign & ((*digit & '.'_lit_lit & +digit) | +digit);
  }
}

#endif<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  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 Willow Garage, Inc. 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.
 *
 * $Id$
 *
 */

#ifndef PCL_SURFACE_RECONSTRUCTION_IMPL_H_
#define PCL_SURFACE_RECONSTRUCTION_IMPL_H_

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::SurfaceReconstruction<PointInT>::reconstruct (pcl::PolygonMesh &output)
{
  // Copy the header
  output.header = input_->header;

  if (!initCompute ())
  {
    output.cloud.width = output.cloud.height = 0;
    output.cloud.data.clear ();
    output.polygons.clear ();
    return;
  }

  // Check if a space search locator was given
  if (check_tree_)
  {
    if (!tree_)
    {
      PCL_ERROR ("[pcl::%s::compute] No spatial search method was given!\n", getClassName ().c_str ());
      output.cloud.width = output.cloud.height = 0;
      output.cloud.data.clear ();
      output.polygons.clear ();
      return;
    }

    // Send the surface dataset to the spatial locator
    tree_->setInputCloud (input_, indices_);
  }

  // Set up the output dataset
  pcl::toROSMsg (*input_, output.cloud); /// NOTE: passing in boost shared pointer with * as const& should be OK here
  output.polygons.clear ();
  output.polygons.reserve (2*indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
  // Perform the actual surface reconstruction
  performReconstruction (output);

  deinitCompute ();
}

#endif  // PCL_SURFACE_RECONSTRUCTION_IMPL_H_

<commit_msg>implemented defaults for the search method, per http://www.pcl-developers.org/defaults-for-search-method-td4725514.html<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  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 Willow Garage, Inc. 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.
 *
 * $Id$
 *
 */

#ifndef PCL_SURFACE_RECONSTRUCTION_IMPL_H_
#define PCL_SURFACE_RECONSTRUCTION_IMPL_H_
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/organized_data.h>

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::SurfaceReconstruction<PointInT>::reconstruct (pcl::PolygonMesh &output)
{
  // Copy the header
  output.header = input_->header;

  if (!initCompute ())
  {
    output.cloud.width = output.cloud.height = 0;
    output.cloud.data.clear ();
    output.polygons.clear ();
    return;
  }

  // Check if a space search locator was given
  if (check_tree_)
  {
    if (!tree_)
    {
      if (input_->isOrganized ())
        tree_.reset (new pcl::OrganizedDataIndex<PointInT> ());
      else
        tree_.reset (new pcl::KdTreeFLANN<PointInT> (false));
    }

    // Send the surface dataset to the spatial locator
    tree_->setInputCloud (input_, indices_);
  }

  // Set up the output dataset
  pcl::toROSMsg (*input_, output.cloud); /// NOTE: passing in boost shared pointer with * as const& should be OK here
  output.polygons.clear ();
  output.polygons.reserve (2*indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
  // Perform the actual surface reconstruction
  performReconstruction (output);

  deinitCompute ();
}

#endif  // PCL_SURFACE_RECONSTRUCTION_IMPL_H_

<|endoftext|>


<commit_before>#include<bits/stdc++.h>
#define FOR(i,m,n) for(int i=m;i<(n);i++)
#define REP(i,n) FOR(i,0,n)
#define ALL(x) (x).begin(),(x).end()
using namespace std;
typedef long long ll;
const ll inf = INT_MAX;
const double eps = 1e-8;
const double pi = acos(-1.0);
const int di[][2] = {{1,0},{0,1},{-1,0},{0,-1},{1,1},{-1,-1},{-1,1},{1,-1}};

struct status {
  int y;
  int x;
  int turn;
};

int main(void){
  int t,n;
  while(cin>>t>>n,t){
    char masu[64][64];
    memset(masu,'.',sizeof masu);
    REP(i,n){
      int x,y;cin>>x>>y;
      masu[x+32][y+32]='#';
    }
    int sx,sy;cin>>sx>>sy;
    queue<status> q;
    int ans=0;
    q.push({sx+32,sy+32,0});
    while(!q.empty()){
      auto a=q.front();q.pop();
      masu[a.y][a.x]='*';
      if(a.turn>t)continue;
      ans++;
      REP(i,6){
        int dx=a.x+di[i][0],dy=a.y+di[i][1];
        if(dx>=0&&dx<64&&dy>=0&&dy<64&&masu[dy][dx]=='.'){
          q.push({dy,dx,a.turn+1});
          masu[dy][dx]='*';
        }
      }
    }
    // REP(i,64){
    //   REP(j,64)cout<<masu[i][j];
    //   cout<<endl;
    // }
    cout<<ans<<endl;
  }
  return 0;
}
<commit_msg>AC aoj/2253<commit_after>#include<bits/stdc++.h>
#define FOR(i,m,n) for(int i=m;i<(n);i++)
#define REP(i,n) FOR(i,0,n)
#define ALL(x) (x).begin(),(x).end()
using namespace std;
typedef long long ll;
const ll inf = INT_MAX;
const double eps = 1e-8;
const double pi = acos(-1.0);
// const int di[][2] = {{1,0},{0,1},{-1,0},{0,-1},{1,1},{-1,-1},{-1,1},{-1,-1}};
const int di[][2] = {{0,1},{1,1},{1,0},{0,-1},{-1,-1},{-1,0}};

const int geta = 100;

int main(void){
  int t,n;
  while(cin>>t>>n,t){
    vector<vector<int>> masu(300, vector<int>(300));
    REP(i,n){
      int x,y;cin>>x>>y;
      masu[x+geta][y+geta]=1;
    }
    int sx,sy;cin>>sx>>sy;
    sx+=geta;
    sy+=geta;
    queue<tuple<int,int,int>> next;
    next.push(make_tuple(sx,sy,t));
    masu[sx][sy]=-1;
    int answer=0;
    while(!next.empty()){
      auto a=next.front();next.pop();
      int x,y,turn;
      tie(x,y,turn)=a;
      // cout<<turn<<endl;
      // cout<<x<<","<<y<<endl;
      answer++;
      REP(j,6){
        int nx=x+di[j][0],ny=y+di[j][1];
        if(masu[nx][ny]==0&&turn>0){
          next.push(make_tuple(nx,ny,turn-1));
          masu[nx][ny]=-1;
        }
      }
    }
    cout<<answer<<endl;
  }
  return 0;
}
<|endoftext|>


<commit_before>#include "includes.h"

static const char *tested_logger_name = "null_logger";
static const char *tested_logger_name2 = "null_logger2";

TEST_CASE("register_drop", "[registry]")
{
    spdlog::drop_all();
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name);
    REQUIRE(spdlog::get(tested_logger_name) != nullptr);
    // Throw if registring existing name
    REQUIRE_THROWS_AS(spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name), spdlog::spdlog_ex);
}

TEST_CASE("explicit register"
          "[registry]")
{
    spdlog::drop_all();
    auto logger = std::make_shared<spdlog::logger>(tested_logger_name, std::make_shared<spdlog::sinks::null_sink_st>());
    spdlog::register_logger(logger);
    REQUIRE(spdlog::get(tested_logger_name) != nullptr);
    // Throw if registring existing name
    REQUIRE_THROWS_AS(spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name), spdlog::spdlog_ex);
}

TEST_CASE("apply_all"
          "[registry]")
{
    spdlog::drop_all();
    auto logger = std::make_shared<spdlog::logger>(tested_logger_name, std::make_shared<spdlog::sinks::null_sink_st>());
    spdlog::register_logger(logger);
    auto logger2 = std::make_shared<spdlog::logger>(tested_logger_name2, std::make_shared<spdlog::sinks::null_sink_st>());
    spdlog::register_logger(logger2);

    int counter = 0;
    spdlog::apply_all([&counter](std::shared_ptr<spdlog::logger> l) { counter++; });
    REQUIRE(counter == 2);

    counter = 0;
    spdlog::drop(tested_logger_name2);
    spdlog::apply_all([&counter](std::shared_ptr<spdlog::logger> l) {
        REQUIRE(l->name() == tested_logger_name);
        counter++;
    });
    REQUIRE(counter == 1);
}

TEST_CASE("drop"
          "[registry]")
{
    spdlog::drop_all();
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name);
    spdlog::drop(tested_logger_name);
    REQUIRE_FALSE(spdlog::get(tested_logger_name));
}

TEST_CASE("drop_all"
          "[registry]")
{
    spdlog::drop_all();
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name);
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name2);
    spdlog::drop_all();
    REQUIRE_FALSE(spdlog::get(tested_logger_name));
    REQUIRE_FALSE(spdlog::get(tested_logger_name));
}

TEST_CASE("drop non existing"
          "[registry]")
{
    spdlog::drop_all();
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name);
    spdlog::drop("some_name");
    REQUIRE_FALSE(spdlog::get("some_name"));
    REQUIRE(spdlog::get(tested_logger_name));
    spdlog::drop_all();
}
<commit_msg>Fixed warning in tests<commit_after>#include "includes.h"

static const char *tested_logger_name = "null_logger";
static const char *tested_logger_name2 = "null_logger2";

TEST_CASE("register_drop", "[registry]")
{
    spdlog::drop_all();
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name);
    REQUIRE(spdlog::get(tested_logger_name) != nullptr);
    // Throw if registring existing name
    REQUIRE_THROWS_AS(spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name), spdlog::spdlog_ex);
}

TEST_CASE("explicit register"
          "[registry]")
{
    spdlog::drop_all();
    auto logger = std::make_shared<spdlog::logger>(tested_logger_name, std::make_shared<spdlog::sinks::null_sink_st>());
    spdlog::register_logger(logger);
    REQUIRE(spdlog::get(tested_logger_name) != nullptr);
    // Throw if registring existing name
    REQUIRE_THROWS_AS(spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name), spdlog::spdlog_ex);
}

TEST_CASE("apply_all"
          "[registry]")
{
    spdlog::drop_all();
    auto logger = std::make_shared<spdlog::logger>(tested_logger_name, std::make_shared<spdlog::sinks::null_sink_st>());
    spdlog::register_logger(logger);
    auto logger2 = std::make_shared<spdlog::logger>(tested_logger_name2, std::make_shared<spdlog::sinks::null_sink_st>());
    spdlog::register_logger(logger2);

    int counter = 0;
    spdlog::apply_all([&counter](std::shared_ptr<spdlog::logger> ) { counter++; });
    REQUIRE(counter == 2);

    counter = 0;
    spdlog::drop(tested_logger_name2);
    spdlog::apply_all([&counter](std::shared_ptr<spdlog::logger> l) {
        REQUIRE(l->name() == tested_logger_name);
        counter++;
    });
    REQUIRE(counter == 1);
}

TEST_CASE("drop"
          "[registry]")
{
    spdlog::drop_all();
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name);
    spdlog::drop(tested_logger_name);
    REQUIRE_FALSE(spdlog::get(tested_logger_name));
}

TEST_CASE("drop_all"
          "[registry]")
{
    spdlog::drop_all();
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name);
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name2);
    spdlog::drop_all();
    REQUIRE_FALSE(spdlog::get(tested_logger_name));
    REQUIRE_FALSE(spdlog::get(tested_logger_name));
}

TEST_CASE("drop non existing"
          "[registry]")
{
    spdlog::drop_all();
    spdlog::create<spdlog::sinks::null_sink_mt>(tested_logger_name);
    spdlog::drop("some_name");
    REQUIRE_FALSE(spdlog::get("some_name"));
    REQUIRE(spdlog::get(tested_logger_name));
    spdlog::drop_all();
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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.
 *
 ****************************************************************************/

/**
 * @file battery.cpp
 *
 * Library calls for battery functionality.
 *
 * @author Julian Oes <julian@oes.ch>
 */

#include "battery.h"
#include <mathlib/mathlib.h>

Battery::Battery() :
	SuperBlock(nullptr, "BAT"),
	_v_empty(this, "V_EMPTY"),
	_v_charged(this, "V_CHARGED"),
	_n_cells(this, "N_CELLS"),
	_capacity(this, "CAPACITY"),
	_v_load_drop(this, "V_LOAD_DROP"),
	_r_internal(this, "R_INTERNAL"),
	_low_thr(this, "LOW_THR"),
	_crit_thr(this, "CRIT_THR"),
	_emergency_thr(this, "EMERGEN_THR"),
	_voltage_filtered_v(-1.f),
	_current_filtered_a(-1.f),
	_discharged_mah(0.f),
	_discharged_mah_loop(0.f),
	_remaining_voltage(1.f),
	_remaining(2.f),
	_scale(1.f),
	_warning(battery_status_s::BATTERY_WARNING_NONE),
	_last_timestamp(0)
{
	/* load initial params */
	updateParams();
}

Battery::~Battery()
{
}

void
Battery::reset(battery_status_s *battery_status)
{
	memset(battery_status, 0, sizeof(*battery_status));
	battery_status->current_a = -1.f;
	battery_status->remaining = 1.f;
	battery_status->scale = 1.f;
	battery_status->cell_count = _n_cells.get();
	// TODO: check if it is sane to reset warning to NONE
	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
	battery_status->connected = false;
}

void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
			     bool connected, bool selected_source, int priority,
			     float throttle_normalized,
			     bool armed, battery_status_s *battery_status)
{
	reset(battery_status);
	battery_status->timestamp = timestamp;
	filterVoltage(voltage_v);
	filterCurrent(current_a);
	sumDischarged(timestamp, current_a);
	estimateRemaining(_voltage_filtered_v, _current_filtered_a, throttle_normalized, armed);
	determineWarning(connected);
	computeScale();

	if (_voltage_filtered_v > 2.1f) {
		battery_status->voltage_v = voltage_v;
		battery_status->voltage_filtered_v = _voltage_filtered_v;
		battery_status->scale = _scale;
		battery_status->current_a = current_a;
		battery_status->current_filtered_a = _current_filtered_a;
		battery_status->discharged_mah = _discharged_mah;
		battery_status->warning = _warning;
		battery_status->remaining = _remaining;
		battery_status->connected = connected;
		battery_status->system_source = selected_source;
		battery_status->priority = priority;
	}
}

void
Battery::filterVoltage(float voltage_v)
{
	if (_voltage_filtered_v < 0.f) {
		_voltage_filtered_v = voltage_v;
	}

	// TODO: inspect that filter performance
	const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f;

	if (PX4_ISFINITE(filtered_next)) {
		_voltage_filtered_v = filtered_next;
	}
}

void
Battery::filterCurrent(float current_a)
{
	if (_current_filtered_a < 0.f) {
		_current_filtered_a = current_a;
	}

	// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
	const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;

	if (PX4_ISFINITE(filtered_next)) {
		_current_filtered_a = filtered_next;
	}
}

void
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
{
	// Not a valid measurement
	if (current_a < 0.f) {
		// Because the measurement was invalid we need to stop integration
		// and re-initialize with the next valid measurement
		_last_timestamp = 0;
		return;
	}

	// Ignore first update because we don't know dt.
	if (_last_timestamp != 0) {
		const float dt = (timestamp - _last_timestamp) / 1e6;
		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
		_discharged_mah += _discharged_mah_loop;
	}

	_last_timestamp = timestamp;
}

void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed)
{

	// remaining battery capacity based on voltage
	float cell_voltage = voltage_v / _n_cells.get();

	// correct battery voltage locally for load drop to avoid estimation fluctuations
	if (_r_internal.get() >= 0.f) {
		cell_voltage += _r_internal.get() * current_a;

	} else {
		// assume quadratic relation between throttle and current
		// good assumption if throttle represents RPM
		cell_voltage += throttle_normalized * throttle_normalized * _v_load_drop.get();
	}
	_remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f);

	// choose which quantity we're using for final reporting
	if (_capacity.get() > 0.f) {
		// if battery capacity is known, fuse voltage measurement with used capacity
		if (_remaining > 1.f) {
			// initialization of the estimation state
			_remaining = _remaining_voltage;

		} else {
			// The lower the voltage the more adjust the estimate with it to avoid deep discharge
			const float weight_v = 3e-4f * (1 - _remaining_voltage);
			_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
			// directly apply current capacity slope calculated using current
			_remaining -= _discharged_mah_loop / _capacity.get();
			_remaining = math::max(_remaining, 0.f);
		}

	} else {
		// else use voltage
		_remaining = _remaining_voltage;
	}
}

void
Battery::determineWarning(bool connected)
{
	if (connected) {
		// propagate warning state only if the state is higher, otherwise remain in current warning state
		if (_remaining < _emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;

		} else if (_remaining < _crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;

		} else if (_remaining < _low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
			_warning = battery_status_s::BATTERY_WARNING_LOW;
		}
	}
}

void
Battery::computeScale()
{
	const float voltage_range = (_v_charged.get() - _v_empty.get());

	// reusing capacity calculation to get single cell voltage before drop
	const float bat_v = _v_empty.get() + (voltage_range * _remaining_voltage);

	_scale = _v_charged.get() / bat_v;

	if (_scale > 1.3f) { // Allow at most 30% compensation
		_scale = 1.3f;

	} else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery
		_scale = 1.f;
	}
}
<commit_msg>fix formattting<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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.
 *
 ****************************************************************************/

/**
 * @file battery.cpp
 *
 * Library calls for battery functionality.
 *
 * @author Julian Oes <julian@oes.ch>
 */

#include "battery.h"
#include <mathlib/mathlib.h>

Battery::Battery() :
	SuperBlock(nullptr, "BAT"),
	_v_empty(this, "V_EMPTY"),
	_v_charged(this, "V_CHARGED"),
	_n_cells(this, "N_CELLS"),
	_capacity(this, "CAPACITY"),
	_v_load_drop(this, "V_LOAD_DROP"),
	_r_internal(this, "R_INTERNAL"),
	_low_thr(this, "LOW_THR"),
	_crit_thr(this, "CRIT_THR"),
	_emergency_thr(this, "EMERGEN_THR"),
	_voltage_filtered_v(-1.f),
	_current_filtered_a(-1.f),
	_discharged_mah(0.f),
	_discharged_mah_loop(0.f),
	_remaining_voltage(1.f),
	_remaining(2.f),
	_scale(1.f),
	_warning(battery_status_s::BATTERY_WARNING_NONE),
	_last_timestamp(0)
{
	/* load initial params */
	updateParams();
}

Battery::~Battery()
{
}

void
Battery::reset(battery_status_s *battery_status)
{
	memset(battery_status, 0, sizeof(*battery_status));
	battery_status->current_a = -1.f;
	battery_status->remaining = 1.f;
	battery_status->scale = 1.f;
	battery_status->cell_count = _n_cells.get();
	// TODO: check if it is sane to reset warning to NONE
	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
	battery_status->connected = false;
}

void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
			     bool connected, bool selected_source, int priority,
			     float throttle_normalized,
			     bool armed, battery_status_s *battery_status)
{
	reset(battery_status);
	battery_status->timestamp = timestamp;
	filterVoltage(voltage_v);
	filterCurrent(current_a);
	sumDischarged(timestamp, current_a);
	estimateRemaining(_voltage_filtered_v, _current_filtered_a, throttle_normalized, armed);
	determineWarning(connected);
	computeScale();

	if (_voltage_filtered_v > 2.1f) {
		battery_status->voltage_v = voltage_v;
		battery_status->voltage_filtered_v = _voltage_filtered_v;
		battery_status->scale = _scale;
		battery_status->current_a = current_a;
		battery_status->current_filtered_a = _current_filtered_a;
		battery_status->discharged_mah = _discharged_mah;
		battery_status->warning = _warning;
		battery_status->remaining = _remaining;
		battery_status->connected = connected;
		battery_status->system_source = selected_source;
		battery_status->priority = priority;
	}
}

void
Battery::filterVoltage(float voltage_v)
{
	if (_voltage_filtered_v < 0.f) {
		_voltage_filtered_v = voltage_v;
	}

	// TODO: inspect that filter performance
	const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f;

	if (PX4_ISFINITE(filtered_next)) {
		_voltage_filtered_v = filtered_next;
	}
}

void
Battery::filterCurrent(float current_a)
{
	if (_current_filtered_a < 0.f) {
		_current_filtered_a = current_a;
	}

	// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
	const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;

	if (PX4_ISFINITE(filtered_next)) {
		_current_filtered_a = filtered_next;
	}
}

void
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
{
	// Not a valid measurement
	if (current_a < 0.f) {
		// Because the measurement was invalid we need to stop integration
		// and re-initialize with the next valid measurement
		_last_timestamp = 0;
		return;
	}

	// Ignore first update because we don't know dt.
	if (_last_timestamp != 0) {
		const float dt = (timestamp - _last_timestamp) / 1e6;
		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
		_discharged_mah += _discharged_mah_loop;
	}

	_last_timestamp = timestamp;
}

void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed)
{

	// remaining battery capacity based on voltage
	float cell_voltage = voltage_v / _n_cells.get();

	// correct battery voltage locally for load drop to avoid estimation fluctuations
	if (_r_internal.get() >= 0.f) {
		cell_voltage += _r_internal.get() * current_a;

	} else {
		// assume quadratic relation between throttle and current
		// good assumption if throttle represents RPM
		cell_voltage += throttle_normalized * throttle_normalized * _v_load_drop.get();
	}

	_remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f);

	// choose which quantity we're using for final reporting
	if (_capacity.get() > 0.f) {
		// if battery capacity is known, fuse voltage measurement with used capacity
		if (_remaining > 1.f) {
			// initialization of the estimation state
			_remaining = _remaining_voltage;

		} else {
			// The lower the voltage the more adjust the estimate with it to avoid deep discharge
			const float weight_v = 3e-4f * (1 - _remaining_voltage);
			_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
			// directly apply current capacity slope calculated using current
			_remaining -= _discharged_mah_loop / _capacity.get();
			_remaining = math::max(_remaining, 0.f);
		}

	} else {
		// else use voltage
		_remaining = _remaining_voltage;
	}
}

void
Battery::determineWarning(bool connected)
{
	if (connected) {
		// propagate warning state only if the state is higher, otherwise remain in current warning state
		if (_remaining < _emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;

		} else if (_remaining < _crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;

		} else if (_remaining < _low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
			_warning = battery_status_s::BATTERY_WARNING_LOW;
		}
	}
}

void
Battery::computeScale()
{
	const float voltage_range = (_v_charged.get() - _v_empty.get());

	// reusing capacity calculation to get single cell voltage before drop
	const float bat_v = _v_empty.get() + (voltage_range * _remaining_voltage);

	_scale = _v_charged.get() / bat_v;

	if (_scale > 1.3f) { // Allow at most 30% compensation
		_scale = 1.3f;

	} else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery
		_scale = 1.f;
	}
}
<|endoftext|>


<commit_before>/*
    This .cpp file will be used to generate the different camera angles around the
    .pcd file. It will pass these views (poses) to the file findPoints.cpp

    Program stucture:

    class{
        .
        .
        .
        .
            Generates the views around the .pcd as poses
        .
        .
        .
        for(){
            - goes through the list of poses as passes them over to
                the findPoints.cpp
            - A parrallel array will be made to hold the value this class will
                return

        }

        .
        .
        .
        .
        TBD

    }

*/

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <ros/package.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

//count is a varuiable used to make sure that the node only generates the poses once
bool first_time = false;
//A two dimensional vector used to hold the poses generated around the object
std::vector<std::vector<geometry_msgs::Pose> > pose_2Dcontainer;

geometry_msgs::Point center;

void callback(const PointCloud::ConstPtr& msg)
{

    if(!first_time)
    {
    	int numPoints = 0;
    	printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);

    	/*The radius of the hypothetical circle around teh center of the point cloud where
		  generate our poses															*/
    	int circle_radius = (msg->width * 1.5);

    	//Finds the center of the PointCloud
    	BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
    	{
    		center.x += pt.x;
    		center.y += pt.y;
    		center.z += pt.z;
    		numPoints++;
    	}
    		center.x /= numPoints;
    		center.y /= numPoints;
    		center.z /= numPoints;

    	/*
    	Looping structure to create the poses and load them into a two dimmensional list
					GENERATING THE POSES
						1) Constant z
						2) Constant difference from center of PointCloud
						3) Stored in a 2D vector
							-- Each row is a different coordinate position of the pose
							-- Each column is a different orientation (angle) of the pose
		*/

    	first_time = true;
    }

    ROS_INFO("Center x = %f,  y = %f,  z = %f \n", center.x, center.y, center.z);

    /*
    BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
        printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
    */
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "generate_views");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<PointCloud>("cloud", 1, callback);
    ros::spin();
}

<commit_msg>Finished generating the poses around the PointCloud file<commit_after>/*
    This .cpp file will be used to generate the different camera angles around the
    .pcd file. It will pass these views (poses) to the file findPoints.cpp

    Program stucture:

    class{
        .
        .
        .
        .
            Generates the views around the .pcd as poses
        .
        .
        .
        for(){
            - goes through the list of poses as passes them over to
                the findPoints.cpp
            - A parrallel array will be made to hold the value this class will
                return

        }

        .
        .
        .
        .
        TBD

    }

*/

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <ros/package.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

#define PI 3.141592653589793238462643383279502884197169
#define CAMERA_HEIGHT 0;

//count is a varuiable used to make sure that the node only generates the poses once
bool first_time = false;
//A two dimensional vector used to hold the poses generated around the object
std::vector<std::vector<geometry_msgs::Pose> > pose_2Dcontainer;

geometry_msgs::Point center;

void callback(const PointCloud::ConstPtr& msg)
{

    if(!first_time)
    {
    	int numPoints = 0;
    	printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);

    	/*The radius of the hypothetical circle around teh center of the point cloud where
		  generate our poses															*/
    	int circle_radius = (msg->width * 1.5);

    	//Finds the center of the PointCloud
    	BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
    	{
    		center.x += pt.x;
    		center.y += pt.y;
    		center.z += pt.z;
    		numPoints++;
    	}
    		center.x /= numPoints;
    		center.y /= numPoints;
    		center.z /= numPoints;

    	/*
    	Looping structure to create the poses and load them into a two dimmensional list
					GENERATING THE POSES
						1) Constant z
						2) Constant difference from center of PointCloud
						3) Stored in a 2D vector
							-- Each row is a different coordinate position of the pose
							-- Each column is a different orientation (angle) of the pose
		*/
    	//Finds the points of the corcel around the center of the object
    	for (int theta = 0; theta < 2*PI; theta += 10){
    		float x = circle_radius * cos(theta);
    		float y = circle_radius * sin(theta);
    		std::vector<geometry_msgs::Pose> new_coordinate_pose;
    		//for loop for the 5 different orientations of the poses
    		for(int pitch_angle = 30; pitch_angle <= 150; pitch_angle += 30){
    			geometry_msgs::Pose createdPoint;
    			createdPoint.position.x = x;
    			createdPoint.position.y = y;
    			createdPoint.position.z = CAMERA_HEIGHT;
    			createdPoint.orientation.w = 0;
    			createdPoint.orientation.x = 0;
    			createdPoint.orientation.y = sin(.5 * pitch_angle);
    			createdPoint.orientation.z = sin(.5 * theta); //replace with degree change to look at the center

    			//Add the calculated pose to the array of poses
    			new_coordinate_pose.push_back(createdPoint);
    		}
    		//Add the vector of poses containing the same position but different orientations
    		pose_2Dcontainer.push_back(new_coordinate_pose);
    	}

    	first_time = true;
    }

    ROS_INFO("Center x = %f,  y = %f,  z = %f \n", center.x, center.y, center.z);

    /*
    BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
        printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
    */
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "generate_views");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<PointCloud>("cloud", 1, callback);
    ros::spin();
}

<|endoftext|>


<commit_before>/**
 * WordSenseProcessor.cc
 *
 * Top-leve word-sense disambiguation entry point.
 *
 * XXX Currently, this is very crude scaffolding to interface
 * to the opencog server. It needs to be replaced/expanded as
 * appropriate.
 *
 * Copyright (c) 2008 Linas Vepstas <linas@linas.org>
 */
#include <stdio.h>

#include "AtomSpace.h"
#include "CogServer.h"
#include "Foreach.h"
#include "Link.h"
#include "MindAgent.h"
#include "Node.h"
#include "WordSenseProcessor.h"

using namespace opencog;

// ----------------------------------------
WordSenseProcessor::WordSenseProcessor(void)
{
	cnt = 0;
}

WordSenseProcessor::~WordSenseProcessor()
{
	atom_space = NULL;
}

void WordSenseProcessor::run(CogServer *server)
{
	atom_space = server->getAtomSpace();

	// Look for recently entered text
	atom_space->foreach_handle_of_type("SentenceNode",
	               &WordSenseProcessor::do_sentence, this);

	/* XXX HACK ALERT -- no scheduling, so just sleep */
	usleep(1000000);  // 1 second
	// usleep(10000);  // 10 millisecs == 100HZ
// printf("hellow wrold\n");
}

/**
 * Process a sentence fed into the system.
 */
bool WordSenseProcessor::do_sentence(Handle h)
{
	cnt++;
	printf ("duuuude found sentence %d handle=%lx\n", cnt, (unsigned long) h);

	Node node(CONCEPT_NODE, "#WSD_completed");
	completion_handle = atom_space->addRealAtom(node);

printf("duude donehand=%x\n", (unsigned long) completion_handle);

	// fl.follow_binary_link(at, INHERITANCE_LINK)
	bool found = foreach_incoming_handle(h, &WordSenseProcessor::check_done, this);

printf("duude found = %d\n", found);

	// Mark as completed
	std::vector<Handle> out;
	out.push_back(h);
	out.push_back(completion_handle);

	atom_space->addLink(INHERITANCE_LINK, out);

	return false;
}

bool WordSenseProcessor::check_done(Handle h)
{
	printf ("duude check=%x\n", (unsigned long) h);
	return false;
}

/* ======================= END OF FILE ==================== */
<commit_msg>misc hacking<commit_after>/**
 * WordSenseProcessor.cc
 *
 * Top-leve word-sense disambiguation entry point.
 *
 * XXX Currently, this is very crude scaffolding to interface
 * to the opencog server. It needs to be replaced/expanded as
 * appropriate.
 *
 * Copyright (c) 2008 Linas Vepstas <linas@linas.org>
 */
#include <stdio.h>

#include "AtomSpace.h"
#include "CogServer.h"
#include "Foreach.h"
#include "ForeachChaseLink.h"
#include "Link.h"
#include "MindAgent.h"
#include "Node.h"
#include "WordSenseProcessor.h"

using namespace opencog;

// ----------------------------------------
WordSenseProcessor::WordSenseProcessor(void)
{
	cnt = 0;
}

WordSenseProcessor::~WordSenseProcessor()
{
	atom_space = NULL;
}

void WordSenseProcessor::run(CogServer *server)
{
	atom_space = server->getAtomSpace();

	// Look for recently entered text
	atom_space->foreach_handle_of_type("SentenceNode",
	               &WordSenseProcessor::do_sentence, this);

	/* XXX HACK ALERT -- no scheduling, so just sleep */
	usleep(1000000);  // 1 second
	// usleep(10000);  // 10 millisecs == 100HZ
// printf("hellow wrold\n");
}

/**
 * Process a sentence fed into the system.
 */
bool WordSenseProcessor::do_sentence(Handle h)
{
	cnt++;
	printf ("duuuude found sentence %d handle=%lx\n", cnt, (unsigned long) h);

	Node node(CONCEPT_NODE, "#WSD_completed");
	completion_handle = atom_space->addRealAtom(node);

printf("duude donehand=%x\n", (unsigned long) completion_handle);

	// fl.follow_binary_link(at, INHERITANCE_LINK)
	// bool found = foreach_incoming_handle(h, &WordSenseProcessor::check_done, this);

	ForeachChaseLink<WordSenseProcessor> lc;
	lc.follow_link(h, INHERITANCE_LINK, 1, 0, &WordSenseProcessor::check_done, this);

// printf("duude found = %d\n", found);

	// Mark as completed
	std::vector<Handle> out;
	out.push_back(h);
	out.push_back(completion_handle);

	atom_space->addLink(INHERITANCE_LINK, out);

	return false;
}

bool WordSenseProcessor::check_done(Handle h)
{
	printf ("duude check=%x\n", (unsigned long) h);
	return false;
}

/* ======================= END OF FILE ==================== */
<|endoftext|>


<commit_before>/*********************************************************************************
 * Nelder-Mead Optimierungsalgorithmus.                                          *
 * Autor: Sonja Biedermann, a1402891 (Gruppe 4)                                  *
 *                                                                               *
 * Minimiert eine gegebene Funktion f.                                           *
 * Der Kontrollfluss liegt hierbei außerhalb des Optimierers – die               *
 * aufrufende Funktion trägt die Verantwortung, die step() Methode               *
 * aufzurufen, bis ein Minimum approximiert wurde. Das ermöglicht                *
 * das leichte Einsetzen von unterschiedlichen Tools zum Visualisieren des       *
 * Optimierungsvorganges.                                                        *
 *                                                                               *
 * Als Referenzen wurden folgende Quellen verwendet:                             *
 * http://mathfaculty.fullerton.edu/mathews/n2003/neldermead/NelderMeadProof.pdf *
 * https://en.wikipedia.org/wiki/Nelder%E2%80%93Mead_method                      *
 *********************************************************************************/

#pragma once
#include <cassert>
#include <cmath>
#include <iostream>
#include <tuple>
#include <sstream>
#include <exception>

#include "Funktion.h"
#include "point.hpp"

struct invalid_value: public std::exception {
    char const* err = "invalid value";
    invalid_value(char const* e) : err{ e } {}
    char const* what() const noexcept { return err; }
};

class nelder_mead_optimizer {
private:
    Funktion& f;
    point b; // best
    point g; // good
    point w; // worst
    double eps;

    double alpha_; // Alpha: Reflexionsfaktor, default: 1
    double gamma_; // Gamma: Expansionsfaktor, default: 2
    double beta_;  // Rho: Kontrahierungsfaktor, default: .5
    double delta_; // Komprimierungsfaktor, default: .5

    bool is_done = false;

    point& min(point& p1, point& p2) {
        return f(p1.x, p1.y) < f(p2.x, p2.y) ? p1 : p2;
    }

    point& min(point& p1, point& p2, point& p3) {
        double y1 = f(p1.x, p1.y);
        double y2 = f(p2.x, p2.y);
        double y3 = f(p3.x, p3.y);
        return y1 < y2 ? (y1 < y3 ? p1 : p3) : (y2 < y3 ? p2 : p3);
    }

    void sort_points_by_fvalue() {
        std::swap(b, min(b, g, w));
        std::swap(g, min(g, w));
    }

    void do_step() {
        point m = (b + g) / 2; // Mittelpunkt der besten beiden Punkte
        point r = m + alpha_ * (m - w); // Reflektiere schlechtesten Punkt

        if(f(r.x, r.y) < f(w.x, w.y)) {
            if(f(r.x, r.y) < f(b.x, b.y)) {
                point e = m + gamma_ * (m - w); // r ist bester Punkt bis jetzt. Expandiere weiter
                w = min(e, r); // Ersetze schlechtesten Punkt durch den besseren der beiden
                return;
            }
            w = r;
            return;
        }

        point c; // Kontrahierter Punkt
        if(f(r.x, r.y) < f(w.x, w.y)) { // Outside: r ist besser als w, also wollen wir
            c = m + beta_ * (r - m);    // den kontrahierten Punkt, der näher an r liegt
            if(f(c.x, c.y) < f(r.x, r.y)) {
                w = c;
                return;
            }
        } else { // Inside: R ist schlechter als w, also wählen wir den Punkt, der näher an w liegt
            c = m + beta_ * (w - m);
            if(f(c.x, c.y) < f(w.x, w.y)) {
                w = c;
                return;
            }
        }

        // Komprimiere die zwei schlechteren Punkte in Richtung b
        g = b + delta_ * (g - b);
        w = b + delta_ * (w - b);
    }

public:
    nelder_mead_optimizer(Funktion& f, point const& p1, point const& p2, point const& p3, double eps = 0.00001,
            double alpha = 1, double gamma = 2, double beta = .5, double delta = .5)
        : f( f ), b{ p1 }, g{ p2 }, w{ p3 }, eps{ eps } {
            sort_points_by_fvalue();
            set_alpha(alpha);
            set_gamma(gamma);
            set_beta(beta);
            set_delta(delta);
        }

    double alpha() const { return alpha_; }
    double gamma() const { return gamma_; }
    double beta() const  { return beta_; }
    double delta() const { return delta_; }

    void set_alpha(double alpha) {
        if(alpha <= 0) {
            throw invalid_value("alpha value violates constraint alpha > 0");
        }
        alpha_ = alpha;
    }

    void set_gamma(double gamma) {
        if(gamma <= 1) {
            throw invalid_value("gamma value violates constraint gamma > 1");
        } else if(gamma < alpha_) {
            throw invalid_value("gamma value violates constraint gamma > alpha");
        }
        gamma_ = gamma;
    }

    void set_beta(double beta) {
        if(!(0 < beta && beta < 1)) {
            throw invalid_value("beta value violates constraint 0 < beta < 1");
        }
        beta_ = beta;
    }

    void set_delta(double delta) {
        if(!(0 < delta && delta < 1)) {
            throw invalid_value("delta value violates constraint 0 < delta < 1");
        }
        delta_ = delta;
    }

    point get_best_point() const {
        return std::get<0>(retrieve_current_simplex());
    }

    std::tuple<point, point, point> retrieve_current_simplex() const {
        return std::make_tuple(b, g, w);
    }

    bool done() const { return is_done; }

    void step() {
        if(is_done) { return; }
        do_step();
        sort_points_by_fvalue();

        /* Konvergenzkriterium: Differenz zwischen bestem und schlechtetestem Punkt
         * erreicht oder unterschreitet Epsilon. */
        if(std::abs(f(b.x, b.y) - f(w.x, w.y)) <= eps) { is_done = true; }
    }

    void optimize() {
        while(!is_done) { step(); }
    }
};
/* vim: set ts=4 sw=4 tw=0 et :*/
<commit_msg>reorder<commit_after>/*********************************************************************************
 * Nelder-Mead Optimierungsalgorithmus.                                          *
 * Autor: Sonja Biedermann, a1402891 (Gruppe 4)                                  *
 *                                                                               *
 * Minimiert eine gegebene Funktion f.                                           *
 * Der Kontrollfluss liegt hierbei außerhalb des Optimierers – die               *
 * aufrufende Funktion trägt die Verantwortung, die step() Methode               *
 * aufzurufen, bis ein Minimum approximiert wurde. Das ermöglicht                *
 * das leichte Einsetzen von unterschiedlichen Tools zum Visualisieren des       *
 * Optimierungsvorganges.                                                        *
 *                                                                               *
 * Als Referenzen wurden folgende Quellen verwendet:                             *
 * http://mathfaculty.fullerton.edu/mathews/n2003/neldermead/NelderMeadProof.pdf *
 * https://en.wikipedia.org/wiki/Nelder%E2%80%93Mead_method                      *
 *********************************************************************************/

#pragma once
#include <cassert>
#include <cmath>
#include <iostream>
#include <tuple>
#include <sstream>
#include <exception>

#include "Funktion.h"
#include "point.hpp"

struct invalid_value: public std::exception {
    char const* err = "invalid value";
    invalid_value(char const* e) : err{ e } {}
    char const* what() const noexcept { return err; }
};

class nelder_mead_optimizer {
private:
    Funktion& f;
    point b; // best
    point g; // good
    point w; // worst
    double eps;

    double alpha_; // Alpha: Reflexionsfaktor, default: 1
    double gamma_; // Gamma: Expansionsfaktor, default: 2
    double beta_;  // Rho: Kontrahierungsfaktor, default: .5
    double delta_; // Komprimierungsfaktor, default: .5

    bool is_done = false;

    point& min(point& p1, point& p2) {
        return f(p1.x, p1.y) < f(p2.x, p2.y) ? p1 : p2;
    }

    point& min(point& p1, point& p2, point& p3) {
        double y1 = f(p1.x, p1.y);
        double y2 = f(p2.x, p2.y);
        double y3 = f(p3.x, p3.y);
        return y1 < y2 ? (y1 < y3 ? p1 : p3) : (y2 < y3 ? p2 : p3);
    }

    void sort_points_by_fvalue() {
        std::swap(b, min(b, g, w));
        std::swap(g, min(g, w));
    }

    void do_step() {
        point m = (b + g) / 2; // Mittelpunkt der besten beiden Punkte
        point r = m + alpha_ * (m - w); // Reflektiere schlechtesten Punkt

        if(f(r.x, r.y) < f(w.x, w.y)) {
            if(f(r.x, r.y) < f(b.x, b.y)) {
                point e = m + gamma_ * (m - w); // r ist bester Punkt bis jetzt. Expandiere weiter
                w = min(e, r); // Ersetze schlechtesten Punkt durch den besseren der beiden
                return;
            }
            w = r;
            return;
        }

        point c; // Kontrahierter Punkt
        if(f(r.x, r.y) < f(w.x, w.y)) { // Outside: r ist besser als w, also wollen wir
            c = m + beta_ * (r - m);    // den kontrahierten Punkt, der näher an r liegt
            if(f(c.x, c.y) < f(r.x, r.y)) {
                w = c;
                return;
            }
        } else { // Inside: R ist schlechter als w, also wählen wir den Punkt, der näher an w liegt
            c = m + beta_ * (w - m);
            if(f(c.x, c.y) < f(w.x, w.y)) {
                w = c;
                return;
            }
        }

        // Komprimiere die zwei schlechteren Punkte in Richtung b
        g = b + delta_ * (g - b);
        w = b + delta_ * (w - b);
    }

public:
    nelder_mead_optimizer(Funktion& f, point const& p1, point const& p2, point const& p3, double eps = 0.00001,
            double alpha = 1, double gamma = 2, double beta = .5, double delta = .5)
        : f( f ), b{ p1 }, g{ p2 }, w{ p3 }, eps{ eps } {
            sort_points_by_fvalue();
            set_alpha(alpha);
            set_gamma(gamma);
            set_beta(beta);
            set_delta(delta);
        }

    double alpha() const { return alpha_; }
    double gamma() const { return gamma_; }
    double beta() const  { return beta_; }
    double delta() const { return delta_; }

    point get_best_point() const { return b; }

    bool done() const { return is_done; }

    std::tuple<point, point, point> retrieve_current_simplex() const {
        return std::make_tuple(b, g, w);
    }

    void set_alpha(double alpha) {
        if(alpha <= 0) {
            throw invalid_value("alpha value violates constraint alpha > 0");
        }
        alpha_ = alpha;
    }

    void set_gamma(double gamma) {
        if(gamma <= 1) {
            throw invalid_value("gamma value violates constraint gamma > 1");
        } else if(gamma < alpha_) {
            throw invalid_value("gamma value violates constraint gamma > alpha");
        }
        gamma_ = gamma;
    }

    void set_beta(double beta) {
        if(!(0 < beta && beta < 1)) {
            throw invalid_value("beta value violates constraint 0 < beta < 1");
        }
        beta_ = beta;
    }

    void set_delta(double delta) {
        if(!(0 < delta && delta < 1)) {
            throw invalid_value("delta value violates constraint 0 < delta < 1");
        }
        delta_ = delta;
    }

    void step() {
        if(is_done) { return; }
        do_step();
        sort_points_by_fvalue();

        /* Konvergenzkriterium: Differenz zwischen bestem und schlechtetestem Punkt
         * erreicht oder unterschreitet Epsilon. */
        if(std::abs(f(b.x, b.y) - f(w.x, w.y)) <= eps) { is_done = true; }
    }

    void optimize() { while(!is_done) { step(); } }
};
/* vim: set ts=4 sw=4 tw=0 et :*/
<|endoftext|>