<commit_before>/*=============================================================================
	Copyright (c) 2012-2013 Richard Otis

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

// equilibrium.hpp -- declaration for Equilibrium object

/*
 * What this class needs to do:
 * This class will be the foundational piece of how TDBread interacts with
 * libGibbs. Code will construct a Database (using libTDB) and set conditions
 * using an evalconditions object. They will then pass those parameters to an
 * Equilibrium object. The ctor will minimize the Gibbs energy of the system
 * and find the hypersurface corresponding to the n-phase equilibrium.
 * I will move a lot of the evaluate.cpp code into the Equilibrium object.
 * Once minimization is complete, I will have the values of the phase fractions
 * and sublattice site fractions which minimize the Gibbs energy.
 * For convenience in debugging, I will overload an operator to make Equilibrium
 * objects prettyprint in a Thermo-Calc style way.
 * Exception handling will have to be done carefully since a lot of numerical
 * stuff will be happening in the ctor.
 *
 * Equilibrium objects will contain:
 * 1) the name of the database used (for compatibility checking)
 * 2) the value of the minimum Gibbs energy (J/mol)
 * 3) the evalconditions object used for minimization
 * 3) For each phase:
 * 		a) name of phase
 * 		b) fraction of phase
 * 		c) For each sublattice: the stoichiometric coefficient
 * 		d) For each sublattice: the site fraction of each species
 *
 * Equilibrium objects will have:
 * 1) Gibbs minimization for m species and n phases
 * 2) Convenience function for converting site fraction to mole fraction (phase and overall)
 * 3) prettyprint functionality by overloading insertion operator
 * 4) getter functions for the conditions of the equilibrium
 * 4) (FUTURE) the ability to be constructed from arbitrary (e.g. experimental) data
 */
class Equilibrium {

};
<commit_msg>Sketch of the Equilibrium data structure, and some musings on possibly incorporating Boost MultiIndex.<commit_after>/*=============================================================================
	Copyright (c) 2012-2013 Richard Otis

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

// equilibrium.hpp -- declaration for Equilibrium object

#include <string>
#include "libtdb/include/conditions.hpp"

/*
 * What this class needs to do:
 * This class will be the foundational piece of how TDBread interacts with
 * libGibbs. Code will construct a Database (using libTDB) and set conditions
 * using an evalconditions object. They will then pass those parameters to an
 * Equilibrium object. The ctor will minimize the Gibbs energy of the system
 * and find the hypersurface corresponding to the n-phase equilibrium.
 * I will move a lot of the evaluate.cpp code into the Equilibrium object.
 * Once minimization is complete, I will have the values of the phase fractions
 * and sublattice site fractions which minimize the Gibbs energy.
 * For convenience in debugging, I will overload an operator to make Equilibrium
 * objects prettyprint in a Thermo-Calc style way.
 * Exception handling will have to be done carefully since a lot of numerical
 * stuff will be happening in the ctor.
 *
 * THOUGHT: Is Boost MultiIndex my salvation here?
 *
 * Equilibrium objects will contain:
 * 1) the name of the database used (for compatibility checking)
 * 2) the value of the minimum Gibbs energy (J/mol)
 * 3) the evalconditions object used for minimization
 * 3) For each phase:
 * 		a) name of phase
 * 		b) fraction of phase
 * 		c) For each sublattice: the stoichiometric coefficient
 * 		d) For each sublattice: the site fraction of each species
 *
 * Equilibrium objects will have:
 * 1) Gibbs minimization for m species and n phases
 * 2) Convenience function for converting site fraction to mole fraction (phase and overall)
 * 3) prettyprint functionality by overloading insertion operator
 * 4) getter functions for the conditions of the equilibrium
 * 4) (FUTURE) the ability to be constructed from arbitrary (e.g. experimental) data
 */

class Equilibrium {
private:
	const std::string dbname;
	const double mingibbs;
	const evalconditions conditions;
	struct sitefraction_entry {
		std::string phase_name;
		double phase_frac;
		double subl_stoicoef;
		std::string spec_name;
		double spec_sitefrac;
	};
};
<|endoftext|>


<commit_before>#ifdef COMPONENT_PARSER_H
#ifndef COMPONENT_PARSER_IO_H
#define COMPONENT_PARSER_IO_H

namespace IOhandler {
	Token write(Token);

	Token read(tokenType);
	Token get();
	Token readLine();
};

#endif /* COMPONENT_PARSER_IO_H */
#endif /* COMPONENT_PARSER_H */
<commit_msg>IO Functions<commit_after>#ifdef COMPONENT_PARSER_H
#ifndef COMPONENT_PARSER_IO_H
#define COMPONENT_PARSER_IO_H

namespace IOhandler {
	Token write(Token);

	Token read(tokenType);
	Token get();
	Token readLine();
};

Token IOhandler::write(Token t1) {
    if(t1.subtype() == NUMBER || t1.subtype() == BOOLEAN ) {
        for(int i = 0;i < t1.value().length();i++) cout<<t1.value()[i];
        return Lexer::toToken("true");
    }
    if(t1.subtype() == STRING) {
        for(int i = 1;i < t1.value().length() - 1;i++) cout<<t1.value()[i];
        return Lexer::toToken("true");
    }
    return Lexer::toToken("false");
}

Token IOhandler::read(tokenType t1) {
    if(t1 == NUMBER) {
        double number;
        cin>>number;
        String s1 = numberToString(number);
        return Lexer::toToken(s1);
    }
    if(t1 == STRING) {
        String s1,s2("\"");
        char A[MAX_STRING_LENGTH];
        cin.getline(A,MAX_STRING_LENGTH);
        s1 = A;
        s1 = s2 + s1 + s2;
        return Lexer::toToken(s1);
    }
    if(t1 == BOOLEAN) {
        String s1;
        char A[MAX_STRING_LENGTH];
        cin.getline(A,MAX_STRING_LENGTH);
        s1 = A;
        if(s1 != "true" && s1 != "false") return nullToken;
        return Lexer::toToken(s1);
    }
}

Token IOhandler::get() {
    char c;
    cin>>c;
    String s1(c);
    Token t1 = Lexer::toToken(s1);
    return t1;
}

Token IOhandler::readLine() {
    String s1;
    char c;
    c = cin.get();
    while(c != '\n'){
        s1 += c;
        c = cin.get();
    }
    return Lexer::toToken(s1);
}

#endif /* COMPONENT_PARSER_IO_H */
#endif /* COMPONENT_PARSER_H */
<|endoftext|>


<commit_before>// MGenTest.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "MGenTest.h"
#include "../MGen/GLibrary/GLib.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

// The one and only application object

CWinApp theApp;

using namespace std;

CString current_dir, full_url, server, url;
DWORD service;
WORD port;
ofstream logfile;
int ci = 0;
int nRetCode = 0;

/*
string url_encode(const string &value) {
	ostringstream escaped;
	escaped.fill('0');
	escaped << hex;

	for (string::const_iterator i = value.begin(), n = value.end(); i != n; ++i) {
		string::value_type c = (*i);

		// Keep alphanumeric and other accepted characters intact
		if (isalnum(c) || c == '-' || c == '_' || c == '.' || c == '~') {
			escaped << c;
			continue;
		}

		// Any other characters are percent-encoded
		escaped << uppercase;
		escaped << '%' << setw(2) << int((unsigned char)c);
		escaped << nouppercase;
	}

	return escaped.str();
}

void HTTPPost(CString server, WORD port, CString url, CString data) {
	cout << "HTTPPost to server " << server << " port " << port << " url " << url << " : " << data << "\n";
	CString strHeaders = "Content-Type: application/x-www-form-urlencoded";
	try {
		char szBuf [2550];
		DWORD dwRet;
		CInternetSession session;
		CHttpConnection* pConnection = session.GetHttpConnection(server, port);
		CHttpFile* pFile = pConnection->OpenRequest(CHttpConnection::HTTP_VERB_POST, url);
		pFile->AddRequestHeaders(strHeaders);
		data = url_encode(data.GetBuffer()).c_str();
		cout << "HTTPPost to server " << server << " port " << port << " url " << url << " : " << data << "\n";
		pFile->SendRequestEx(data.GetLength());
		pFile->Write(data, data.GetLength());
		//BOOL result = pFile->SendRequest(strHeaders, (LPVOID)(LPCTSTR)data, data.GetLength());
		pFile->QueryInfoStatusCode(dwRet);
		cout << "HTTP return code: " << dwRet << "\n";
		if (dwRet == HTTP_STATUS_OK) {
			UINT nRead = pFile->Read(szBuf, 2550);
			if (nRead > 0) {
				cout << "HTTP result: " << szBuf << "\n";
			}
		}
	}
	catch (CInternetException *e) {
		//e->ReportError();
		TCHAR   szCause[2550];
		e->GetErrorMessage(szCause, 2550);
		cout << "Error when sending HTTP request: " << szCause << "\n";
		e->Delete();
	}
}
*/

void Run(CString fname, CString par, int delay) {
	SHELLEXECUTEINFO sei = { 0 };
	sei.cbSize = sizeof(SHELLEXECUTEINFO);
	sei.fMask = SEE_MASK_NOCLOSEPROCESS | SEE_MASK_FLAG_NO_UI;
	sei.hwnd = NULL;
	sei.lpVerb = NULL;
	sei.lpFile = fname;
	sei.lpParameters = par;
	sei.lpDirectory = NULL;
	sei.nShow = SW_SHOWNORMAL;
	sei.hInstApp = NULL;
	ShellExecuteEx(&sei);
	WaitForSingleObject(sei.hProcess, delay);
}

void Log(CString st, int level = 0) {
	cout << st;
	logfile << st;
	if (ci && level > 0) {
		CString cat;
		if (level == 1) cat = "Information";
		if (level == 2) cat = "Warning";
		if (level == 3) cat = "Error";
		CString par = "AddMessage \"" + st + "\" -Category " + cat;
		Run("appveyor", par, 1000);
	}
}

CString file(CString fname) {
	CString st, st2;
	ifstream fs;
	// Check file exists
	if (!CGLib::fileExists(fname)) {
		cout << "Not found file " << fname << "\n";
	}
	fs.open(fname);
	char pch[2550];
	while (fs.good()) {
		// Get line
		fs.getline(pch, 2550);
		st2 = pch;
		if (st2 != "") {
			st += "- " + st2 + "\n";
		}
	}
	fs.close();
	return st;
}

void ClearBuffer() {
	fstream fs;
	fs.open("autotest\\buffer.log", ios::out);
	fs.close();
}

void PublishTest(CString tname, int result, int tpassed) {
	CString st;
	CString st2;
	st2.Format("%s: code %d in %d ms\n", tname, result, tpassed);
	if (result) {
		nRetCode = 2;
		Log(st2, 3);
	}
	else {
		Log(st2, 1);
	}

	// Show errors
	CString errors = file("autotest/buffer.log");
	cout << errors;

	if (ci) {
		errors.Replace("\n- ", "<br> ");
		CString cat = "Passed";
		if (result) cat = "Failed";
		st.Format("UpdateTest \"%s\" -Framework MSTest -FileName MGen.exe -Duration %d -Outcome %s -ErrorMessage \"%d. %s\"", tname, tpassed, cat, result, errors);
		Run("appveyor", st, 1000);
		st.Format("UpdateTest \"%s\" -StdErr \"%s\"", tname, errors);
		Run("appveyor", st, 1000);
	}

	/*
	// Send HTTP
	st.Format("{"
		" \"testName\": \"%s\","
		" \"testFramework\" : \"MSTest\","
		" \"fileName\" : \"MGen.exe\","
		" \"outcome\" : \"%s\","
		" \"durationMilliseconds\" : \"%d\","
		" \"ErrorMessage\" : \"%s\","
		" \"ErrorStackTrace\" : \"\","
		" \"StdOut\" : \"\","
		" \"StdErr\" : \"\""
		" }", tname, cat, tpassed, errors);
	if (ci) {
		HTTPPost(server, port, url + "api/tests", st);
	}
	*/
}

void LoadConfig() {
	milliseconds time_start, time_stop;
	CString fname = "autotest\\test.txt";
	// Check file exists
	if (!CGLib::fileExists(fname)) {
		cout << "Not found file " << fname << "\n";
	}
	ifstream fs;
	fs.open(fname);
	LPDWORD ecode = new DWORD;
	CString st, st2;
	char pch[2550];
	int pos = 0;
	int passed;
	while (fs.good()) {
		fs.getline(pch, 2550);
		st = pch;
		pos = st.Find("#");
		if (pos != -1) st = st.Left(pos);
		st.Trim();
		if (st.GetLength()) {
			ClearBuffer();
			if (ci) Run("appveyor", "AddTest \"" + st + "\" -Framework MSTest -FileName MGen.exe -Outcome Running", 1000);
			Log("Starting test config: " + st + "\n");
			//::ShellExecute(GetDesktopWindow(), "open", "MGen", "-test " + st, NULL, SW_SHOWNORMAL);
			st2 = "-test " + st;
			SHELLEXECUTEINFO sei = { 0 };
			sei.cbSize = sizeof(SHELLEXECUTEINFO);
			sei.fMask = SEE_MASK_NOCLOSEPROCESS;
			sei.hwnd = NULL;
			sei.lpVerb = NULL;
			sei.lpFile = "MGen.exe";
			sei.lpParameters = st2;
			sei.lpDirectory = NULL;
			sei.nShow = SW_SHOW;
			sei.hInstApp = NULL;
			time_start = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
			ShellExecuteEx(&sei);
			if (WaitForSingleObject(sei.hProcess, 60000) == WAIT_TIMEOUT) {
				Log(st + ": Timeout waiting for process\n", 3);
				exit(1);
			}

			time_stop = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
			passed = static_cast<int>((time_stop - time_start).count());
			GetExitCodeProcess(sei.hProcess, ecode);
			PublishTest(st, *ecode, passed);
		}
	}
	delete ecode;
	fs.close();
}

int test() {
	//full_url = "http://my.test.server:882/some/path/script.php?parameters=123";
	//AfxParseURL(full_url, service, server, url, port);
	//ci = 1;
	if (getenv("APPVEYOR_PROJECT_NAME") != NULL) {
		ci = 1;
		//if (getenv("APPVEYOR_API_URL") != NULL) full_url = getenv("APPVEYOR_API_URL");
		//AfxParseURL(full_url, service, server, url, port);
	}
	logfile.open("autotest\\test.log", ios_base::app);

	TCHAR buffer[MAX_PATH];
	GetCurrentDirectory(MAX_PATH, buffer);
	current_dir = string(buffer).c_str();
	Log("Current dir: " + current_dir + "\n");

	LoadConfig();
	logfile.close();
	// Do not pause if continuous integration
	if (!ci) {
		cout << "Press any key to continue... ";
		_getch();
	}
	return 0;
}

int main()
{
    HMODULE hModule = ::GetModuleHandle(nullptr);

    if (hModule != nullptr)
    {
        // initialize MFC and print and error on failure
        if (!AfxWinInit(hModule, nullptr, ::GetCommandLine(), 0))
        {
            // TODO: change error code to suit your needs
            wprintf(L"Fatal Error: MFC initialization failed\n");
            nRetCode = 1;
        }
        else
        {
					test();
				}
    }
    else
    {
        // TODO: change error code to suit your needs
        wprintf(L"Fatal Error: GetModuleHandle failed\n");
        nRetCode = 1;
    }

    return nRetCode;
}
<commit_msg>MgenTest: Try newline2 in errors<commit_after>// MGenTest.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "MGenTest.h"
#include "../MGen/GLibrary/GLib.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

// The one and only application object

CWinApp theApp;

using namespace std;

CString current_dir, full_url, server, url;
DWORD service;
WORD port;
ofstream logfile;
int ci = 0;
int nRetCode = 0;

/*
string url_encode(const string &value) {
	ostringstream escaped;
	escaped.fill('0');
	escaped << hex;

	for (string::const_iterator i = value.begin(), n = value.end(); i != n; ++i) {
		string::value_type c = (*i);

		// Keep alphanumeric and other accepted characters intact
		if (isalnum(c) || c == '-' || c == '_' || c == '.' || c == '~') {
			escaped << c;
			continue;
		}

		// Any other characters are percent-encoded
		escaped << uppercase;
		escaped << '%' << setw(2) << int((unsigned char)c);
		escaped << nouppercase;
	}

	return escaped.str();
}

void HTTPPost(CString server, WORD port, CString url, CString data) {
	cout << "HTTPPost to server " << server << " port " << port << " url " << url << " : " << data << "\n";
	CString strHeaders = "Content-Type: application/x-www-form-urlencoded";
	try {
		char szBuf [2550];
		DWORD dwRet;
		CInternetSession session;
		CHttpConnection* pConnection = session.GetHttpConnection(server, port);
		CHttpFile* pFile = pConnection->OpenRequest(CHttpConnection::HTTP_VERB_POST, url);
		pFile->AddRequestHeaders(strHeaders);
		data = url_encode(data.GetBuffer()).c_str();
		cout << "HTTPPost to server " << server << " port " << port << " url " << url << " : " << data << "\n";
		pFile->SendRequestEx(data.GetLength());
		pFile->Write(data, data.GetLength());
		//BOOL result = pFile->SendRequest(strHeaders, (LPVOID)(LPCTSTR)data, data.GetLength());
		pFile->QueryInfoStatusCode(dwRet);
		cout << "HTTP return code: " << dwRet << "\n";
		if (dwRet == HTTP_STATUS_OK) {
			UINT nRead = pFile->Read(szBuf, 2550);
			if (nRead > 0) {
				cout << "HTTP result: " << szBuf << "\n";
			}
		}
	}
	catch (CInternetException *e) {
		//e->ReportError();
		TCHAR   szCause[2550];
		e->GetErrorMessage(szCause, 2550);
		cout << "Error when sending HTTP request: " << szCause << "\n";
		e->Delete();
	}
}
*/

void Run(CString fname, CString par, int delay) {
	SHELLEXECUTEINFO sei = { 0 };
	sei.cbSize = sizeof(SHELLEXECUTEINFO);
	sei.fMask = SEE_MASK_NOCLOSEPROCESS | SEE_MASK_FLAG_NO_UI;
	sei.hwnd = NULL;
	sei.lpVerb = NULL;
	sei.lpFile = fname;
	sei.lpParameters = par;
	sei.lpDirectory = NULL;
	sei.nShow = SW_SHOWNORMAL;
	sei.hInstApp = NULL;
	ShellExecuteEx(&sei);
	WaitForSingleObject(sei.hProcess, delay);
}

void Log(CString st, int level = 0) {
	cout << st;
	logfile << st;
	if (ci && level > 0) {
		CString cat;
		if (level == 1) cat = "Information";
		if (level == 2) cat = "Warning";
		if (level == 3) cat = "Error";
		CString par = "AddMessage \"" + st + "\" -Category " + cat;
		Run("appveyor", par, 1000);
	}
}

CString file(CString fname) {
	CString st, st2;
	ifstream fs;
	// Check file exists
	if (!CGLib::fileExists(fname)) {
		cout << "Not found file " << fname << "\n";
	}
	fs.open(fname);
	char pch[2550];
	while (fs.good()) {
		// Get line
		fs.getline(pch, 2550);
		st2 = pch;
		if (st2 != "") {
			st += "- " + st2 + "\n";
		}
	}
	fs.close();
	return st;
}

void ClearBuffer() {
	fstream fs;
	fs.open("autotest\\buffer.log", ios::out);
	fs.close();
}

void PublishTest(CString tname, int result, int tpassed) {
	CString st;
	CString st2;
	st2.Format("%s: code %d in %d ms\n", tname, result, tpassed);
	if (result) {
		nRetCode = 2;
		Log(st2, 3);
	}
	else {
		Log(st2, 1);
	}

	// Show errors
	CString errors = file("autotest/buffer.log");
	cout << errors;

	if (ci) {
		errors.Replace("\n- ", "\\n ");
		CString cat = "Passed";
		if (result) cat = "Failed";
		st.Format("UpdateTest \"%s\" -Framework MSTest -FileName MGen.exe -Duration %d -Outcome %s -ErrorMessage \"%d. %s\"", tname, tpassed, cat, result, errors);
		Run("appveyor", st, 1000);
		st.Format("UpdateTest \"%s\" -StdErr \"%s\"", tname, errors);
		Run("appveyor", st, 1000);
	}

	/*
	// Send HTTP
	st.Format("{"
		" \"testName\": \"%s\","
		" \"testFramework\" : \"MSTest\","
		" \"fileName\" : \"MGen.exe\","
		" \"outcome\" : \"%s\","
		" \"durationMilliseconds\" : \"%d\","
		" \"ErrorMessage\" : \"%s\","
		" \"ErrorStackTrace\" : \"\","
		" \"StdOut\" : \"\","
		" \"StdErr\" : \"\""
		" }", tname, cat, tpassed, errors);
	if (ci) {
		HTTPPost(server, port, url + "api/tests", st);
	}
	*/
}

void LoadConfig() {
	milliseconds time_start, time_stop;
	CString fname = "autotest\\test.txt";
	// Check file exists
	if (!CGLib::fileExists(fname)) {
		cout << "Not found file " << fname << "\n";
	}
	ifstream fs;
	fs.open(fname);
	LPDWORD ecode = new DWORD;
	CString st, st2;
	char pch[2550];
	int pos = 0;
	int passed;
	while (fs.good()) {
		fs.getline(pch, 2550);
		st = pch;
		pos = st.Find("#");
		if (pos != -1) st = st.Left(pos);
		st.Trim();
		if (st.GetLength()) {
			ClearBuffer();
			if (ci) Run("appveyor", "AddTest \"" + st + "\" -Framework MSTest -FileName MGen.exe -Outcome Running", 1000);
			Log("Starting test config: " + st + "\n");
			//::ShellExecute(GetDesktopWindow(), "open", "MGen", "-test " + st, NULL, SW_SHOWNORMAL);
			st2 = "-test " + st;
			SHELLEXECUTEINFO sei = { 0 };
			sei.cbSize = sizeof(SHELLEXECUTEINFO);
			sei.fMask = SEE_MASK_NOCLOSEPROCESS;
			sei.hwnd = NULL;
			sei.lpVerb = NULL;
			sei.lpFile = "MGen.exe";
			sei.lpParameters = st2;
			sei.lpDirectory = NULL;
			sei.nShow = SW_SHOW;
			sei.hInstApp = NULL;
			time_start = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
			ShellExecuteEx(&sei);
			if (WaitForSingleObject(sei.hProcess, 60000) == WAIT_TIMEOUT) {
				Log(st + ": Timeout waiting for process\n", 3);
				exit(1);
			}

			time_stop = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
			passed = static_cast<int>((time_stop - time_start).count());
			GetExitCodeProcess(sei.hProcess, ecode);
			PublishTest(st, *ecode, passed);
		}
	}
	delete ecode;
	fs.close();
}

int test() {
	//full_url = "http://my.test.server:882/some/path/script.php?parameters=123";
	//AfxParseURL(full_url, service, server, url, port);
	//ci = 1;
	if (getenv("APPVEYOR_PROJECT_NAME") != NULL) {
		ci = 1;
		//if (getenv("APPVEYOR_API_URL") != NULL) full_url = getenv("APPVEYOR_API_URL");
		//AfxParseURL(full_url, service, server, url, port);
	}
	logfile.open("autotest\\test.log", ios_base::app);

	TCHAR buffer[MAX_PATH];
	GetCurrentDirectory(MAX_PATH, buffer);
	current_dir = string(buffer).c_str();
	Log("Current dir: " + current_dir + "\n");

	LoadConfig();
	logfile.close();
	// Do not pause if continuous integration
	if (!ci) {
		cout << "Press any key to continue... ";
		_getch();
	}
	return 0;
}

int main()
{
    HMODULE hModule = ::GetModuleHandle(nullptr);

    if (hModule != nullptr)
    {
        // initialize MFC and print and error on failure
        if (!AfxWinInit(hModule, nullptr, ::GetCommandLine(), 0))
        {
            // TODO: change error code to suit your needs
            wprintf(L"Fatal Error: MFC initialization failed\n");
            nRetCode = 1;
        }
        else
        {
					test();
				}
    }
    else
    {
        // TODO: change error code to suit your needs
        wprintf(L"Fatal Error: GetModuleHandle failed\n");
        nRetCode = 1;
    }

    return nRetCode;
}
<|endoftext|>


<commit_before>/*
 * MiTACCSC.cpp
 *
 *  Created on: 2015年12月15日
 *      Author: nemo
 */

#include "MiTACCSC.h"

#include <iostream>
using namespace std;

MiTACCSC::MiTACCSC() {
	// TODO Auto-generated constructor stub

}

MiTACCSC::~MiTACCSC() {
	// TODO Auto-generated destructor stub
}

void MiTACCSC::runFun()
{
	if(funPtr != NULL)
	{
		*funPtr;
	}
	else
	{
		cout << "funPtr is NULL." << endl;
	}
}
<commit_msg>Marked run function action.<commit_after>/*
 * MiTACCSC.cpp
 *
 *  Created on: 2015年12月15日
 *      Author: nemo
 */

#include "MiTACCSC.h"

#include <iostream>
using namespace std;

MiTACCSC::MiTACCSC() {
	// TODO Auto-generated constructor stub

}

MiTACCSC::~MiTACCSC() {
	// TODO Auto-generated destructor stub
}

void MiTACCSC::runFun()
{
	if(funPtr != NULL)
	{
//		*funPtr;
	}
	else
	{
		cout << "funPtr is NULL." << endl;
	}
}
<|endoftext|>


<commit_before>/*
 Copyright 2016 Nervana Systems Inc.
 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.
 You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

 Unless required by applicable law or agreed to in writing, software
 distributed under the License is distributed on an "AS IS" BASIS,
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.
*/

#include <sys/stat.h>

#include <cassert>
#include <sstream>

#include "batch_file_loader.hpp"

using namespace std;

BatchFileLoader::BatchFileLoader(shared_ptr<Manifest> manifest, uint subsetPercent)
: _manifest(manifest), _subsetPercent(subsetPercent) {
    assert(_subsetPercent >= 0 && _subsetPercent <= 100);
}

void BatchFileLoader::loadBlock(BufferPair& dest, uint block_num, uint block_size) {
    // NOTE: thread safe so long as you aren't modifying the manifest
    // NOTE: dest memory must already be allocated at the correct size
    // NOTE: end_i - begin_i may not be a full block for the last
    // block_num

    size_t begin_i = block_num * block_size;
    size_t end_i = min((block_num + 1) * (size_t)block_size, _manifest->getSize());

    if (_subsetPercent != 100) {
        // adjust end_i in relation to begin_i.  We want to scale (end_i
        // - begin_i) by _subsetPercent.  In the case of a smaller block
        // than block_size (in the last block), we want _subsetPercent
        // of them so we need to make sure we first shorten the end_i to
        // the corrent smaller block size, and then scale that.
        end_i = begin_i + (((end_i - begin_i) * _subsetPercent) / 100);
    }

    // ensure we stay within bounds of manifest
    assert(begin_i <= _manifest->getSize());
    assert(end_i <= _manifest->getSize());

    // TODO: move index offset logic and bounds asserts into Manifest
    // interface to more easily support things like offset/limit queries.
    // It isn't obvious yet what the best interface for this will be ...
    // perhaps the manifest should know about block_num and block_size itself?
    // perhaps it should just expose an at(index) method?  Perhaps it should
    // expose a getCursor(index_begin, index_end) which more closely mirrors most
    // database query patterns (limit/offset)
    auto begin_it = _manifest->begin() + begin_i;
    auto end_it = _manifest->begin() + end_i;

    uint i = 0;
    for(auto it = begin_it; it != end_it; ++it, ++i) {
        // load both object and target files into respective buffers
        //
        // NOTE: if at some point in the future, loadFile is loading
        // files from a network like s3 it may make sense to use multiple
        // threads to make loads faster.  multiple threads would only
        // slow down reads from a magnetic disk.
        loadFile(dest.first, it->first);
        loadFile(dest.second, it->second);
    }
}

void BatchFileLoader::loadFile(Buffer* buff, const string& filename) {
    off_t size = getFileSize(filename);
    ifstream fin(filename, ios::binary);
    buff->read(fin, size);
}

off_t BatchFileLoader::getFileSize(const string& filename) {
    // ensure that filename exists and get its size

    struct stat stats;
    int result = stat(filename.c_str(), &stats);
    if (result == -1) {
        stringstream ss;
        ss << "Could not find " << filename;
        throw std::runtime_error(ss.str());
    }

    return stats.st_size;
}

uint BatchFileLoader::objectCount() {
    return _manifest->getSize();
}
<commit_msg>renaming<commit_after>/*
 Copyright 2016 Nervana Systems Inc.
 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.
 You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

 Unless required by applicable law or agreed to in writing, software
 distributed under the License is distributed on an "AS IS" BASIS,
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.
*/

#include <sys/stat.h>

#include <cassert>
#include <sstream>

#include "batch_file_loader.hpp"

using namespace std;

BatchFileLoader::BatchFileLoader(shared_ptr<Manifest> manifest, uint subsetPercent)
: _manifest(manifest), _subsetPercent(subsetPercent) {
    assert(_subsetPercent >= 0 && _subsetPercent <= 100);
}

void BatchFileLoader::loadBlock(BufferPair& dest, uint block_num, uint block_size) {
    // NOTE: thread safe so long as you aren't modifying the manifest
    // NOTE: dest memory must already be allocated at the correct size
    // NOTE: end_i - begin_i may not be a full block for the last
    // block_num

    size_t begin_i = block_num * block_size;
    size_t end_i = min((block_num + 1) * (size_t)block_size, _manifest->getSize());

    if (_subsetPercent != 100) {
        // adjust end_i in relation to begin_i.  We want to scale (end_i
        // - begin_i) by _subsetPercent.  In the case of a smaller block
        // than block_size (in the last block), we want _subsetPercent
        // of them so we need to make sure we first shorten the end_i to
        // the corrent smaller block size, and then scale that.
        end_i = begin_i + (((end_i - begin_i) * _subsetPercent) / 100);
    }

    // ensure we stay within bounds of manifest
    assert(begin_i <= _manifest->getSize());
    assert(end_i <= _manifest->getSize());

    // TODO: move index offset logic and bounds asserts into Manifest
    // interface to more easily support things like offset/limit queries.
    // It isn't obvious yet what the best interface for this will be ...
    // perhaps the manifest should know about block_num and block_size itself?
    // perhaps it should just expose an at(index) method?  Perhaps it should
    // expose a getCursor(index_begin, index_end) which more closely mirrors most
    // database query patterns (limit/offset)
    auto begin_it = _manifest->begin() + begin_i;
    auto end_it = _manifest->begin() + end_i;

    uint i = 0;
    for(auto it = begin_it; it != end_it; ++it, ++i) {
        // load both object and target files into respective buffers
        //
        // NOTE: if at some point in the future, loadFile is loading
        // files from a network like s3 it may make sense to use multiple
        // threads to make loads faster.  multiple threads would only
        // slow down reads from a magnetic disk.
        loadFile(dest.first, it->first);
        loadFile(dest.second, it->second);
    }
}

void BatchFileLoader::loadFile(Buffer* buff, const string& filename) {
    off_t size = getFileSize(filename);
    ifstream fin(filename, ios::binary);
    buff->read(fin, size);
}

off_t BatchFileLoader::getFileSize(const string& filename) {
    // ensure that filename exists and get its size

    struct stat stats;
    int result = stat(filename.c_str(), &stats);
    if (result == -1) {
        stringstream ss;
        ss << "Could not find " << filename;
        throw std::runtime_error(ss.str());
    }

    return stats.st_size;
}

uint BatchFileLoader::objectCount() {
    return _manifest->getSize();
}
<|endoftext|>


<commit_before>#pragma once

#include <matazure/algorithm.hpp>

namespace matazure { namespace expr {

	inline void conv_kenel3x3(tensor<const float, 2> ts_input, static_tensor<float, dim<3,3>> sts_kenel, tensor<float, 2> ts_output){
		auto kenel_radius = sts_kenel.shape() / 2;

		auto width = ts_input.shape()[0];
		auto height = ts_input.shape()[1];

		for(int_t j = 0; j < ts_input.shape()[1]; ++j) {
			for (int_t i = 0; i < ts_input.shape()[0]; ++i) {
				if (i > 0 && j > 0 && i < width && j < height){
					float sum = 0.0f;
					for (int_t n = 0; n < sts_kenel.shape()[1]; ++n) {
						for (int_t m = 0; m < sts_kenel.shape()[0]; ++m) {
							sum += ts_input[pointi<2>{i, j} +pointi<2>{m, n} -kenel_radius] * sts_kenel[pointi<2>{m, n}];
						}
					}
					ts_output[pointi<2>{i, j}] = sum;
				}else{
					ts_output[pointi<2>{i, j}] = 0.0f;
				}
			}
		}
	}

} }
<commit_msg>fixed<commit_after>#pragma once

#include <matazure/algorithm.hpp>

namespace matazure { namespace expr {

	template <typename _Kenel>
	inline void conv_kenel3x3(tensor<const float, 2> ts_input, _Kenel sts_kenel, tensor<float, 2> ts_output){
		auto kenel_radius = sts_kenel.shape() / 2;

		auto width = ts_input.shape()[0];
		auto height = ts_input.shape()[1];

		for(int_t j = 0; j < ts_input.shape()[1]; ++j) {
			for (int_t i = 0; i < ts_input.shape()[0]; ++i) {
				if (i > 0 && j > 0 && i < width && j < height){
					float sum = 0.0f;
					for (int_t n = 0; n < sts_kenel.shape()[1]; ++n) {
						for (int_t m = 0; m < sts_kenel.shape()[0]; ++m) {
							sum += ts_input[pointi<2>{i, j} +pointi<2>{m, n} -kenel_radius] * sts_kenel[pointi<2>{m, n}];
						}
					}
					ts_output[pointi<2>{i, j}] = sum;
				}else{
					ts_output[pointi<2>{i, j}] = 0.0f;
				}
			}
		}
	}

} }
<|endoftext|>


<commit_before>#include <iostream>
#include <string>

#include "export_gr2.h"
#include "gr2_file.h"

using namespace std;

static FbxVector4 quat_to_euler(FbxQuaternion &q)
{
	FbxAMatrix m;
	m.SetQ(q);
	return m.GetR();
}

void export_bones(FbxScene *scene, FbxNode *parent_node, GR2_skeleton *skel,
	int32_t parent_index, std::vector<FbxNode*> &fbx_bones);

static void export_bone(FbxScene *scene, FbxNode *parent_node, GR2_skeleton *skel,
	int32_t bone_index, std::vector<FbxNode*> &fbx_bones)
{
	GR2_bone &bone = skel->bones[bone_index];
	cout << "    Exporting bone: " << bone.name << endl;
	auto node = FbxNode::Create(scene, bone.name);

	node->LclTranslation.Set(FbxDouble3(bone.transform.translation[0],
		bone.transform.translation[1],
		bone.transform.translation[2]));
	if (bone.transform.flags & 0x2) {
		FbxQuaternion rotation(bone.transform.rotation[0],
			bone.transform.rotation[1],
			bone.transform.rotation[2],
			bone.transform.rotation[3]);
		node->LclRotation.Set(quat_to_euler(rotation));
	}

	FbxSkeleton *skel_attr = FbxSkeleton::Create(scene, bone.name);
	skel_attr->SetSkeletonType(FbxSkeleton::eLimbNode);
	node->SetNodeAttribute(skel_attr);

	parent_node->AddChild(node);
	fbx_bones[bone_index] = node;

	export_bones(scene, node, skel, bone_index, fbx_bones);
}

static void export_bones(FbxScene *scene, FbxNode *parent_node, GR2_skeleton *skel,
	int32_t parent_index, std::vector<FbxNode*> &fbx_bones)
{
	fbx_bones.resize(skel->bones_count);

	for (int32_t i = 0; i < skel->bones_count; ++i) {
		GR2_bone &bone = skel->bones[i];
		if (bone.parent_index == parent_index) {
			export_bone(scene, parent_node, skel, i, fbx_bones);
		}
	}
}

static void process_fbx_bones(std::vector<FbxNode*> &fbx_bones)
{
	FbxNode *ribcage = nullptr;
	for (auto it = fbx_bones.begin(); it != fbx_bones.end();) {
		if (strncmp((*it)->GetName(), "ap_", 3) == 0) {
			// Remove "ap_..." bones as they are not used in skinning.
			it = fbx_bones.erase(it);
		}
		else if (strcmp((*it)->GetName(), "Ribcage") == 0) {
			// Erase "Ribcage" bone and keep it to reinsert later.
			ribcage = *it;
			it = fbx_bones.erase(it);
		}
		else
			++it;
	}

	// Reinsert "Ribcage bone"
	if (ribcage)
		fbx_bones.insert(fbx_bones.begin() + 53, ribcage);
}

static void export_skeleton(FbxScene *scene, GR2_skeleton *skel,
	std::vector<FbxNode*> &fbx_bones)
{
	cout << "  Exporting: " << skel->name << endl;

	auto node = FbxNode::Create(scene, (string(skel->name) + "|Skel").c_str());
	node->LclRotation.Set(FbxDouble3(-90, 0, 0));
	node->LclScaling.Set(FbxDouble3(1, 1, 1));

	auto null_attr = FbxNull::Create(scene, skel->name);
	node->SetNodeAttribute(null_attr);

	scene->GetRootNode()->AddChild(node);

	export_bones(scene, node, skel, -1, fbx_bones);

	process_fbx_bones(fbx_bones);
}

static void export_skeletons(FbxScene *scene, GR2_file_info *info,
	std::vector<FbxNode*> &fbx_bones)
{
	for (int i = 0; i < info->skeletons_count; ++i) {
		export_skeleton(scene, info->skeletons[i], fbx_bones);
	}
}

void create_anim_position(FbxNode *node, FbxAnimLayer *anim_layer,
	GR2_transform_track &transform_track)
{
	GR2_curve_view view(transform_track.position_curve);

	auto curvex = node->LclTranslation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_X, true);
	curvex->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvex->KeyAdd(time);
		curvex->KeySetValue(k, view.controls()[i].x);
	}

	curvex->KeyModifyEnd();

	auto curvey = node->LclTranslation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_Y, true);
	curvey->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvey->KeyAdd(time);
		curvey->KeySetValue(k, view.controls()[i].y);
	}

	curvey->KeyModifyEnd();

	auto curvez = node->LclTranslation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_Z, true);
	curvez->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvez->KeyAdd(time);
		curvez->KeySetValue(k, view.controls()[i].z);
	}

	curvez->KeyModifyEnd();
}

void create_anim_rotation(FbxNode *node, FbxAnimLayer *anim_layer, GR2_transform_track &transform_track)
{
	GR2_curve_view view(transform_track.orientation_curve);

	std::vector<FbxVector4> rotations;
	for (auto &c : view.controls())
		rotations.push_back(quat_to_euler(FbxQuaternion(c.x, c.y, c.z, c.w)));

	auto curvex = node->LclRotation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_X, true);
	curvex->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvex->KeyAdd(time);
		curvex->KeySetValue(k, float(rotations[i][0]));
	}

	curvex->KeyModifyEnd();

	auto curvey = node->LclRotation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_Y, true);
	curvey->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvey->KeyAdd(time);
		curvey->KeySetValue(k, float(rotations[i][1]));
	}

	curvey->KeyModifyEnd();

	auto curvez = node->LclRotation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_Z, true);
	curvez->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvez->KeyAdd(time);
		curvez->KeySetValue(k, float(rotations[i][2]));
	}

	curvez->KeyModifyEnd();
}

void export_animation(FbxScene *scene, GR2_transform_track &transform_track,
	FbxAnimLayer *anim_layer)
{
	auto node = scene->FindNodeByName(transform_track.name);
	if (node) {
		create_anim_position(node, anim_layer, transform_track);
		create_anim_rotation(node, anim_layer, transform_track);
	}
}

static void export_animation(FbxScene *scene, GR2_track_group *track_group,
	FbxAnimLayer *anim_layer)
{
	for (int i = 0; i < track_group->transform_tracks_count; ++i)
		export_animation(scene, track_group->transform_tracks[i]
			, anim_layer);
}

static void export_animation(FbxScene *scene, GR2_animation *anim)
{
	auto anim_stack = FbxAnimStack::Create(scene, anim->name);
	auto anim_layer = FbxAnimLayer::Create(scene, "Layer");
	anim_stack->AddMember(anim_layer);

	for (int i = 0; i < anim->track_groups_count; ++i)
		export_animation(scene, anim->track_groups[i], anim_layer);
}

static void export_animations(FbxScene *scene, GR2_file_info *info)
{
	for (int i = 0; i < info->animations_count; ++i) {
		export_animation(scene, info->animations[i]);
	}
}

void export_gr2(const char *filename, FbxScene *scene,
	std::vector<FbxNode*> &fbx_bones)
{
	GR2_file gr2(filename);
	if (!gr2) {
		cout << gr2.error_string() << endl;
		return;
	}

	cout << endl;
	cout << "===\n";
	cout << "GR2\n";
	cout << "===\n\n";

	cout << "Skeletons: " << gr2.file_info->skeletons_count << endl;
	cout << endl;

	export_skeletons(scene, gr2.file_info, fbx_bones);
	export_animations(scene, gr2.file_info);
}<commit_msg>Change name of the exported skeleton<commit_after>#include <iostream>
#include <string>

#include "export_gr2.h"
#include "gr2_file.h"

using namespace std;

static FbxVector4 quat_to_euler(FbxQuaternion &q)
{
	FbxAMatrix m;
	m.SetQ(q);
	return m.GetR();
}

void export_bones(FbxScene *scene, FbxNode *parent_node, GR2_skeleton *skel,
	int32_t parent_index, std::vector<FbxNode*> &fbx_bones);

static void export_bone(FbxScene *scene, FbxNode *parent_node, GR2_skeleton *skel,
	int32_t bone_index, std::vector<FbxNode*> &fbx_bones)
{
	GR2_bone &bone = skel->bones[bone_index];
	cout << "    Exporting bone: " << bone.name << endl;
	auto node = FbxNode::Create(scene, bone.name);

	node->LclTranslation.Set(FbxDouble3(bone.transform.translation[0],
		bone.transform.translation[1],
		bone.transform.translation[2]));
	if (bone.transform.flags & 0x2) {
		FbxQuaternion rotation(bone.transform.rotation[0],
			bone.transform.rotation[1],
			bone.transform.rotation[2],
			bone.transform.rotation[3]);
		node->LclRotation.Set(quat_to_euler(rotation));
	}

	FbxSkeleton *skel_attr = FbxSkeleton::Create(scene, bone.name);
	skel_attr->SetSkeletonType(FbxSkeleton::eLimbNode);
	node->SetNodeAttribute(skel_attr);

	parent_node->AddChild(node);
	fbx_bones[bone_index] = node;

	export_bones(scene, node, skel, bone_index, fbx_bones);
}

static void export_bones(FbxScene *scene, FbxNode *parent_node, GR2_skeleton *skel,
	int32_t parent_index, std::vector<FbxNode*> &fbx_bones)
{
	fbx_bones.resize(skel->bones_count);

	for (int32_t i = 0; i < skel->bones_count; ++i) {
		GR2_bone &bone = skel->bones[i];
		if (bone.parent_index == parent_index) {
			export_bone(scene, parent_node, skel, i, fbx_bones);
		}
	}
}

static void process_fbx_bones(std::vector<FbxNode*> &fbx_bones)
{
	FbxNode *ribcage = nullptr;
	for (auto it = fbx_bones.begin(); it != fbx_bones.end();) {
		if (strncmp((*it)->GetName(), "ap_", 3) == 0) {
			// Remove "ap_..." bones as they are not used in skinning.
			it = fbx_bones.erase(it);
		}
		else if (strcmp((*it)->GetName(), "Ribcage") == 0) {
			// Erase "Ribcage" bone and keep it to reinsert later.
			ribcage = *it;
			it = fbx_bones.erase(it);
		}
		else
			++it;
	}

	// Reinsert "Ribcage bone"
	if (ribcage)
		fbx_bones.insert(fbx_bones.begin() + 53, ribcage);
}

static void export_skeleton(FbxScene *scene, GR2_skeleton *skel,
	std::vector<FbxNode*> &fbx_bones)
{
	cout << "  Exporting: " << skel->name << endl;

	auto node = FbxNode::Create(scene, (string(skel->name) + ".gr2").c_str());
	node->LclRotation.Set(FbxDouble3(-90, 0, 0));
	node->LclScaling.Set(FbxDouble3(1, 1, 1));

	auto null_attr = FbxNull::Create(scene, skel->name);
	node->SetNodeAttribute(null_attr);

	scene->GetRootNode()->AddChild(node);

	export_bones(scene, node, skel, -1, fbx_bones);

	process_fbx_bones(fbx_bones);
}

static void export_skeletons(FbxScene *scene, GR2_file_info *info,
	std::vector<FbxNode*> &fbx_bones)
{
	for (int i = 0; i < info->skeletons_count; ++i) {
		export_skeleton(scene, info->skeletons[i], fbx_bones);
	}
}

void create_anim_position(FbxNode *node, FbxAnimLayer *anim_layer,
	GR2_transform_track &transform_track)
{
	GR2_curve_view view(transform_track.position_curve);

	auto curvex = node->LclTranslation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_X, true);
	curvex->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvex->KeyAdd(time);
		curvex->KeySetValue(k, view.controls()[i].x);
	}

	curvex->KeyModifyEnd();

	auto curvey = node->LclTranslation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_Y, true);
	curvey->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvey->KeyAdd(time);
		curvey->KeySetValue(k, view.controls()[i].y);
	}

	curvey->KeyModifyEnd();

	auto curvez = node->LclTranslation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_Z, true);
	curvez->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvez->KeyAdd(time);
		curvez->KeySetValue(k, view.controls()[i].z);
	}

	curvez->KeyModifyEnd();
}

void create_anim_rotation(FbxNode *node, FbxAnimLayer *anim_layer, GR2_transform_track &transform_track)
{
	GR2_curve_view view(transform_track.orientation_curve);

	std::vector<FbxVector4> rotations;
	for (auto &c : view.controls())
		rotations.push_back(quat_to_euler(FbxQuaternion(c.x, c.y, c.z, c.w)));

	auto curvex = node->LclRotation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_X, true);
	curvex->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvex->KeyAdd(time);
		curvex->KeySetValue(k, float(rotations[i][0]));
	}

	curvex->KeyModifyEnd();

	auto curvey = node->LclRotation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_Y, true);
	curvey->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvey->KeyAdd(time);
		curvey->KeySetValue(k, float(rotations[i][1]));
	}

	curvey->KeyModifyEnd();

	auto curvez = node->LclRotation.GetCurve(anim_layer, FBXSDK_CURVENODE_COMPONENT_Z, true);
	curvez->KeyModifyBegin();

	for (unsigned i = 0; i < view.knots().size(); ++i) {
		if (i > 0 && view.knots()[i] <= view.knots()[i - 1]) continue;

		FbxTime time;
		time.SetSecondDouble(view.knots()[i]);
		auto k = curvez->KeyAdd(time);
		curvez->KeySetValue(k, float(rotations[i][2]));
	}

	curvez->KeyModifyEnd();
}

void export_animation(FbxScene *scene, GR2_transform_track &transform_track,
	FbxAnimLayer *anim_layer)
{
	auto node = scene->FindNodeByName(transform_track.name);
	if (node) {
		create_anim_position(node, anim_layer, transform_track);
		create_anim_rotation(node, anim_layer, transform_track);
	}
}

static void export_animation(FbxScene *scene, GR2_track_group *track_group,
	FbxAnimLayer *anim_layer)
{
	for (int i = 0; i < track_group->transform_tracks_count; ++i)
		export_animation(scene, track_group->transform_tracks[i]
			, anim_layer);
}

static void export_animation(FbxScene *scene, GR2_animation *anim)
{
	auto anim_stack = FbxAnimStack::Create(scene, anim->name);
	auto anim_layer = FbxAnimLayer::Create(scene, "Layer");
	anim_stack->AddMember(anim_layer);

	for (int i = 0; i < anim->track_groups_count; ++i)
		export_animation(scene, anim->track_groups[i], anim_layer);
}

static void export_animations(FbxScene *scene, GR2_file_info *info)
{
	for (int i = 0; i < info->animations_count; ++i) {
		export_animation(scene, info->animations[i]);
	}
}

void export_gr2(const char *filename, FbxScene *scene,
	std::vector<FbxNode*> &fbx_bones)
{
	GR2_file gr2(filename);
	if (!gr2) {
		cout << gr2.error_string() << endl;
		return;
	}

	cout << endl;
	cout << "===\n";
	cout << "GR2\n";
	cout << "===\n\n";

	cout << "Skeletons: " << gr2.file_info->skeletons_count << endl;
	cout << endl;

	export_skeletons(scene, gr2.file_info, fbx_bones);
	export_animations(scene, gr2.file_info);
}<|endoftext|>


<commit_before>/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/

/**
 * \file rq_sensor.cpp
 * \date July 14, 2014
 *  \author Jonathan Savoie <jonathan.savoie@robotiq.com>
 *  \maintainer Nicolas Lauzier <nicolas@robotiq.com>
 */

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

#include "boost/bind.hpp"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Empty.h"
#include "geometry_msgs/WrenchStamped.h"
#include "robotiq_force_torque_sensor/rq_sensor_state.h"
#include "robotiq_force_torque_sensor/ft_sensor.h"
#include "robotiq_force_torque_sensor/sensor_accessor.h"

static void decode_message_and_do(INT_8 const  * const buff, INT_8 * const ret);
bool wait_for_other_connection(void);
bool try_connection(void);
static int max_retries_(5);
void shutdown(void);
ros::Publisher sensor_pub;
ros::Publisher wrench_pub;
ros::Publisher watchdog_pub;
ros::ServiceServer service;

/**
 * \brief Decode the message received and do the associated action
 * \param buff message to decode
 * \param ret buffer containing the return value from a GET command
 */
static void decode_message_and_do(INT_8 const  * const buff, INT_8 * const ret)
{
	INT_8 get_or_set[3];
	INT_8 nom_var[4];

	if(buff == NULL || strlen(buff) != 7)
	{
		return;
	}

	strncpy(get_or_set, &buff[0], 3);
	strncpy(nom_var, &buff[4], strlen(buff) -3);

	if(strstr(get_or_set, "GET"))
	{
		rq_state_get_command(nom_var, ret);
	}
	else if(strstr(get_or_set, "SET"))
	{
		if(strstr(nom_var, "ZRO"))
		{
			rq_state_do_zero_force_flag();
			strcpy(ret,"Done");
		}
	}
}

bool receiverCallback(robotiq_force_torque_sensor::sensor_accessor::Request& req,
	robotiq_force_torque_sensor::sensor_accessor::Response& res)
{
	ROS_INFO("I heard: [%s]",req.command.c_str());
	INT_8 buffer[512];
	decode_message_and_do((char*)req.command.c_str(), buffer);
	res.res = buffer;
	ROS_INFO("I send: [%s]", res.res.c_str());
	return true;
}

/**
 * \fn bool wait_for_other_connection()
 * \brief Each second, checks for a sensor that has been connected
 */
bool wait_for_other_connection(void)
{
	INT_8 ret;
	INT_8 count = 0;

	while(ros::ok() && count < 6)
	{
	    if (count % 2 == 0)
	    {
		    ROS_INFO("Waiting for sensor connection...");
		}
		usleep(10000); //Attend 1 seconde.

		ret = rq_sensor_state(max_retries_);
		if(ret == 0)
		{
			ROS_INFO("Sensor connected!");
			return true;
		}
        count++;
		ros::spinOnce();
	}
	return false;
}

/**
 * \fn bool try_connection()
 * \brief A helper function to avoid repeated code
 */
bool try_connection(void)
{
	INT_8 ret;
	ret = rq_sensor_state(max_retries_);
	if(ret == -1)
	{
		if (!wait_for_other_connection())
		{
		    return false;
		}
	}
	return true;
}

/**
 * \fn void get_data(void)
 * \brief Builds the message with the force/torque data
 * \return ft_sensor updated with the latest data
 */
static robotiq_force_torque_sensor::ft_sensor get_data(void)
{
	robotiq_force_torque_sensor::ft_sensor msgStream;

	msgStream.Fx = rq_state_get_received_data(0);
	msgStream.Fy = rq_state_get_received_data(1);
	msgStream.Fz = rq_state_get_received_data(2);
	msgStream.Mx = rq_state_get_received_data(3);
	msgStream.My = rq_state_get_received_data(4);
	msgStream.Mz = rq_state_get_received_data(5);

	return msgStream;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "robotiq_force_torque_sensor");
	ROS_INFO("Trying to Start Sensor");
	ros::NodeHandle n;
	std_msgs::Bool connection_msg;
	ros::param::param<int>("~max_retries", max_retries_, 100);

	INT_8 bufStream[512];
	INT_32 publish_count = 0;
	robotiq_force_torque_sensor::ft_sensor msgStream;
    bool connected = false;
    uint8_t created_publishers = 0;

	//If we can't initialize, we return an error
	connected = try_connection();

	//Reads basic info on the sensor
	if (connected) {
	    connected = try_connection();
	}

	//Starts the stream
	if (connected) {
	    connected = try_connection();
	}

	ros::Publisher connection_pub = n.advertise<std_msgs::Bool>("robotiq_force_torque_sensor_connected", 1);
	if (connected) {
		sensor_pub = n.advertise<robotiq_force_torque_sensor::ft_sensor>("robotiq_force_torque_sensor", 512);
		wrench_pub = n.advertise<geometry_msgs::WrenchStamped>("robotiq_force_torque_wrench", 512);
		watchdog_pub = n.advertise<std_msgs::Empty>("/robotiq_ft300_force_torque_sensor/watchdog", 1);
		service = n.advertiseService("robotiq_force_torque_sensor_acc", receiverCallback);
		created_publishers = 1;
	}

	//std_msgs::String msg;
	geometry_msgs::WrenchStamped wrenchMsg;
	std_msgs::Empty watchdog_msg;
	ros::param::param<std::string>("~frame_id", wrenchMsg.header.frame_id, "robotiq_force_torque_frame_id");

	// Publish the connection state
	connection_msg.data = connected;
	connection_pub.publish(connection_msg);
	ros::spinOnce();

    if (connected)
    {
	    ROS_INFO("Starting Sensor");
	}

	while(ros::ok() && connected)
	{
	    connected = try_connection();

		if(rq_sensor_get_current_state() == RQ_STATE_RUN)
		{
			strcpy(bufStream,"");
			msgStream = get_data();

			if(rq_state_got_new_message())
			{
				sensor_pub.publish(msgStream);

				// compose WrenchStamped Msg
				wrenchMsg.header.stamp = ros::Time::now();
				wrenchMsg.wrench.force.x = msgStream.Fx;
				wrenchMsg.wrench.force.y = msgStream.Fy;
				wrenchMsg.wrench.force.z = msgStream.Fz;
				wrenchMsg.wrench.torque.x = msgStream.Mx;
				wrenchMsg.wrench.torque.y = msgStream.My;
				wrenchMsg.wrench.torque.z = msgStream.Mz;
				wrench_pub.publish(wrenchMsg);
			}
		}

        // Publish the connection state every 1000 iterations
        if (publish_count % 10 == 0)
        {
	        watchdog_pub.publish(watchdog_msg);
        }

        if (publish_count == 1000)
        {
            connection_msg.data = connected;
	        connection_pub.publish(connection_msg);
	        publish_count = 0;
        }
        publish_count++;
		ros::spinOnce();
	}

	if (!connected)
	{
	    connection_msg.data = connected;
	    connection_pub.publish(connection_msg);
	    ros::spinOnce();
	}

	ROS_INFO("Sensor Stopped");
	if (created_publishers)
	{
		// unregister publishers
		sensor_pub.shutdown();
		wrench_pub.shutdown();
		watchdog_pub.shutdown();
		service.shutdown();
	}

	// unregister
	connection_pub.shutdown();
	return 0;
}
<commit_msg>added force sensor simulation (#21)<commit_after>/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/

/**
 * \file rq_sensor.cpp
 * \date July 14, 2014
 *  \author Jonathan Savoie <jonathan.savoie@robotiq.com>
 *  \maintainer Nicolas Lauzier <nicolas@robotiq.com>
 */

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

#include "boost/bind.hpp"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Empty.h"
#include "geometry_msgs/WrenchStamped.h"
#include "robotiq_force_torque_sensor/rq_sensor_state.h"
#include "robotiq_force_torque_sensor/ft_sensor.h"
#include "robotiq_force_torque_sensor/sensor_accessor.h"

static void decode_message_and_do(INT_8 const  * const buff, INT_8 * const ret);
bool wait_for_other_connection(void);
bool try_connection(void);
static int max_retries_(5);
void shutdown(void);
void simulateSensor(ros::NodeHandle& nh);
void launchRealSensor(ros::NodeHandle& nh);
ros::Publisher sensor_pub;
ros::Publisher wrench_pub;
ros::Publisher watchdog_pub;
ros::ServiceServer service;

/**
 * \brief Decode the message received and do the associated action
 * \param buff message to decode
 * \param ret buffer containing the return value from a GET command
 */
static void decode_message_and_do(INT_8 const  * const buff, INT_8 * const ret)
{
    INT_8 get_or_set[3];
    INT_8 nom_var[4];

    if(buff == NULL || strlen(buff) != 7)
    {
        return;
    }

    strncpy(get_or_set, &buff[0], 3);
    strncpy(nom_var, &buff[4], strlen(buff) -3);

    if(strstr(get_or_set, "GET"))
    {
        rq_state_get_command(nom_var, ret);
    }
    else if(strstr(get_or_set, "SET"))
    {
        if(strstr(nom_var, "ZRO"))
        {
            rq_state_do_zero_force_flag();
            strcpy(ret,"Done");
        }
    }
}

bool receiverCallback(robotiq_force_torque_sensor::sensor_accessor::Request& req,
    robotiq_force_torque_sensor::sensor_accessor::Response& res)
{
    ROS_INFO("I heard: [%s]",req.command.c_str());
    INT_8 buffer[512];
    decode_message_and_do((char*)req.command.c_str(), buffer);
    res.res = buffer;
    ROS_INFO("I send: [%s]", res.res.c_str());
    return true;
}

/**
 * \fn bool wait_for_other_connection()
 * \brief Each second, checks for a sensor that has been connected
 */
bool wait_for_other_connection(void)
{
    INT_8 ret;
    INT_8 count = 0;

    while(ros::ok() && count < 6)
    {
        if (count % 2 == 0)
        {
            ROS_INFO("Waiting for sensor connection...");
        }
        usleep(10000); //Attend 1 seconde.

        ret = rq_sensor_state(max_retries_);
        if(ret == 0)
        {
            ROS_INFO("Sensor connected!");
            return true;
        }
        count++;
        ros::spinOnce();
    }
    return false;
}

/**
 * \fn bool try_connection()
 * \brief A helper function to avoid repeated code
 */
bool try_connection(void)
{
    INT_8 ret;
    ret = rq_sensor_state(max_retries_);
    if(ret == -1)
    {
        if (!wait_for_other_connection())
        {
            return false;
        }
    }
    return true;
}

/**
 * \fn void get_data(void)
 * \brief Builds the message with the force/torque data
 * \return ft_sensor updated with the latest data
 */
static robotiq_force_torque_sensor::ft_sensor get_data(void)
{
    robotiq_force_torque_sensor::ft_sensor msgStream;

    msgStream.Fx = rq_state_get_received_data(0);
    msgStream.Fy = rq_state_get_received_data(1);
    msgStream.Fz = rq_state_get_received_data(2);
    msgStream.Mx = rq_state_get_received_data(3);
    msgStream.My = rq_state_get_received_data(4);
    msgStream.Mz = rq_state_get_received_data(5);

    return msgStream;
}

void simulateSensor(ros::NodeHandle& nh)
{
    ros::Publisher connection_pub = nh.advertise<std_msgs::Bool>("robotiq_force_torque_sensor_connected", 1);
    sensor_pub = nh.advertise<robotiq_force_torque_sensor::ft_sensor>("robotiq_force_torque_sensor", 512);
    wrench_pub = nh.advertise<geometry_msgs::WrenchStamped>("robotiq_force_torque_wrench", 512);
    watchdog_pub = nh.advertise<std_msgs::Empty>("/robotiq_ft300_force_torque_sensor/watchdog", 1);
    service = nh.advertiseService("robotiq_force_torque_sensor_acc", receiverCallback);

    std_msgs::Bool connection_msg;
    robotiq_force_torque_sensor::ft_sensor msgStream;
    geometry_msgs::WrenchStamped wrenchMsg;
    std_msgs::Empty watchdog_msg;
    ros::param::param<std::string>("~frame_id", wrenchMsg.header.frame_id, "robotiq_force_torque_frame_id");
    INT_32 publish_count = 0;

    // Publish the connection state
    connection_msg.data = true;
    connection_pub.publish(connection_msg);
    ros::spinOnce();

    ROS_INFO("Starting Sensor");

    msgStream.Fx = 0.0;
    msgStream.Fy = 0.0;
    msgStream.Fz = 0.0;
    msgStream.Mx = 0.0;
    msgStream.My = 0.0;
    msgStream.Mz = 0.0;
    wrenchMsg.wrench.force.x = msgStream.Fx;
    wrenchMsg.wrench.force.y = msgStream.Fy;
    wrenchMsg.wrench.force.z = msgStream.Fz;
    wrenchMsg.wrench.torque.x = msgStream.Mx;
    wrenchMsg.wrench.torque.y = msgStream.My;
    wrenchMsg.wrench.torque.z = msgStream.Mz;

    while(ros::ok()) {
        sensor_pub.publish(msgStream);
        wrenchMsg.header.stamp = ros::Time::now();
        wrench_pub.publish(wrenchMsg);

        // Publish the connection state every 1000 iterations
        if (publish_count % 10 == 0) {
            watchdog_pub.publish(watchdog_msg);
        }

        if (publish_count == 1000) {
            connection_msg.data = true;
            connection_pub.publish(connection_msg);
            publish_count = 0;
        }
        publish_count++;
        // Keep publishing rate to 100Hz
        ros::Duration(0.01).sleep();
        ros::spinOnce();
    }

    ROS_INFO("Sensor Stopped");

    // Publish the connection state
    connection_msg.data = false;
    connection_pub.publish(connection_msg);
    ros::spinOnce();

    // unregister publishers
    sensor_pub.shutdown();
    wrench_pub.shutdown();
    watchdog_pub.shutdown();
    service.shutdown();
    connection_pub.shutdown();
}

void launchRealSensor(ros::NodeHandle& nh)
{
    std_msgs::Bool connection_msg;
    ros::param::param<int>("~max_retries", max_retries_, 100);

    INT_8 bufStream[512];
    INT_32 publish_count = 0;
    robotiq_force_torque_sensor::ft_sensor msgStream;
    bool connected = false;
    uint8_t created_publishers = 0;

    //If we can't initialize, we return an error
    connected = try_connection();

    //Reads basic info on the sensor
    if (connected) {
        connected = try_connection();
    }

    //Starts the stream
    if (connected) {
        connected = try_connection();
    }

    ros::Publisher connection_pub = nh.advertise<std_msgs::Bool>("robotiq_force_torque_sensor_connected", 1);
    if (connected) {
        sensor_pub = nh.advertise<robotiq_force_torque_sensor::ft_sensor>("robotiq_force_torque_sensor", 512);
        wrench_pub = nh.advertise<geometry_msgs::WrenchStamped>("robotiq_force_torque_wrench", 512);
        watchdog_pub = nh.advertise<std_msgs::Empty>("/robotiq_ft300_force_torque_sensor/watchdog", 1);
        service = nh.advertiseService("robotiq_force_torque_sensor_acc", receiverCallback);
        created_publishers = 1;
    }

    //std_msgs::String msg;
    geometry_msgs::WrenchStamped wrenchMsg;
    std_msgs::Empty watchdog_msg;
    ros::param::param<std::string>("~frame_id", wrenchMsg.header.frame_id, "robotiq_force_torque_frame_id");

    // Publish the connection state
    connection_msg.data = connected;
    connection_pub.publish(connection_msg);
    ros::spinOnce();

    if (connected)
    {
        ROS_INFO("Starting Sensor");
    }

    while(ros::ok() && connected)
    {
        connected = try_connection();

        if(rq_sensor_get_current_state() == RQ_STATE_RUN)
        {
            strcpy(bufStream,"");
            msgStream = get_data();

            if(rq_state_got_new_message())
            {
                sensor_pub.publish(msgStream);

                // compose WrenchStamped Msg
                wrenchMsg.header.stamp = ros::Time::now();
                wrenchMsg.wrench.force.x = msgStream.Fx;
                wrenchMsg.wrench.force.y = msgStream.Fy;
                wrenchMsg.wrench.force.z = msgStream.Fz;
                wrenchMsg.wrench.torque.x = msgStream.Mx;
                wrenchMsg.wrench.torque.y = msgStream.My;
                wrenchMsg.wrench.torque.z = msgStream.Mz;
                wrench_pub.publish(wrenchMsg);
            }
        }

        // Publish the connection state every 1000 iterations
        if (publish_count % 10 == 0)
        {
            watchdog_pub.publish(watchdog_msg);
        }

        if (publish_count == 1000)
        {
            connection_msg.data = connected;
            connection_pub.publish(connection_msg);
            publish_count = 0;
        }
        publish_count++;
        ros::spinOnce();
    }

    if (!connected)
    {
        connection_msg.data = connected;
        connection_pub.publish(connection_msg);
        ros::spinOnce();
    }

    ROS_INFO("Sensor Stopped");
    if (created_publishers)
    {
        // unregister publishers
        sensor_pub.shutdown();
        wrench_pub.shutdown();
        watchdog_pub.shutdown();
        service.shutdown();
    }

    // unregister
    connection_pub.shutdown();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "robotiq_force_torque_sensor");
    ROS_INFO("Trying to Start Sensor");
    ros::NodeHandle n;

    bool simulated;
    ros::param::param<bool>("/simulate_robot", simulated, false);
    if (simulated) {
        simulateSensor(n);
    }
    else {
        launchRealSensor(n);
    }
    return 0;
}
<|endoftext|>


<commit_before>// @(#)root/base:$Id$
// Author: G. Ganis 2012

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

/**

\class TStatistic

Statistical variable, defined by its mean and variance (RMS).
Named, streamable, storable and mergeable.

@ingroup MathCore

*/

#include "TStatistic.h"

templateClassImp(TStatistic);

////////////////////////////////////////////////////////////////////////////////
/// Constructor from a vector of values

TStatistic::TStatistic(const char *name, Int_t n, const Double_t *val, const Double_t *w)
         : fName(name), fN(0), fW(0.), fW2(0.), fM(0.), fM2(0.)
{
   if (n > 0) {
      for (Int_t i = 0; i < n; i++) {
         if (w) {
            Fill(val[i], w[i]);
         } else {
            Fill(val[i]);
         }
      }
   }
}

////////////////////////////////////////////////////////////////////////////////
/// TStatistic destructor.

TStatistic::~TStatistic()
{
   // Required since we overload TObject::Hash.
   ROOT::CallRecursiveRemoveIfNeeded(*this);
}

void TStatistic::Fill(Double_t val, Double_t w) {
      // Incremental quantities
   // use formula 1.4 in Chan-Golub, LeVeque
   // Algorithms for computing the Sample Variance (1983)
   // genralized by LM for the case of weights

   if (w == 0) return;

   fN++;

   Double_t tW = fW + w;
   fM += w * val;

//      Double_t dt = val - fM ;
   if (tW == 0) {
      Warning("Fill","Sum of weights is zero - ignore current data point");
      fN--;
      return;
   }
   if (fW != 0) {  // from the second time
      Double_t rr = ( tW * val - fM);
      fM2 += w * rr * rr / (tW * fW);
   }
   fW = tW;
   fW2 += w*w;
}

void TStatistic::Print(Option_t *) const {
   // Print this parameter content
   TROOT::IndentLevel();
   Printf(" OBJ: TStatistic\t %s = %.5g +- %.4g \t RMS = %.5g \t N = %lld",
          fName.Data(), GetMean(), GetMeanErr(), GetRMS(), fN);
}

// Implementation of Merge
Int_t TStatistic::Merge(TCollection *in) {
   // Merge objects in the list.
   // Returns the number of objects that were in the list.
   TIter nxo(in);
   Int_t n = 0;
   while (TObject *o = nxo()) {
      TStatistic *c = dynamic_cast<TStatistic *>(o);
      if (c) {
         if (fW == 0 || c->fW == 0 || ((fW + c->fW) == 0) ) {
            Error("Merge","Zero sum of weights - cannot merge data from %s",c->GetName() );
            continue;
         }
         double temp = (c->fW)/(fW) * fM - c->fM;
         fM2 += c->fM2 + fW/(c->fW*(c->fW + fW) ) * temp * temp;
         fM  += c->fM;
         fW  += c->fW;
         fW2 += c->fW2;
         fN  += c->fN;
         n++;
      }
   }
   return n;
}
<commit_msg>[MathCore] TStatistic::Merge is now able to deal with empty TStatistic objects silently<commit_after>// @(#)root/base:$Id$
// Author: G. Ganis 2012

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

/**

\class TStatistic

Statistical variable, defined by its mean and variance (RMS).
Named, streamable, storable and mergeable.

@ingroup MathCore

*/

#include "TStatistic.h"

templateClassImp(TStatistic);

////////////////////////////////////////////////////////////////////////////////
/// Constructor from a vector of values

TStatistic::TStatistic(const char *name, Int_t n, const Double_t *val, const Double_t *w)
         : fName(name), fN(0), fW(0.), fW2(0.), fM(0.), fM2(0.)
{
   if (n > 0) {
      for (Int_t i = 0; i < n; i++) {
         if (w) {
            Fill(val[i], w[i]);
         } else {
            Fill(val[i]);
         }
      }
   }
}

////////////////////////////////////////////////////////////////////////////////
/// TStatistic destructor.

TStatistic::~TStatistic()
{
   // Required since we overload TObject::Hash.
   ROOT::CallRecursiveRemoveIfNeeded(*this);
}

void TStatistic::Fill(Double_t val, Double_t w) {
      // Incremental quantities
   // use formula 1.4 in Chan-Golub, LeVeque
   // Algorithms for computing the Sample Variance (1983)
   // genralized by LM for the case of weights

   if (w == 0) return;

   fN++;

   Double_t tW = fW + w;
   fM += w * val;

//      Double_t dt = val - fM ;
   if (tW == 0) {
      Warning("Fill","Sum of weights is zero - ignore current data point");
      fN--;
      return;
   }
   if (fW != 0) {  // from the second time
      Double_t rr = ( tW * val - fM);
      fM2 += w * rr * rr / (tW * fW);
   }
   fW = tW;
   fW2 += w*w;
}

void TStatistic::Print(Option_t *) const {
   // Print this parameter content
   TROOT::IndentLevel();
   Printf(" OBJ: TStatistic\t %s = %.5g +- %.4g \t RMS = %.5g \t N = %lld",
          fName.Data(), GetMean(), GetMeanErr(), GetRMS(), fN);
}

// Implementation of Merge
Int_t TStatistic::Merge(TCollection *in) {

   // Let's organise the list of objects to merge excluding the empty ones
   std::vector<TStatistic*> statPtrs;
   if (this->fN != 0LL) statPtrs.push_back(this);
   TStatistic *statPtr;
   for (auto o : *in) {
      if ((statPtr = dynamic_cast<TStatistic *>(o)) && statPtr->fN != 0LL) {
         statPtrs.push_back(statPtr);
      }
   }

   // No object included this has entries
   const auto nStatsPtrs = statPtrs.size();

   // Early return possible in case nothing has been filled
   if (nStatsPtrs == 0) return 0;

   // Merge the statistic quantities into local variables to then
   // update the data members of this object
   auto firstStatPtr = statPtrs[0];
   auto N = firstStatPtr->fN;
   auto M = firstStatPtr->fM;
   auto M2 = firstStatPtr->fM2;
   auto W = firstStatPtr->fW;
   auto W2 = firstStatPtr->fW2;
   for (auto i = 1U; i < nStatsPtrs; ++i) {
      auto c = statPtrs[i];
      double temp = (c->fW) / (W)*M - c->fM;
      M2 += c->fM2 + W / (c->fW * (c->fW + W)) * temp * temp;
      M += c->fM;
      W += c->fW;
      W2 += c->fW2;
      N += c->fN;
   }

   // Now update the data members of this object
   fN = N;
   fW = W;
   fW2 = W2;
   fM = M;
   fM2 = M2;

   return nStatsPtrs;

}
<|endoftext|>


<commit_before>#ifndef RECURSE_BODYPARSER_HPP
#define RECURSE_BODYPARSER_HPP

#include <QJsonDocument>
#include <QJsonObject>

auto bodyparser()
{
    return [](auto &ctx, auto next) {
        auto &req = ctx.request;

        if (req.body.isEmpty())
            return next();

        if (req.header["content-type"] == "application/json") {
            QJsonDocument json = QJsonDocument::fromJson(req.body.toUtf8());
            const QVariantMap map = json.object().toVariantMap();
            QMap<QString, QVariant>::const_iterator i;

            for (i = map.constBegin(); i != map.constEnd(); ++i) {
                req.body_parsed[i.key()] = i.value();
            }
        }
        else if (req.header["content-type"] == "application/x-www-form-urlencoded") {
        }

        next();
    };
}

#endif // RECURSE_BODYPARSER_HPP
<commit_msg>conform to new request api ('get' method to set headers)<commit_after>#ifndef RECURSE_BODYPARSER_HPP
#define RECURSE_BODYPARSER_HPP

#include <QJsonDocument>
#include <QJsonObject>

auto bodyparser()
{
    return [](auto &ctx, auto next) {
        auto &req = ctx.request;

        if (req.body.isEmpty())
            return next();

        if (req.get("content-type") == "application/json") {
            QJsonDocument json = QJsonDocument::fromJson(req.body.toUtf8());
            const QVariantMap map = json.object().toVariantMap();
            QMap<QString, QVariant>::const_iterator i;

            for (i = map.constBegin(); i != map.constEnd(); ++i) {
                req.body_parsed[i.key()] = i.value();
            }
        }
        else if (req.get("content-type") == "application/x-www-form-urlencoded") {
        }

        next();
    };
}

#endif // RECURSE_BODYPARSER_HPP
<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, 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$
 *
 */

#include "pcl/point_types.h"
#include "pcl/impl/instantiate.hpp"
#include "pcl/recognition/cg/hough_3d.h"
#include "pcl/recognition/impl/cg/hough_3d.hpp"

PCL_INSTANTIATE_PRODUCT(Hough3DGrouping, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
                                         ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
                                         ((pcl::ReferenceFrame))((pcl::ReferenceFrame)))

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::recognition::HoughSpace3D::HoughSpace3D (const Eigen::Vector3d &min_coord, const Eigen::Vector3d &bin_size, const Eigen::Vector3d &max_coord)
{
  min_coord_ = min_coord;
  bin_size_ = bin_size;

  for (int i = 0; i < 3; ++i)
  {
    bin_count_[i] = static_cast<int> (ceil ((max_coord[i] - min_coord_[i]) / bin_size_[i]));
  }

  partial_bin_products_[0] = 1;
  for (int i=1; i<=3; ++i)
    partial_bin_products_[i] = bin_count_[i-1]*partial_bin_products_[i-1];

  total_bins_count_ = partial_bin_products_[3];

  hough_space_.clear ();
  hough_space_.resize (total_bins_count_, 0.0);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::recognition::HoughSpace3D::reset ()
{
  hough_space_.clear ();
  hough_space_.resize (total_bins_count_, 0.0);

  voter_ids_.clear ();
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::recognition::HoughSpace3D::vote (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id)
{
  int index = 0;

  for (int i=0; i<3; ++i)
  {
    int currentBin = static_cast<int> (floor ((single_vote_coord[i] - min_coord_[i])/bin_size_[i]));
    if (currentBin < 0 || currentBin >= bin_count_[i])
    {
      //PCL_ERROR("Current Vote goes out of bounds in the Hough Table!\nDimension: %d, Value inserted: %f, Min value: %f, Max value: %f\n", i,
      //  single_vote_coord[i], min_coord_[i], min_coord_[i] + bin_size_[i]*bin_count_[i]);
      return -1;
    }

    index += partial_bin_products_[i] * currentBin;
  }

  hough_space_[index] += weight;
  voter_ids_[index].push_back (voter_id);

  return (index);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::recognition::HoughSpace3D::voteInt (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id)
{
  int central_bin_index = 0;

  const int n_neigh = 27; // total number of neighbours = 3^nDim = 27

  Eigen::Vector3i central_bin_coord;
  Eigen::Vector3f bin_centroid;
  Eigen::Vector3f central_bin_weight;
  Eigen::Vector3i interp_bin;
  std::vector<float> interp_weight (n_neigh);

  for (int n = 0; n < n_neigh; ++n)
    interp_weight[n] = 1.0;

  for (int d = 0; d < 3; ++d)
  {
    // Compute coordinates of central bin
    central_bin_coord[d] = static_cast<int> (floor ((single_vote_coord[d] - min_coord_[d]) / bin_size_[d]));
    if (central_bin_coord[d] < 0 || central_bin_coord[d] >= bin_count_[d])
    {
      //PCL_ERROR("Current Vote goes out of bounds in the Hough Table!\nDimension: %d, Value inserted: %f, Min value: %f, Max value: %f\n", d,
      //  single_vote_coord[d], min_coord_[d], min_coord_[d] + bin_size_[d]*bin_count_[d]);
      return -1;
    }

    central_bin_index += partial_bin_products_[d] * central_bin_coord[d];

    // Compute coordinates of the centroid of the bin
    bin_centroid[d] = static_cast<float> ((2 * static_cast<double> (central_bin_coord[d]) * bin_size_[d] + bin_size_[d]) / 2.0 );

    // Compute interpolated weight for each coordinate of the central bin
    central_bin_weight[d] = static_cast<float> (1 - (fabs (single_vote_coord[d] - min_coord_[d] - bin_centroid[d]) / bin_size_[d] ) );

    // Compute the neighbor bins where the weight has to be interpolated
    if ((single_vote_coord[d] - min_coord_[d]) < bin_centroid[d])
    {
      interp_bin[d] = central_bin_coord[d] - 1;
    }
    else if ((single_vote_coord[d] - min_coord_[d]) > bin_centroid[d])
    {
      interp_bin[d] = central_bin_coord[d] + 1;
    }
    else
    {
      interp_bin[d] = central_bin_coord[d]; // the vote is precisely in the middle, so along this dimension the vote is given all to the central bin
    }
  }

  // For each neighbor of the central point
  //int counterRealVotes = 0;
  for (int n = 0; n < n_neigh; ++n)
  {
    int final_bin_index = 0;
    int exp = 1;
    bool invalid = false;

    for (int d = 0; d < 3; ++d)
    {
      int curr_neigh_index = central_bin_coord[d] + ( n % (exp*3) ) / exp - 1; // (n % 3^(d+1) / 3^d) - 1
      if (curr_neigh_index >= 0 && curr_neigh_index <= bin_count_[d]-1)
      {
        // Each coordinate of the neighbor has to be equal either to one of the central bin or to one of the interpolated bins
        if(curr_neigh_index == interp_bin[d])
        {
          interp_weight[n] *= 1-central_bin_weight[d];
        }
        else if(curr_neigh_index == central_bin_coord[d])
        {
          interp_weight[n] *= central_bin_weight[d];
        }
        else
        {
          invalid = true;
          break;
        }

        final_bin_index += curr_neigh_index * partial_bin_products_[d];
      }
      else
      {
        invalid = true;
        break;
      }
      exp *= 3;
    }

    if (!invalid)
    {
      hough_space_[final_bin_index] += weight * interp_weight[n];
      voter_ids_[final_bin_index].push_back (voter_id);
    }
  }

  return (central_bin_index);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double
pcl::recognition::HoughSpace3D::findMaxima (double min_threshold, std::vector<double> &maxima_values, std::vector<std::vector<int> > &maxima_voter_ids)
{
  // If min_threshold between -1 and 0 use it as a percentage of maximum vote
  if (min_threshold < 0)
  {
    double hough_maximum = std::numeric_limits<double>::min ();
    for (int i = 0; i < total_bins_count_; ++i)
    {
      if (hough_space_[i] > hough_maximum)
      {
        hough_maximum = hough_space_[i];
      }
    }

    min_threshold = min_threshold >= -1 ? -min_threshold * hough_maximum : hough_maximum;
  }

  maxima_voter_ids.clear ();
  maxima_values.clear ();

  double indexes[3];

  //int zeros = 0;

  for (int i=0; i < total_bins_count_; ++i)
  {
    //if (hough_space_[i] == 0)
    //{
    //  ++zeros;
    //}
    if (hough_space_[i] < min_threshold)
      continue;

    // Check with neighbors
    bool is_maximum = true;
    int moduled_index = i;

    for (int k = 2; k >= 0; --k){

      moduled_index = moduled_index % partial_bin_products_[k+1];
      indexes[k] = moduled_index / partial_bin_products_[k];

      if (indexes[k] > 0 && hough_space_[i] < hough_space_[i-partial_bin_products_[k]])
      {
        is_maximum = false;
        break;
      }
      if (indexes[k] < bin_count_[k]-1 && hough_space_[i] < hough_space_[i+partial_bin_products_[k]])
      {
        is_maximum = false;
        break;
      }
    }

    if (is_maximum)
    {
      maxima_values.push_back (hough_space_[i]);
      maxima_voter_ids.push_back ( voter_ids_[i] );
    }
  }

  return (min_threshold);
}
<commit_msg>Prefer usage of operator%= instead of using operator%<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, 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$
 *
 */

#include "pcl/point_types.h"
#include "pcl/impl/instantiate.hpp"
#include "pcl/recognition/cg/hough_3d.h"
#include "pcl/recognition/impl/cg/hough_3d.hpp"

PCL_INSTANTIATE_PRODUCT(Hough3DGrouping, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
                                         ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
                                         ((pcl::ReferenceFrame))((pcl::ReferenceFrame)))

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::recognition::HoughSpace3D::HoughSpace3D (const Eigen::Vector3d &min_coord, const Eigen::Vector3d &bin_size, const Eigen::Vector3d &max_coord)
{
  min_coord_ = min_coord;
  bin_size_ = bin_size;

  for (int i = 0; i < 3; ++i)
  {
    bin_count_[i] = static_cast<int> (ceil ((max_coord[i] - min_coord_[i]) / bin_size_[i]));
  }

  partial_bin_products_[0] = 1;
  for (int i=1; i<=3; ++i)
    partial_bin_products_[i] = bin_count_[i-1]*partial_bin_products_[i-1];

  total_bins_count_ = partial_bin_products_[3];

  hough_space_.clear ();
  hough_space_.resize (total_bins_count_, 0.0);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::recognition::HoughSpace3D::reset ()
{
  hough_space_.clear ();
  hough_space_.resize (total_bins_count_, 0.0);

  voter_ids_.clear ();
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::recognition::HoughSpace3D::vote (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id)
{
  int index = 0;

  for (int i=0; i<3; ++i)
  {
    int currentBin = static_cast<int> (floor ((single_vote_coord[i] - min_coord_[i])/bin_size_[i]));
    if (currentBin < 0 || currentBin >= bin_count_[i])
    {
      //PCL_ERROR("Current Vote goes out of bounds in the Hough Table!\nDimension: %d, Value inserted: %f, Min value: %f, Max value: %f\n", i,
      //  single_vote_coord[i], min_coord_[i], min_coord_[i] + bin_size_[i]*bin_count_[i]);
      return -1;
    }

    index += partial_bin_products_[i] * currentBin;
  }

  hough_space_[index] += weight;
  voter_ids_[index].push_back (voter_id);

  return (index);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::recognition::HoughSpace3D::voteInt (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id)
{
  int central_bin_index = 0;

  const int n_neigh = 27; // total number of neighbours = 3^nDim = 27

  Eigen::Vector3i central_bin_coord;
  Eigen::Vector3f bin_centroid;
  Eigen::Vector3f central_bin_weight;
  Eigen::Vector3i interp_bin;
  std::vector<float> interp_weight (n_neigh);

  for (int n = 0; n < n_neigh; ++n)
    interp_weight[n] = 1.0;

  for (int d = 0; d < 3; ++d)
  {
    // Compute coordinates of central bin
    central_bin_coord[d] = static_cast<int> (floor ((single_vote_coord[d] - min_coord_[d]) / bin_size_[d]));
    if (central_bin_coord[d] < 0 || central_bin_coord[d] >= bin_count_[d])
    {
      //PCL_ERROR("Current Vote goes out of bounds in the Hough Table!\nDimension: %d, Value inserted: %f, Min value: %f, Max value: %f\n", d,
      //  single_vote_coord[d], min_coord_[d], min_coord_[d] + bin_size_[d]*bin_count_[d]);
      return -1;
    }

    central_bin_index += partial_bin_products_[d] * central_bin_coord[d];

    // Compute coordinates of the centroid of the bin
    bin_centroid[d] = static_cast<float> ((2 * static_cast<double> (central_bin_coord[d]) * bin_size_[d] + bin_size_[d]) / 2.0 );

    // Compute interpolated weight for each coordinate of the central bin
    central_bin_weight[d] = static_cast<float> (1 - (fabs (single_vote_coord[d] - min_coord_[d] - bin_centroid[d]) / bin_size_[d] ) );

    // Compute the neighbor bins where the weight has to be interpolated
    if ((single_vote_coord[d] - min_coord_[d]) < bin_centroid[d])
    {
      interp_bin[d] = central_bin_coord[d] - 1;
    }
    else if ((single_vote_coord[d] - min_coord_[d]) > bin_centroid[d])
    {
      interp_bin[d] = central_bin_coord[d] + 1;
    }
    else
    {
      interp_bin[d] = central_bin_coord[d]; // the vote is precisely in the middle, so along this dimension the vote is given all to the central bin
    }
  }

  // For each neighbor of the central point
  //int counterRealVotes = 0;
  for (int n = 0; n < n_neigh; ++n)
  {
    int final_bin_index = 0;
    int exp = 1;
    bool invalid = false;

    for (int d = 0; d < 3; ++d)
    {
      int curr_neigh_index = central_bin_coord[d] + ( n % (exp*3) ) / exp - 1; // (n % 3^(d+1) / 3^d) - 1
      if (curr_neigh_index >= 0 && curr_neigh_index <= bin_count_[d]-1)
      {
        // Each coordinate of the neighbor has to be equal either to one of the central bin or to one of the interpolated bins
        if(curr_neigh_index == interp_bin[d])
        {
          interp_weight[n] *= 1-central_bin_weight[d];
        }
        else if(curr_neigh_index == central_bin_coord[d])
        {
          interp_weight[n] *= central_bin_weight[d];
        }
        else
        {
          invalid = true;
          break;
        }

        final_bin_index += curr_neigh_index * partial_bin_products_[d];
      }
      else
      {
        invalid = true;
        break;
      }
      exp *= 3;
    }

    if (!invalid)
    {
      hough_space_[final_bin_index] += weight * interp_weight[n];
      voter_ids_[final_bin_index].push_back (voter_id);
    }
  }

  return (central_bin_index);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double
pcl::recognition::HoughSpace3D::findMaxima (double min_threshold, std::vector<double> &maxima_values, std::vector<std::vector<int> > &maxima_voter_ids)
{
  // If min_threshold between -1 and 0 use it as a percentage of maximum vote
  if (min_threshold < 0)
  {
    double hough_maximum = std::numeric_limits<double>::min ();
    for (int i = 0; i < total_bins_count_; ++i)
    {
      if (hough_space_[i] > hough_maximum)
      {
        hough_maximum = hough_space_[i];
      }
    }

    min_threshold = min_threshold >= -1 ? -min_threshold * hough_maximum : hough_maximum;
  }

  maxima_voter_ids.clear ();
  maxima_values.clear ();

  double indexes[3];

  //int zeros = 0;

  for (int i=0; i < total_bins_count_; ++i)
  {
    //if (hough_space_[i] == 0)
    //{
    //  ++zeros;
    //}
    if (hough_space_[i] < min_threshold)
      continue;

    // Check with neighbors
    bool is_maximum = true;
    int moduled_index = i;

    for (int k = 2; k >= 0; --k){

      moduled_index %= partial_bin_products_[k+1];
      indexes[k] = moduled_index / partial_bin_products_[k];

      if (indexes[k] > 0 && hough_space_[i] < hough_space_[i-partial_bin_products_[k]])
      {
        is_maximum = false;
        break;
      }
      if (indexes[k] < bin_count_[k]-1 && hough_space_[i] < hough_space_[i+partial_bin_products_[k]])
      {
        is_maximum = false;
        break;
      }
    }

    if (is_maximum)
    {
      maxima_values.push_back (hough_space_[i]);
      maxima_voter_ids.push_back ( voter_ids_[i] );
    }
  }

  return (min_threshold);
}
<|endoftext|>