<commit_before>/*
 * DISTRHO Plugin Framework (DPF)
 * Copyright (C) 2012-2014 Filipe Coelho <falktx@falktx.com>
 *
 * Permission to use, copy, modify, and/or distribute this software for any purpose with
 * or without fee is hereby granted, provided that the above copyright notice and this
 * permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD
 * TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN
 * NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
 * DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER
 * IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#ifndef DGL_BASE_HPP_INCLUDED
#define DGL_BASE_HPP_INCLUDED

#include "../distrho/extra/d_leakdetector.hpp"
#include "../distrho/extra/d_scopedpointer.hpp"

// -----------------------------------------------------------------------
// Define namespace

#ifndef DGL_NAMESPACE
# define DGL_NAMESPACE DGL
#endif

#define START_NAMESPACE_DGL namespace DGL_NAMESPACE {
#define END_NAMESPACE_DGL }
#define USE_NAMESPACE_DGL using namespace DGL_NAMESPACE;

// -----------------------------------------------------------------------
// OpenGL includes

#ifdef DISTRHO_OS_MAC
# include "OpenGL/gl.h"
#else
# include "GL/gl.h"
#endif

// -----------------------------------------------------------------------
// Missing OpenGL defines

#if defined(GL_BGR_EXT) && ! defined(GL_BGR)
# define GL_BGR GL_BGR_EXT
#endif

#if defined(GL_BGRA_EXT) && ! defined(GL_BGRA)
# define GL_BGRA GL_BGRA_EXT
#endif

#ifndef GL_CLAMP_TO_BORDER
# define GL_CLAMP_TO_BORDER 0x812D
#endif

START_NAMESPACE_DGL

// -----------------------------------------------------------------------
// Base DGL enums

/**
   Convenience symbols for ASCII control characters.
 */
enum Char {
    CHAR_BACKSPACE = 0x08,
    CHAR_ESCAPE    = 0x1B,
    CHAR_DELETE    = 0x7F
};

/**
   Special (non-Unicode) keyboard keys.
 */
enum Key {
    KEY_F1 = 1,
    KEY_F2,
    KEY_F3,
    KEY_F4,
    KEY_F5,
    KEY_F6,
    KEY_F7,
    KEY_F8,
    KEY_F9,
    KEY_F10,
    KEY_F11,
    KEY_F12,
    KEY_LEFT,
    KEY_UP,
    KEY_RIGHT,
    KEY_DOWN,
    KEY_PAGE_UP,
    KEY_PAGE_DOWN,
    KEY_HOME,
    KEY_END,
    KEY_INSERT,
    KEY_SHIFT,
    KEY_CTRL,
    KEY_ALT,
    KEY_SUPER
};

/**
   Keyboard modifier flags.
 */
enum Modifier {
    MODIFIER_SHIFT = 1 << 0, /**< Shift key */
    MODIFIER_CTRL  = 1 << 1, /**< Control key */
    MODIFIER_ALT   = 1 << 2, /**< Alt/Option key */
    MODIFIER_SUPER = 1 << 3  /**< Mod4/Command/Windows key */
};

// -----------------------------------------------------------------------
// Base DGL classes

/**
   Idle callback.
 */
class IdleCallback
{
public:
    virtual ~IdleCallback() {}
    virtual void idleCallback() = 0;
};

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

END_NAMESPACE_DGL

#endif // DGL_BASE_HPP_INCLUDED
<commit_msg>Try to not include windows.h for GL headers<commit_after>/*
 * DISTRHO Plugin Framework (DPF)
 * Copyright (C) 2012-2014 Filipe Coelho <falktx@falktx.com>
 *
 * Permission to use, copy, modify, and/or distribute this software for any purpose with
 * or without fee is hereby granted, provided that the above copyright notice and this
 * permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD
 * TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN
 * NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
 * DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER
 * IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#ifndef DGL_BASE_HPP_INCLUDED
#define DGL_BASE_HPP_INCLUDED

#include "../distrho/extra/d_leakdetector.hpp"
#include "../distrho/extra/d_scopedpointer.hpp"

// -----------------------------------------------------------------------
// Define namespace

#ifndef DGL_NAMESPACE
# define DGL_NAMESPACE DGL
#endif

#define START_NAMESPACE_DGL namespace DGL_NAMESPACE {
#define END_NAMESPACE_DGL }
#define USE_NAMESPACE_DGL using namespace DGL_NAMESPACE;

// -----------------------------------------------------------------------
// Fix OpenGL includes for Windows, based on glfw code

#ifdef DISTRHO_OS_WINDOWS

#ifndef APIENTRY
# define APIENTRY __stdcall
#endif // APIENTRY

/* We need WINGDIAPI defined */
#ifndef WINGDIAPI
# if defined(_MSC_VER) || defined(__BORLANDC__) || defined(__POCC__)
#  define WINGDIAPI __declspec(dllimport)
# elif defined(__LCC__)
#  define WINGDIAPI __stdcall
# else
#  define WINGDIAPI extern
# endif
# define DGL_WINGDIAPI_DEFINED
#endif // WINGDIAPI

/* Some <GL/glu.h> files also need CALLBACK defined */
#ifndef CALLBACK
# if defined(_MSC_VER)
#  if (defined(_M_MRX000) || defined(_M_IX86) || defined(_M_ALPHA) || defined(_M_PPC)) && !defined(MIDL_PASS)
#   define CALLBACK __stdcall
#  else
#   define CALLBACK
#  endif
# else
#  define CALLBACK __stdcall
# endif
# define DGL_CALLBACK_DEFINED
#endif // CALLBACK

/* Most GL/glu.h variants on Windows need wchar_t */
#include <cstddef>

#endif // DISTRHO_OS_WINDOWS

// -----------------------------------------------------------------------
// OpenGL includes

#ifdef DISTRHO_OS_MAC
# include "OpenGL/gl.h"
#else
# include "GL/gl.h"
#endif

// -----------------------------------------------------------------------
// Missing OpenGL defines

#if defined(GL_BGR_EXT) && ! defined(GL_BGR)
# define GL_BGR GL_BGR_EXT
#endif

#if defined(GL_BGRA_EXT) && ! defined(GL_BGRA)
# define GL_BGRA GL_BGRA_EXT
#endif

#ifndef GL_CLAMP_TO_BORDER
# define GL_CLAMP_TO_BORDER 0x812D
#endif

#ifdef DISTRHO_OS_WINDOWS
// -----------------------------------------------------------------------
// Fix OpenGL includes for Windows, based on glfw code

#ifdef DGL_WINGDIAPI_DEFINED
# undef WINGDIAPI
# undef DGL_WINGDIAPI_DEFINED
#endif

#ifdef DGL_CALLBACK_DEFINED
# undef CALLBACK
# undef DGL_CALLBACK_DEFINED
#endif

#endif // DISTRHO_OS_WINDOWS

START_NAMESPACE_DGL

// -----------------------------------------------------------------------
// Base DGL enums

/**
   Convenience symbols for ASCII control characters.
 */
enum Char {
    CHAR_BACKSPACE = 0x08,
    CHAR_ESCAPE    = 0x1B,
    CHAR_DELETE    = 0x7F
};

/**
   Special (non-Unicode) keyboard keys.
 */
enum Key {
    KEY_F1 = 1,
    KEY_F2,
    KEY_F3,
    KEY_F4,
    KEY_F5,
    KEY_F6,
    KEY_F7,
    KEY_F8,
    KEY_F9,
    KEY_F10,
    KEY_F11,
    KEY_F12,
    KEY_LEFT,
    KEY_UP,
    KEY_RIGHT,
    KEY_DOWN,
    KEY_PAGE_UP,
    KEY_PAGE_DOWN,
    KEY_HOME,
    KEY_END,
    KEY_INSERT,
    KEY_SHIFT,
    KEY_CTRL,
    KEY_ALT,
    KEY_SUPER
};

/**
   Keyboard modifier flags.
 */
enum Modifier {
    MODIFIER_SHIFT = 1 << 0, /**< Shift key */
    MODIFIER_CTRL  = 1 << 1, /**< Control key */
    MODIFIER_ALT   = 1 << 2, /**< Alt/Option key */
    MODIFIER_SUPER = 1 << 3  /**< Mod4/Command/Windows key */
};

// -----------------------------------------------------------------------
// Base DGL classes

/**
   Idle callback.
 */
class IdleCallback
{
public:
    virtual ~IdleCallback() {}
    virtual void idleCallback() = 0;
};

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

END_NAMESPACE_DGL

#endif // DGL_BASE_HPP_INCLUDED
<|endoftext|>


<commit_before>/*
 * Copyright 2001-2014 Todd Richmond
 *
 * This file is part of Blister - a light weight, scalable, high performance
 * C++ server framework.
 *
 * 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 "stdapi.h"
#include <ctype.h>
#include <time.h>
#include "HTTPClient.h"
#include "Log.h"

static const int StreamSize = 3 * 1460;

URL &URL::operator =(const URL &url) {
    if (this != &url) {
	prot = url.prot;
	host = url.host;
	path = url.path;
	query = url.query;
	port = url.port;
    }
    return *this;
}

const tstring URL::fullpath(void) const {
    tstring s(prot + T("://") + host);

    if (port != 80) {
	tchar buf[8];

	tsprintf(buf, T(":%u"), port);
	s += buf;
    }
    return s + relpath();
}

bool URL::set(const tchar *url) {
    const tchar *p, *pp;

    query.erase();
    if (!url) {
	prot = T("http");
	host = T("localhost");
	port = 80;
	path = T("/");
	return true;
    }
    p = tstrchr(url, ':');
    if (p && p[1] == '/' && p[2] == '/') {
	prot.assign(url, p - url);
	url = p + 3;
	p = tstrchr(url, ':');
    } else if (p && !istdigit(p[1])) {
	return false;
    } else {
	prot = T("http");
    }
    pp = tstrchr(url, '/');
    if (p && (!pp || p < pp)) {
	port = (ushort)tstrtoul(p + 1, NULL, 10);
	if (!port)
	    return false;
	host.assign(url, p - url);
	p = tstrchr(p, '/');
    } else {
	port = 80;
	if (pp && pp != url)
	    host.assign(url, pp - url);
	else if (pp)
	    host = T("localhost");
	else
	    host = url;
	p = pp;
    }
    if (p) {
	pp = tstrchr(p, '?');
	if (pp) {
	    path.assign(p, pp - p);
	    query = pp + 1;
	    unescape(query);
	} else {
	    path = p;
	}
	unescape(path);
    } else {
	path = T("/");
    }
    return true;
}

void URL::unescape(tchar *str, bool plus) {
    for (tchar *p = str; *p; p++) {
	if (*p == '%') {
	    uint hex;

	    if (*++p <= '9')
		hex = *p - '0';
	    else
		hex = *p - 'A' + 10;
	    hex <<= 4;
	    if (*++p <= '9')
		hex += *p - '0';
	    else
		hex += *p - 'A' + 10;
	    *str++ = (tchar)hex;
	} else if (*p == '+' && plus) {
	    *str++ = ' ';
	} else {
	    *str++ = *p;
	}
    }
    *str = '\0';
}

void URL::unescape(tstring &str, bool plus) {
    uint i, j;

    for (i = 0, j = 0; j < str.size(); j++) {
	tchar p = str[j];

	if (p == '%') {
	    uint hex;

	    p = str[++j];
	    if (p <= '9')
		hex = p - '0';
	    else
		hex = p - 'A' + 10;
	    hex <<= 4;
	    p = str[++j];
	    if (p <= '9')
		hex += p - '0';
	    else
		hex += p - 'A' + 10;
	    str[i] = (tchar)hex;
	} else if (p == '+' && plus) {
	    str[i] = ' ';
	} else if (i != j) {
	    str[i] = p;
	}
	i++;
    }
    str.erase(i);
}

HTTPClient::HTTPClient(): ka(true), ressz(0), result(0), rto(90 * 1000),
    wto(60 * 1000), sstrm(StreamSize), sts(0), sz(0) {}

bool HTTPClient::connect(const Sockaddr &sa, bool keepalive, uint to) {
    if (sock.open() && sa == addr)
	return true;
    if (addr != sa)
	addr = sa;
    sock.close();
    ka = keepalive;
    if (!sock.connect(addr, to)) {
	dlog << Log::Info << T("mod=http cmd=connect addr=") << addr.host() <<
	    T(":") << (uint)addr.port() << T(" errno=") << sock.err() << endlog;
	sock.close();
	return false;
    }
    sock.rtimeout(rto);
    sock.wtimeout(wto);
    sstrm.rdbuf()->attach(sock);
    sstrm.rdbuf()->reset();
    sstrm.clear(sstrm.rdstate() & ~(ios::badbit | ios::eofbit | ios::failbit));
    DLOGD(T("mod=http cmd=connect addr=") << addr.host() << ':' << addr.port());
    return true;
}

bool HTTPClient::send(const tchar *op, const tchar *path, const void *data,
    size_t datasz) {
    char buf[16];
    bool first = true;
    size_t in;
    iovec iov[2];
    bool keep = false;
    const char *p, *pp;
    const tchar *resp;
    bool ret = false;
    string req, s, ss, sss;
    bool sent;
    msec_t start;
    static tchar connection[] = T("Connection");
    static tchar contentlen[] = T("Content-Length");
    static tchar keep_alive[] = T("Keep-Alive");
    static tchar pragma[] = T("Pragma");

    sts = 0;
    s.reserve(128);
    reshdrs.clear();
    req = tchartoachar(op);
    req += ' ';
    req += tchartoachar(path);
    req += " HTTP/1.1\r\nHost: ";
    req += addr.host();
    if (addr.port() != 80) {
	req += ':';
	req += addr.port();
    }
    req += "\r\n";
    if (datasz) {
	req += "Content-Length: ";
	sprintf(buf, "%lu", (ulong)datasz);
	req += buf;
	req += "\r\n";
    }
    if (ka)
	req += "Pragma: Keep-Alive\r\nConnection: Keep-Alive\r\n";
    if (hstrm.size())
	req += tchartoachar(hstrm.str());
    req += "\r\n";
    iov[0].iov_base = (char *)req.c_str();
    iov[0].iov_len = req.size();
    iov[1].iov_base = (char *)data;
    iov[1].iov_len = datasz;
loop:
    if (!connect(addr, ka))
	goto done;
    start = milliticks();
    if ((sent = ((ulong)sock.writev(iov, 2) == (ulong)(req.size() + datasz))) ==
	false ||
	// shutdown causes huge cpu spikes on NT - not sure why
#if 0
	(!ka && !sock.shutdown(false, true)) ||
#endif
	!getline(sstrm, s)) {
	sock.close();
	if (first && ka && (!sent || rto - (milliticks() - start) > 200)) {
	    dlog << Log::Debug << T("mod=http cmd=reconnect") << endlog;
	    sstrm.seekp(0, ios::beg);
	    first = false;
	    goto loop;
	} else {
	    dlog << Log::Note << T("mod=http cmd=disconnect") << endlog;
	    goto done;
	}
    }
    p = s.c_str();
    while (*p != ' ' && *p != '\t')
	p++;
    while (*p == ' ' || *p == '\t')
	p++;
    sts = atoi(p);
    dlog << Log::Debug << T("mod=http status=") << sts << endlog;
    while (getline(sstrm, s)) {		    // does not support folded hdrs
	p = s.c_str();
	while (*p == ' ' || *p == '\t')
	    p++;
	if (*p == '\r' || !*p)
	    break;
	if ((pp = strchr(p, ':')) == NULL)
	    continue;
	ss = s.substr(p - s.c_str(), pp - p);
	do {
	    pp++;
	} while (*pp && (*pp == '\r' || *pp == ' ' || *pp == '\t'));
	sss.assign(pp, strlen(pp) - 1);

	pair<tstring, tstring> pr(astringtotstring(ss), astringtotstring(sss));

	reshdrs.insert(pr);
    }
    if (!sstrm)
	goto done;
    if (ka) {
	if ((resp = response(connection)) != NULL &&
	    !tstrnicmp(resp, keep_alive, sizeof (keep_alive) - 1))
	    keep = true;
	else if ((resp = response(pragma)) != NULL &&
	    !tstrnicmp(resp, keep_alive, sizeof (keep_alive) - 1))
	    keep = true;
    }
    if ((resp = response(contentlen)) == NULL)
	ressz = (ulong)-1;
    else
	ressz = tstrtoul(resp, NULL, 10);
    if (keep && !sz && ressz != (ulong)-1)
	sock.nodelay(true);
    if (ressz && ressz != (ulong)-1) {
	if (ressz > sz) {
	    delete [] result;
	    sz = ressz;
	    result = new char[sz + 1];
	}
	ret = (ulong)sstrm.read(result, ressz) == ressz;
    } else if (ressz) {
	char *newres;
	size_t room = sz;

	keep = false;
	ressz = 0;
	for (;;) {
	    if (!room) {
		room = ressz ? ressz : 12 * 1024;
		sz = ressz + room;
		newres = new char[sz + 1];
		memcpy(newres, result, ressz);
		delete [] result;
		result = newres;
	    }
	    if ((long)(in = (size_t)sstrm.read(result + ressz, room)) > 0) {
		ressz += in;
		ret = true;
	    }
	    if (in == (size_t)-1 || in < room)
		break;
	    room -= in;
	}
    } else {
	ret = true;
    }
done:
    if (!keep || !ret)
	sock.close();
    if (ret && result)
	result[ressz] = '\0';
    hstrm.reset();
    return ret;
}

tostream &HTTPClient::operator <<(tostream &os) {
    attrmap::const_iterator it;

    os << sts << endl;
    for (it = reshdrs.begin(); it != reshdrs.end(); ++it)
	os << it->first << ": " << it->second << endl;
    os.write(achartotchar(result), ressz);
    os << endl;
    return os;
}
<commit_msg>Win32 compile err<commit_after>/*
 * Copyright 2001-2014 Todd Richmond
 *
 * This file is part of Blister - a light weight, scalable, high performance
 * C++ server framework.
 *
 * 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 "stdapi.h"
#include <ctype.h>
#include <time.h>
#include "HTTPClient.h"
#include "Log.h"

static const int StreamSize = 3 * 1460;

URL &URL::operator =(const URL &url) {
    if (this != &url) {
	prot = url.prot;
	host = url.host;
	path = url.path;
	query = url.query;
	port = url.port;
    }
    return *this;
}

const tstring URL::fullpath(void) const {
    tstring s(prot + T("://") + host);

    if (port != 80) {
	tchar buf[8];

	tsprintf(buf, T(":%u"), port);
	s += buf;
    }
    return s + relpath();
}

bool URL::set(const tchar *url) {
    const tchar *p, *pp;

    query.erase();
    if (!url) {
	prot = T("http");
	host = T("localhost");
	port = 80;
	path = T("/");
	return true;
    }
    p = tstrchr(url, ':');
    if (p && p[1] == '/' && p[2] == '/') {
	prot.assign(url, p - url);
	url = p + 3;
	p = tstrchr(url, ':');
    } else if (p && !istdigit(p[1])) {
	return false;
    } else {
	prot = T("http");
    }
    pp = tstrchr(url, '/');
    if (p && (!pp || p < pp)) {
	port = (ushort)tstrtoul(p + 1, NULL, 10);
	if (!port)
	    return false;
	host.assign(url, p - url);
	p = tstrchr(p, '/');
    } else {
	port = 80;
	if (pp && pp != url)
	    host.assign(url, pp - url);
	else if (pp)
	    host = T("localhost");
	else
	    host = url;
	p = pp;
    }
    if (p) {
	pp = tstrchr(p, '?');
	if (pp) {
	    path.assign(p, pp - p);
	    query = pp + 1;
	    unescape(query);
	} else {
	    path = p;
	}
	unescape(path);
    } else {
	path = T("/");
    }
    return true;
}

void URL::unescape(tchar *str, bool plus) {
    for (tchar *p = str; *p; p++) {
	if (*p == '%') {
	    uint hex;

	    if (*++p <= '9')
		hex = *p - '0';
	    else
		hex = *p - 'A' + 10;
	    hex <<= 4;
	    if (*++p <= '9')
		hex += *p - '0';
	    else
		hex += *p - 'A' + 10;
	    *str++ = (tchar)hex;
	} else if (*p == '+' && plus) {
	    *str++ = ' ';
	} else {
	    *str++ = *p;
	}
    }
    *str = '\0';
}

void URL::unescape(tstring &str, bool plus) {
    uint i, j;

    for (i = 0, j = 0; j < str.size(); j++) {
	tchar p = str[j];

	if (p == '%') {
	    uint hex;

	    p = str[++j];
	    if (p <= '9')
		hex = p - '0';
	    else
		hex = p - 'A' + 10;
	    hex <<= 4;
	    p = str[++j];
	    if (p <= '9')
		hex += p - '0';
	    else
		hex += p - 'A' + 10;
	    str[i] = (tchar)hex;
	} else if (p == '+' && plus) {
	    str[i] = ' ';
	} else if (i != j) {
	    str[i] = p;
	}
	i++;
    }
    str.erase(i);
}

HTTPClient::HTTPClient(): ka(true), ressz(0), result(0), rto(90 * 1000),
    wto(60 * 1000), sstrm(StreamSize), sts(0), sz(0) {}

bool HTTPClient::connect(const Sockaddr &sa, bool keepalive, uint to) {
    if (sock.open() && sa == addr)
	return true;
    if (addr != sa)
	addr = sa;
    sock.close();
    ka = keepalive;
    if (!sock.connect(addr, to)) {
	dlog << Log::Info << T("mod=http cmd=connect addr=") << addr.host() <<
	    T(":") << (uint)addr.port() << T(" errno=") << sock.err() << endlog;
	sock.close();
	return false;
    }
    sock.rtimeout(rto);
    sock.wtimeout(wto);
    sstrm.rdbuf()->attach(sock);
    sstrm.rdbuf()->reset();
    sstrm.clear(sstrm.rdstate() & ~(ios::badbit | ios::eofbit | ios::failbit));
    DLOGD(T("mod=http cmd=connect addr=") << addr.host() << ':' << addr.port());
    return true;
}

bool HTTPClient::send(const tchar *op, const tchar *path, const void *data,
    size_t datasz) {
    char buf[32];
    bool first = true;
    size_t in;
    iovec iov[2];
    bool keep = false;
    const char *p, *pp;
    const tchar *resp;
    bool ret = false;
    string req, s, ss, sss;
    bool sent;
    msec_t start;
    static tchar connection[] = T("Connection");
    static tchar contentlen[] = T("Content-Length");
    static tchar keep_alive[] = T("Keep-Alive");
    static tchar pragma[] = T("Pragma");

    sts = 0;
    s.reserve(128);
    reshdrs.clear();
    req = tchartoachar(op);
    req += ' ';
    req += tchartoachar(path);
    req += " HTTP/1.1\r\nHost: ";
    req += tstringtoastring(addr.host());
    if (addr.port() != 80) {
	sprintf(buf, ":%u", addr.port());
	req += buf;
    }
    req += "\r\n";
    if (datasz) {
	sprintf(buf, "Content-Length: %lu\r\n", (ulong)datasz);
	req += buf;
    }
    if (ka)
	req += "Pragma: Keep-Alive\r\nConnection: Keep-Alive\r\n";
    if (hstrm.size())
	req += tchartoachar(hstrm.str());
    req += "\r\n";
    iov[0].iov_base = (char *)req.c_str();
    iov[0].iov_len = req.size();
    iov[1].iov_base = (char *)data;
    iov[1].iov_len = datasz;
loop:
    if (!connect(addr, ka))
	goto done;
    start = milliticks();
    if ((sent = ((ulong)sock.writev(iov, 2) == (ulong)(req.size() + datasz))) ==
	false ||
	// shutdown causes huge cpu spikes on NT - not sure why
#if 0
	(!ka && !sock.shutdown(false, true)) ||
#endif
	!getline(sstrm, s)) {
	sock.close();
	if (first && ka && (!sent || rto - (milliticks() - start) > 200)) {
	    dlog << Log::Debug << T("mod=http cmd=reconnect") << endlog;
	    sstrm.seekp(0, ios::beg);
	    first = false;
	    goto loop;
	} else {
	    dlog << Log::Note << T("mod=http cmd=disconnect") << endlog;
	    goto done;
	}
    }
    p = s.c_str();
    while (*p != ' ' && *p != '\t')
	p++;
    while (*p == ' ' || *p == '\t')
	p++;
    sts = atoi(p);
    dlog << Log::Debug << T("mod=http status=") << sts << endlog;
    while (getline(sstrm, s)) {		    // does not support folded hdrs
	p = s.c_str();
	while (*p == ' ' || *p == '\t')
	    p++;
	if (*p == '\r' || !*p)
	    break;
	if ((pp = strchr(p, ':')) == NULL)
	    continue;
	ss = s.substr(p - s.c_str(), pp - p);
	do {
	    pp++;
	} while (*pp && (*pp == '\r' || *pp == ' ' || *pp == '\t'));
	sss.assign(pp, strlen(pp) - 1);

	pair<tstring, tstring> pr(astringtotstring(ss), astringtotstring(sss));

	reshdrs.insert(pr);
    }
    if (!sstrm)
	goto done;
    if (ka) {
	if ((resp = response(connection)) != NULL &&
	    !tstrnicmp(resp, keep_alive, sizeof (keep_alive) - 1))
	    keep = true;
	else if ((resp = response(pragma)) != NULL &&
	    !tstrnicmp(resp, keep_alive, sizeof (keep_alive) - 1))
	    keep = true;
    }
    if ((resp = response(contentlen)) == NULL)
	ressz = (ulong)-1;
    else
	ressz = tstrtoul(resp, NULL, 10);
    if (keep && !sz && ressz != (ulong)-1)
	sock.nodelay(true);
    if (ressz && ressz != (ulong)-1) {
	if (ressz > sz) {
	    delete [] result;
	    sz = ressz;
	    result = new char[sz + 1];
	}
	ret = (ulong)sstrm.read(result, ressz) == ressz;
    } else if (ressz) {
	char *newres;
	size_t room = sz;

	keep = false;
	ressz = 0;
	for (;;) {
	    if (!room) {
		room = ressz ? ressz : 12 * 1024;
		sz = ressz + room;
		newres = new char[sz + 1];
		memcpy(newres, result, ressz);
		delete [] result;
		result = newres;
	    }
	    if ((long)(in = (size_t)sstrm.read(result + ressz, room)) > 0) {
		ressz += in;
		ret = true;
	    }
	    if (in == (size_t)-1 || in < room)
		break;
	    room -= in;
	}
    } else {
	ret = true;
    }
done:
    if (!keep || !ret)
	sock.close();
    if (ret && result)
	result[ressz] = '\0';
    hstrm.reset();
    return ret;
}

tostream &HTTPClient::operator <<(tostream &os) {
    attrmap::const_iterator it;

    os << sts << endl;
    for (it = reshdrs.begin(); it != reshdrs.end(); ++it)
	os << it->first << ": " << it->second << endl;
    os.write(achartotchar(result), ressz);
    os << endl;
    return os;
}
<|endoftext|>


<commit_before>/*
# Software License Agreement (MIT License)
#
# Copyright (c) 2020, UFACTORY, Inc.
# All rights reserved.
#
# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
*/

#include "xarm/core/instruction/uxbus_cmd_tcp.h"
#include "xarm/core/debug/debug_print.h"
#include "xarm/core/instruction/uxbus_cmd_config.h"

UxbusCmdTcp::UxbusCmdTcp(SocketPort *arm_port) {
	arm_port_ = arm_port;
	bus_flag_ = TX2_BUS_FLAG_MIN_;
	prot_flag_ = TX2_PROT_CON_;
}

UxbusCmdTcp::~UxbusCmdTcp(void) {}

int UxbusCmdTcp::get_prot_flag(void)
{
	return prot_flag_;
}

int UxbusCmdTcp::set_prot_flag(int prot_flag)
{
	std::lock_guard<std::mutex> locker(mutex_);
	if (prot_flag_ != prot_flag || TX2_PROT_CON_ != prot_flag) {
		TX2_PROT_CON_ = prot_flag;
		prot_flag_ = prot_flag;
		printf("change prot_flag to %d\n", prot_flag_);
	}
	return 0;
}

int UxbusCmdTcp::check_xbus_prot(unsigned char *datas, int funcode) {
	unsigned char *data_fp = &datas[4];

	int sizeof_data = bin8_to_32(datas);
	if (sizeof_data < 8 || sizeof_data >= arm_port_->que_maxlen)
	{
		return UXBUS_STATE::ERR_LENG;
	}

	int num = bin8_to_16(&data_fp[0]);
	int prot = bin8_to_16(&data_fp[2]);
	int len = bin8_to_16(&data_fp[4]);
	int fun = data_fp[6];
	int state = data_fp[7];

	int bus_flag = bus_flag_;
	if (bus_flag == TX2_BUS_FLAG_MIN_)
	{
		bus_flag = TX2_BUS_FLAG_MAX_;
	}
	else
	{
		bus_flag -= 1;
	}

	if (num != bus_flag) { return UXBUS_STATE::ERR_NUM; }
	if (prot != TX2_PROT_CON_) { return UXBUS_STATE::ERR_PROT; }
	if (fun != funcode) { return UXBUS_STATE::ERR_FUN; }
	state_is_ready = !(state & 0x10);
	if (state & 0x08) { return UXBUS_STATE::INVALID; }
	if (state & 0x40) { return UXBUS_STATE::ERR_CODE; }
	if (state & 0x20) { return UXBUS_STATE::WAR_CODE; }
	if (sizeof_data != len + 6) { return UXBUS_STATE::ERR_LENG; }
	// if (state & 0x10) { return UXBUS_STATE::STATE_NOT_READY; }
	return 0;
}

int UxbusCmdTcp::send_pend(int funcode, int num, int timeout, unsigned char *ret_data) {
	int i;
	int ret = UXBUS_STATE::ERR_TOUT;
	int ret2;
	// unsigned char rx_data[arm_port_->que_maxlen] = {0};
	unsigned char *rx_data = new unsigned char[arm_port_->que_maxlen];
	long long expired = get_system_time() + (long long)timeout;
	while (get_system_time() < expired) {
		ret2 = arm_port_->read_frame(rx_data);
		if (ret2 != -1) {
			// print_hex("recv:", rx_data, arm_port_->que_maxlen);
			last_recv_ms = get_system_time();
			ret = check_xbus_prot(rx_data, funcode);
			if (ret == 0 || ret == UXBUS_STATE::ERR_CODE || ret == UXBUS_STATE::WAR_CODE) {
				int n = num;
				if (num == -1) {
					n = bin8_to_16(&rx_data[8]) - 2;
				}
				for (i = 0; i < n; i++) { ret_data[i] = rx_data[i + 8 + 4]; }
				// print_hex(" 3", rx_data, num + 8 + 4);
				break;
			}
			else if (ret != UXBUS_STATE::ERR_NUM) {
				break;
			}
		}
		sleep_us(500);
	}
	delete[] rx_data;
	return ret;
}

int UxbusCmdTcp::send_xbus(int funcode, unsigned char *datas, int num) {
	int len = num + 7;
	// unsigned char send_data[len];
	unsigned char *send_data = new unsigned char[len];

	bin16_to_8(bus_flag_, &send_data[0]);
	bin16_to_8(prot_flag_, &send_data[2]);
	bin16_to_8(num + 1, &send_data[4]);
	send_data[6] = funcode;

	for (int i = 0; i < num; i++) { send_data[7 + i] = datas[i]; }
	arm_port_->flush();
	// print_hex("send:", send_data, num + 7);
	int ret = arm_port_->write_frame(send_data, len);
	delete[] send_data;
	if (ret != len) { return -1; }

	bus_flag_ += 1;
	if (bus_flag_ > TX2_BUS_FLAG_MAX_) { bus_flag_ = TX2_BUS_FLAG_MIN_; }

	return 0;
}

void UxbusCmdTcp::close(void) { arm_port_->close_port(); }

int UxbusCmdTcp::is_ok(void) { return arm_port_->is_ok(); }
<commit_msg>[fix] fix the code(5) of recv response<commit_after>/*
# Software License Agreement (MIT License)
#
# Copyright (c) 2020, UFACTORY, Inc.
# All rights reserved.
#
# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
*/

#include "xarm/core/instruction/uxbus_cmd_tcp.h"
#include "xarm/core/debug/debug_print.h"
#include "xarm/core/instruction/uxbus_cmd_config.h"

UxbusCmdTcp::UxbusCmdTcp(SocketPort *arm_port) {
	arm_port_ = arm_port;
	bus_flag_ = TX2_BUS_FLAG_MIN_;
	prot_flag_ = TX2_PROT_CON_;
}

UxbusCmdTcp::~UxbusCmdTcp(void) {}

int UxbusCmdTcp::get_prot_flag(void)
{
	return prot_flag_;
}

int UxbusCmdTcp::set_prot_flag(int prot_flag)
{
	std::lock_guard<std::mutex> locker(mutex_);
	if (prot_flag_ != prot_flag || TX2_PROT_CON_ != prot_flag) {
		TX2_PROT_CON_ = prot_flag;
		prot_flag_ = prot_flag;
		printf("change prot_flag to %d\n", prot_flag_);
	}
	return 0;
}

int UxbusCmdTcp::check_xbus_prot(unsigned char *datas, int funcode) {
	unsigned char *data_fp = &datas[4];

	int sizeof_data = bin8_to_32(datas);
	if (sizeof_data < 8 || sizeof_data >= arm_port_->que_maxlen)
	{
		return UXBUS_STATE::ERR_LENG;
	}

	int num = bin8_to_16(&data_fp[0]);
	int prot = bin8_to_16(&data_fp[2]);
	int len = bin8_to_16(&data_fp[4]);
	int fun = data_fp[6];
	int state = data_fp[7];

	int bus_flag = bus_flag_;
	if (bus_flag == TX2_BUS_FLAG_MIN_)
	{
		bus_flag = TX2_BUS_FLAG_MAX_;
	}
	else
	{
		bus_flag -= 1;
	}

	if (num != bus_flag) { return UXBUS_STATE::ERR_NUM; }
	if (prot != TX2_PROT_CON_) { return UXBUS_STATE::ERR_PROT; }
	if (fun != funcode) { return UXBUS_STATE::ERR_FUN; }
	state_is_ready = !(state & 0x10);
	if (state & 0x08) { return UXBUS_STATE::INVALID; }
	if (state & 0x40) { return UXBUS_STATE::ERR_CODE; }
	if (state & 0x20) { return UXBUS_STATE::WAR_CODE; }
	if (sizeof_data != len + 6) { return UXBUS_STATE::ERR_LENG; }
	// if (state & 0x10) { return UXBUS_STATE::STATE_NOT_READY; }
	return 0;
}

int UxbusCmdTcp::send_pend(int funcode, int num, int timeout, unsigned char *ret_data) {
	int i;
	int ret = UXBUS_STATE::ERR_TOUT;
	int code;
	// unsigned char rx_data[arm_port_->que_maxlen] = {0};
	unsigned char *rx_data = new unsigned char[arm_port_->que_maxlen];
	long long expired = get_system_time() + (long long)timeout;
	while (get_system_time() < expired) {
		code = arm_port_->read_frame(rx_data);
		if (code != -1) {
			// print_hex("recv:", rx_data, arm_port_->que_maxlen);
			last_recv_ms = get_system_time();
			code = check_xbus_prot(rx_data, funcode);
			if (code == 0 || code == UXBUS_STATE::ERR_CODE || code == UXBUS_STATE::WAR_CODE) {
				ret = code;
				int n = num;
				if (num == -1) {
					n = bin8_to_16(&rx_data[8]) - 2;
				}
				for (i = 0; i < n; i++) { ret_data[i] = rx_data[i + 8 + 4]; }
				// print_hex(" 3", rx_data, num + 8 + 4);
				break;
			}
			else if (ret != UXBUS_STATE::ERR_NUM) {
				ret = code;
				break;
			}
		}
		sleep_us(500);
	}
	delete[] rx_data;
	return ret;
}

int UxbusCmdTcp::send_xbus(int funcode, unsigned char *datas, int num) {
	int len = num + 7;
	// unsigned char send_data[len];
	unsigned char *send_data = new unsigned char[len];

	bin16_to_8(bus_flag_, &send_data[0]);
	bin16_to_8(prot_flag_, &send_data[2]);
	bin16_to_8(num + 1, &send_data[4]);
	send_data[6] = funcode;

	for (int i = 0; i < num; i++) { send_data[7 + i] = datas[i]; }
	arm_port_->flush();
	// print_hex("send:", send_data, num + 7);
	int ret = arm_port_->write_frame(send_data, len);
	delete[] send_data;
	if (ret != len) { return -1; }

	bus_flag_ += 1;
	if (bus_flag_ > TX2_BUS_FLAG_MAX_) { bus_flag_ = TX2_BUS_FLAG_MIN_; }

	return 0;
}

void UxbusCmdTcp::close(void) { arm_port_->close_port(); }

int UxbusCmdTcp::is_ok(void) { return arm_port_->is_ok(); }
<|endoftext|>


<commit_before>#include "SetProxy.h"
#include "WebPage.h"
#include "WebPageManager.h"
#include "NetworkAccessManager.h"
#include <QNetworkProxy>

SetProxy::SetProxy(WebPageManager *manager, QStringList &arguments, QObject *parent) : SocketCommand(manager, arguments, parent) {}

void SetProxy::start()
{
  // default to empty proxy
  QNetworkProxy proxy;

  if (arguments().size() > 0) {
    proxy.setType(arguments()[4] == 'socks' ? QNetworkProxy::Socks5Proxy : QNetworkProxy::HttpProxy);
    proxy.setHostName(arguments()[0]);
    proxy.setPort((quint16)(arguments()[1].toInt()));
    proxy.setUser(arguments()[2]);
    proxy.setPassword(arguments()[3]);
  }

  manager()->networkAccessManager()->setProxy(proxy);
  finish(true);
}
<commit_msg>proxy set<commit_after>#include "SetProxy.h"
#include "WebPage.h"
#include "WebPageManager.h"
#include "NetworkAccessManager.h"
#include <QNetworkProxy>

SetProxy::SetProxy(WebPageManager *manager, QStringList &arguments, QObject *parent) : SocketCommand(manager, arguments, parent) {}

void SetProxy::start()
{
  // default to empty proxy
  QNetworkProxy proxy;

  if (arguments().size() > 0) {
    proxy.setType((arguments()[4] == "socks") ? QNetworkProxy::Socks5Proxy : QNetworkProxy::HttpProxy);
    proxy.setHostName(arguments()[0]);
    proxy.setPort((quint16)(arguments()[1].toInt()));
    proxy.setUser(arguments()[2]);
    proxy.setPassword(arguments()[3]);
  }

  manager()->networkAccessManager()->setProxy(proxy);
  finish(true);
}
<|endoftext|>


<commit_before>#include <TSRChain.h>

#include <limits>
#include <vector>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>

using namespace or_ompl;

TSRChain::TSRChain() : _initialized(false), _sample_start(false), _sample_goal(false), _constrain(false), _tsr_robot(TSRRobot::Ptr()) {

}

TSRChain::TSRChain(const bool &sample_start,
				   const bool &sample_goal,
				   const bool &constrain,
				   const std::vector<TSR::Ptr> &tsrs) :
	_initialized(true), _sample_start(sample_start), _sample_goal(sample_goal),
	_constrain(constrain), _tsrs(tsrs), _tsr_robot(TSRRobot::Ptr()) {

}

bool TSRChain::deserialize(std::stringstream &ss) {

	ss >> _sample_start;
	ss >> _sample_goal;
	ss >> _constrain;

	int num_tsrs;
	ss >> num_tsrs;

	_tsrs.resize(num_tsrs);
    bool valid = true;
	for(unsigned int idx = 0; idx < num_tsrs; idx++){
		TSR::Ptr new_tsr = boost::make_shared<TSR>();
		valid &= new_tsr->deserialize(ss);
		_tsrs[idx] = new_tsr;
	}

    return valid;

	// TODO: Ignored are mmicbody name and mimicbodyjoints
}

Eigen::Affine3d TSRChain::sample() const {

	Eigen::Affine3d T0_w;
	if(_tsrs.size() == 0){
		RAVELOG_ERROR("[TSRChain] No TSRs specified for this chain");
		return T0_w;
	}

	T0_w = _tsrs.front()->getOriginTransform();
	BOOST_FOREACH(TSR::Ptr tsr, _tsrs){
		T0_w  = T0_w * tsr->sampleDisplacementTransform() * tsr->getEndEffectorOffsetTransform();
	}

	return T0_w;
}

Eigen::Matrix<double, 6, 1> TSRChain::distance(const Eigen::Affine3d &ee_pose) const {

	if(_tsrs.size() == 1){
		TSR::Ptr tsr = _tsrs.front();
		return tsr->distance(ee_pose);
	}

    if(!_tsr_robot){
        RAVELOG_ERROR("[TSRChain] Failed to compute distance to TSR chain. Did you set an OpenRAVE::Environment using the setEnv function?");
        return std::numeric_limits<double>::infinity() * Eigen::Matrix<double, 6, 1>::Ones();
    }

    if(!_tsr_robot->construct()){
        RAVELOG_ERROR("[TSRChain] Failed to robotize TSR. Cannot compute distance.");
        return std::numeric_limits<double>::infinity() * Eigen::Matrix<double, 6, 1>::Ones();
    }

    RAVELOG_DEBUG("[TSRChain] Solving IK to compute distance");
    // Compute the ideal pose of the end-effector
    Eigen::Affine3d Ttarget = ee_pose * _tsrs.back()->getEndEffectorOffsetTransform().inverse();

    // Ask the robot to solve ik to find the closest possible end-effector transform
    Eigen::Affine3d Tnear = _tsr_robot->findNearestFeasibleTransform(Ttarget);

    // Compute the distance between the two
    Eigen::Affine3d offset = Tnear.inverse() * Ttarget;
    Eigen::Matrix<double, 6, 1> dist = Eigen::Matrix<double, 6, 1>::Zero();
    dist(0,0) = offset.translation()(0);
	dist(1,0) = offset.translation()(1);
	dist(2,0) = offset.translation()(2);
	dist(3,0) = atan2(offset.rotation()(2,1), offset.rotation()(2,2));
	dist(4,0) = -asin(offset.rotation()(2,0));
	dist(5,0) = atan2(offset.rotation()(1,0), offset.rotation()(0,0));

    return dist;
}

void TSRChain::setEnv(const OpenRAVE::EnvironmentBasePtr &penv){

    _tsr_robot = boost::make_shared<TSRRobot>(_tsrs, penv);

}
<commit_msg>Switched to using bracket notation to index vector<commit_after>#include <TSRChain.h>

#include <limits>
#include <vector>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>

using namespace or_ompl;

TSRChain::TSRChain() : _initialized(false), _sample_start(false), _sample_goal(false), _constrain(false), _tsr_robot(TSRRobot::Ptr()) {

}

TSRChain::TSRChain(const bool &sample_start,
				   const bool &sample_goal,
				   const bool &constrain,
				   const std::vector<TSR::Ptr> &tsrs) :
	_initialized(true), _sample_start(sample_start), _sample_goal(sample_goal),
	_constrain(constrain), _tsrs(tsrs), _tsr_robot(TSRRobot::Ptr()) {

}

bool TSRChain::deserialize(std::stringstream &ss) {

	ss >> _sample_start;
	ss >> _sample_goal;
	ss >> _constrain;

	int num_tsrs;
	ss >> num_tsrs;

	_tsrs.resize(num_tsrs);
    bool valid = true;
	for(unsigned int idx = 0; idx < num_tsrs; idx++){
		TSR::Ptr new_tsr = boost::make_shared<TSR>();
		valid &= new_tsr->deserialize(ss);
		_tsrs[idx] = new_tsr;
	}

    return valid;

	// TODO: Ignored are mmicbody name and mimicbodyjoints
}

Eigen::Affine3d TSRChain::sample() const {

	Eigen::Affine3d T0_w;
	if(_tsrs.size() == 0){
		RAVELOG_ERROR("[TSRChain] No TSRs specified for this chain");
		return T0_w;
	}

	T0_w = _tsrs.front()->getOriginTransform();
	BOOST_FOREACH(TSR::Ptr tsr, _tsrs){
		T0_w  = T0_w * tsr->sampleDisplacementTransform() * tsr->getEndEffectorOffsetTransform();
	}

	return T0_w;
}

Eigen::Matrix<double, 6, 1> TSRChain::distance(const Eigen::Affine3d &ee_pose) const {

	if(_tsrs.size() == 1){
		TSR::Ptr tsr = _tsrs.front();
		return tsr->distance(ee_pose);
	}

    if(!_tsr_robot){
        RAVELOG_ERROR("[TSRChain] Failed to compute distance to TSR chain. Did you set an OpenRAVE::Environment using the setEnv function?");
        return std::numeric_limits<double>::infinity() * Eigen::Matrix<double, 6, 1>::Ones();
    }

    if(!_tsr_robot->construct()){
        RAVELOG_ERROR("[TSRChain] Failed to robotize TSR. Cannot compute distance.");
        return std::numeric_limits<double>::infinity() * Eigen::Matrix<double, 6, 1>::Ones();
    }

    RAVELOG_DEBUG("[TSRChain] Solving IK to compute distance");
    // Compute the ideal pose of the end-effector
    Eigen::Affine3d Ttarget = ee_pose * _tsrs.back()->getEndEffectorOffsetTransform().inverse();

    // Ask the robot to solve ik to find the closest possible end-effector transform
    Eigen::Affine3d Tnear = _tsr_robot->findNearestFeasibleTransform(Ttarget);

    // Compute the distance between the two
    Eigen::Affine3d offset = Tnear.inverse() * Ttarget;
    Eigen::Matrix<double, 6, 1> dist = Eigen::Matrix<double, 6, 1>::Zero();
    dist[0] = offset.translation()[0];
	dist[1] = offset.translation()[1];
	dist[2] = offset.translation()[2];
	dist[3] = atan2(offset.rotation()(2,1), offset.rotation()(2,2));
	dist[4] = -asin(offset.rotation()(2,0));
	dist[5] = atan2(offset.rotation()(1,0), offset.rotation()(0,0));

    return dist;
}

void TSRChain::setEnv(const OpenRAVE::EnvironmentBasePtr &penv){

    _tsr_robot = boost::make_shared<TSRRobot>(_tsrs, penv);

}
<|endoftext|>


<commit_before>
#include "ksp_plugin/vessel.hpp"

#include <algorithm>
#include <list>
#include <vector>

#include "integrators/embedded_explicit_runge_kutta_nyström_integrator.hpp"
#include "ksp_plugin/pile_up.hpp"
#include "quantities/si.hpp"
#include "testing_utilities/make_not_null.hpp"

namespace principia {
namespace ksp_plugin {
namespace internal_vessel {

using base::FindOrDie;
using base::make_not_null_unique;
using geometry::BarycentreCalculator;
using geometry::Position;
using integrators::DormandElMikkawyPrince1986RKN434FM;
using integrators::McLachlanAtela1992Order5Optimal;
using quantities::IsFinite;
using quantities::Time;
using quantities::si::Kilogram;
using quantities::si::Metre;
using quantities::si::Milli;
using quantities::si::Second;

Vessel::Vessel(not_null<Celestial const*> const parent,
               not_null<Ephemeris<Barycentric>*> const ephemeris,
               Ephemeris<Barycentric>::AdaptiveStepParameters const&
                   prediction_adaptive_step_parameters)
    : body_(),
      prediction_adaptive_step_parameters_(prediction_adaptive_step_parameters),
      parent_(parent),
      ephemeris_(ephemeris),
      psychohistory_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()),
      prediction_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()) {}

not_null<MasslessBody const*> Vessel::body() const {
  return &body_;
}

not_null<Celestial const*> Vessel::parent() const {
  return parent_;
}

void Vessel::set_parent(not_null<Celestial const*> const parent) {
  parent_ = parent;
}

void Vessel::AddPart(not_null<std::unique_ptr<Part>> part) {
  PartId const id = part->part_id();
  LOG(INFO) << "Adding part " << id << " to vessel at " << this;
  parts_.emplace(id, std::move(part));
  kept_parts_.insert(part.get());
}

not_null<std::unique_ptr<Part>> Vessel::ExtractPart(PartId const id) {
  LOG(INFO) << "Extracting part " << id << " from vessel at " << this;
  auto const it = parts_.find(id);
  CHECK(it != parts_.end()) << id;
  auto result = std::move(it->second);
  parts_.erase(it);
  return result;
}

void Vessel::KeepPart(PartId const id) {
  LOG(ERROR)<<"Keeping part " << id << " in vessel at " << this;
  kept_parts_.insert(FindOrDie(parts_, id).get());
}

void Vessel::FreeParts() {
  LOG(ERROR)<<"FreeParts on vessel at " << this;
  dummy_part_.reset();
  for (auto it = parts_.begin(); it != parts_.end();) {
    not_null<Part const*> part = it->second.get();
    if (kept_parts_.count(part) == 0) {
      it = parts_.erase(it);
    } else {
      ++it;
    }
  }
  CHECK(!parts_.empty());
  kept_parts_.clear();
}

not_null<Part*> Vessel::part(PartId const id) const {
  return FindOrDie(parts_, id).get();
}

void Vessel::ForSomePart(std::function<void(Part&)> action) const {
  CHECK(!parts_.empty());
  action(*parts_.begin()->second);
}

void Vessel::ForAllParts(std::function<void(Part&)> action) const {
  for (auto const& pair : parts_) {
    action(*pair.second);
  }
}

DiscreteTrajectory<Barycentric> const& Vessel::prediction() const {
  return *prediction_;
}

void Vessel::set_prediction_adaptive_step_parameters(
    Ephemeris<Barycentric>::AdaptiveStepParameters const&
        prediction_adaptive_step_parameters) {
  prediction_adaptive_step_parameters_ = prediction_adaptive_step_parameters;
}

Ephemeris<Barycentric>::AdaptiveStepParameters const&
Vessel::prediction_adaptive_step_parameters() const {
  return prediction_adaptive_step_parameters_;
}

FlightPlan& Vessel::flight_plan() const {
  CHECK(has_flight_plan());
  return *flight_plan_;
}

bool Vessel::has_flight_plan() const {
  return flight_plan_ != nullptr;
}

void Vessel::AdvanceTime(Instant const& time) {
  std::vector<DiscreteTrajectory<Barycentric>::Iterator> its;
  for (auto const& pair : parts_) {
    Part const& part = *pair.second;
    its.push_back(part.tail().Begin());
  }
  for (;;) {
    Part const& first_part = *parts_.begin()->second;
    Instant const time = its[0].time();
    bool const at_end_of_tail = its[0] == first_part.tail().last();
    bool const tail_is_authoritative = first_part.tail_is_authoritative();

    BarycentreCalculator<DegreesOfFreedom<Barycentric>, Mass> calculator;
    int i = 0;
    for (auto const& pair : parts_) {
      Part& part = *pair.second;
      auto& it = its[i];
      calculator.Add(it.degrees_of_freedom(), part.mass());
      CHECK_EQ(time, it.time());
      CHECK_EQ(at_end_of_tail, it == part.tail().last());
      CHECK_EQ(tail_is_authoritative, part.tail_is_authoritative());
      if (at_end_of_tail) {
        part.tail().ForgetBefore(astronomy::InfiniteFuture);
      } else {
        ++it;
      }
      ++i;
    }
    DegreesOfFreedom<Barycentric> const vessel_degrees_of_freedom =
        calculator.Get();
    AppendToPsychohistory(
        time,
        vessel_degrees_of_freedom,
        /*authoritative=*/!at_end_of_tail || tail_is_authoritative);

    if (at_end_of_tail) {
      return;
    }
  }
}

void Vessel::ForgetBefore(Instant const& time) {
  // TODO(egg): do something about the psychohistory. I think bad things happen
  // if it becomes empty though.
  prediction_->ForgetBefore(time);
  if (flight_plan_ != nullptr) {
    flight_plan_->ForgetBefore(time, [this]() { flight_plan_.reset(); });
  }
}

void Vessel::CreateFlightPlan(
    Instant const& final_time,
    Mass const& initial_mass,
    Ephemeris<Barycentric>::AdaptiveStepParameters const&
        flight_plan_adaptive_step_parameters) {
  auto const last = last_authoritative();
  flight_plan_ = std::make_unique<FlightPlan>(
      initial_mass,
      /*initial_time=*/last.time(),
      /*initial_degrees_of_freedom=*/last.degrees_of_freedom(),
      final_time,
      ephemeris_,
      flight_plan_adaptive_step_parameters);
}

void Vessel::DeleteFlightPlan() {
  flight_plan_.reset();
}

void Vessel::UpdatePrediction(Instant const& last_time) {
  prediction_ = make_not_null_unique<DiscreteTrajectory<Barycentric>>();
  auto const last = psychohistory_->last();
  prediction_->Append(last.time(), last.degrees_of_freedom());
  FlowPrediction(last_time);
}

DiscreteTrajectory<Barycentric> const& Vessel::psychohistory() const {
  return *psychohistory_;
}

bool Vessel::psychohistory_is_authoritative() const {
  return psychohistory_is_authoritative_;
}

void Vessel::WriteToMessage(
    not_null<serialization::Vessel*> const message) const {
  // TODO(phl): Implement.
  body_.WriteToMessage(message->mutable_body());
  prediction_adaptive_step_parameters_.WriteToMessage(
      message->mutable_prediction_adaptive_step_parameters());
  if (dummy_part_ != nullptr) {
    dummy_part_->WriteToMessage(message->mutable_dummy_part());
  }
  for (auto const& pair : parts_) {
    auto const& part = pair.second;
    part->WriteToMessage(message->add_parts());
  }
  for (auto const& part : kept_parts_) {
    message->add_kept_parts(part->part_id());
  }
  psychohistory_->WriteToMessage(message->mutable_psychohistory(),
                                 /*forks=*/{});
  message->set_psychohistory_is_authoritative(psychohistory_is_authoritative_);
  prediction_->WriteToMessage(message->mutable_prediction(),
                              /*forks=*/{});
  if (flight_plan_ != nullptr) {
    flight_plan_->WriteToMessage(message->mutable_flight_plan());
  }
}

not_null<std::unique_ptr<Vessel>> Vessel::ReadFromMessage(
    serialization::Vessel const& message,
    not_null<Celestial const*> const parent,
    not_null<Ephemeris<Barycentric>*> const ephemeris,
    std::function<void(PartId)> const& deletion_callback) {
  // NOTE(egg): for now we do not read the |MasslessBody| as it can contain no
  // information.
  auto vessel = make_not_null_unique<Vessel>(
      parent,
      ephemeris,
      Ephemeris<Barycentric>::AdaptiveStepParameters::ReadFromMessage(
          message.prediction_adaptive_step_parameters()));
  if (message.has_dummy_part()) {
    vessel->dummy_part_ = Part::ReadFromMessage(message.dummy_part(),
                                                /*deletion_callback=*/nullptr);
  }
  for (auto const& serialized_part : message.parts()) {
    PartId const part_id = serialized_part.part_id();
    auto part = Part::ReadFromMessage(
        serialized_part,
        [deletion_callback, part_id]() { deletion_callback(part_id); });
    vessel->parts_.emplace(part_id, std::move(part));
  }
  for (PartId const part_id : message.kept_parts()) {
    vessel->kept_parts_.insert(vessel->parts_.at(part_id).get());
  }
  vessel->psychohistory_ =
      DiscreteTrajectory<Barycentric>::ReadFromMessage(message.psychohistory(),
                                                       /*forks=*/{});
  vessel->psychohistory_is_authoritative_ =
      message.psychohistory_is_authoritative();
  vessel->prediction_ =
      DiscreteTrajectory<Barycentric>::ReadFromMessage(message.prediction(),
                                                       /*forks=*/{});
  if (message.has_flight_plan()) {
    // TODO(phl): Remove compatibility code.  The |root| is only needed
    // pre-Буняковский.
    vessel->flight_plan_ = FlightPlan::ReadFromMessage(
        message.flight_plan(), /*root=*/nullptr, ephemeris);
  }
  return std::move(vessel);
}

Vessel::Vessel()
    : body_(),
      prediction_adaptive_step_parameters_(DefaultPredictionParameters()),
      parent_(testing_utilities::make_not_null<Celestial const*>()),
      ephemeris_(testing_utilities::make_not_null<Ephemeris<Barycentric>*>()),
      psychohistory_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()),
      prediction_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()) {}

void Vessel::AppendToPsychohistory(
    Instant const& time,
    DegreesOfFreedom<Barycentric> const& degrees_of_freedom,
    bool const authoritative) {
  if (!psychohistory_is_authoritative_) {
    psychohistory_->ForgetAfter((--psychohistory_->last()).time());
  }
  psychohistory_->Append(time, degrees_of_freedom);
  psychohistory_is_authoritative_ = authoritative;
}

void Vessel::FlowPrediction(Instant const& time) {
  if (time > prediction_->last().time()) {
    bool const finite_time = IsFinite(time - prediction_->last().time());
    Instant const t = finite_time ? time : ephemeris_->t_max();
    // This will not prolong the ephemeris if |time| is infinite (but it may do
    // so if it is finite).
    bool const reached_t = ephemeris_->FlowWithAdaptiveStep(
        prediction_.get(),
        Ephemeris<Barycentric>::NoIntrinsicAcceleration,
        t,
        prediction_adaptive_step_parameters_,
        FlightPlan::max_ephemeris_steps_per_frame,
        /*last_point_only=*/false);
    if (!finite_time && reached_t) {
      // This will prolong the ephemeris by |max_ephemeris_steps_per_frame|.
      ephemeris_->FlowWithAdaptiveStep(
        prediction_.get(),
        Ephemeris<Barycentric>::NoIntrinsicAcceleration,
        time,
        prediction_adaptive_step_parameters_,
        FlightPlan::max_ephemeris_steps_per_frame,
        /*last_point_only=*/false);
    }
  }
}

DiscreteTrajectory<Barycentric>::Iterator Vessel::last_authoritative() const {
  auto it = psychohistory_->last();
  if (!psychohistory_is_authoritative_) {
    --it;
  }
  return it;
}

Ephemeris<Barycentric>::FixedStepParameters DefaultHistoryParameters() {
  return Ephemeris<Barycentric>::FixedStepParameters(
             McLachlanAtela1992Order5Optimal<Position<Barycentric>>(),
             /*step=*/10 * Second);
}

Ephemeris<Barycentric>::AdaptiveStepParameters DefaultProlongationParameters() {
  return Ephemeris<Barycentric>::AdaptiveStepParameters(
             DormandElMikkawyPrince1986RKN434FM<Position<Barycentric>>(),
             /*max_steps=*/std::numeric_limits<std::int64_t>::max(),
             /*length_integration_tolerance=*/1 * Milli(Metre),
             /*speed_integration_tolerance=*/1 * Milli(Metre) / Second);
}

Ephemeris<Barycentric>::AdaptiveStepParameters DefaultPredictionParameters() {
  return Ephemeris<Barycentric>::AdaptiveStepParameters(
             DormandElMikkawyPrince1986RKN434FM<Position<Barycentric>>(),
             /*max_steps=*/1000,
             /*length_integration_tolerance=*/1 * Metre,
             /*speed_integration_tolerance=*/1 * Metre / Second);
}

}  // namespace internal_vessel
}  // namespace ksp_plugin
}  // namespace principia
<commit_msg>use-after-move!<commit_after>
#include "ksp_plugin/vessel.hpp"

#include <algorithm>
#include <list>
#include <vector>

#include "integrators/embedded_explicit_runge_kutta_nyström_integrator.hpp"
#include "ksp_plugin/pile_up.hpp"
#include "quantities/si.hpp"
#include "testing_utilities/make_not_null.hpp"

namespace principia {
namespace ksp_plugin {
namespace internal_vessel {

using base::FindOrDie;
using base::make_not_null_unique;
using geometry::BarycentreCalculator;
using geometry::Position;
using integrators::DormandElMikkawyPrince1986RKN434FM;
using integrators::McLachlanAtela1992Order5Optimal;
using quantities::IsFinite;
using quantities::Time;
using quantities::si::Kilogram;
using quantities::si::Metre;
using quantities::si::Milli;
using quantities::si::Second;

Vessel::Vessel(not_null<Celestial const*> const parent,
               not_null<Ephemeris<Barycentric>*> const ephemeris,
               Ephemeris<Barycentric>::AdaptiveStepParameters const&
                   prediction_adaptive_step_parameters)
    : body_(),
      prediction_adaptive_step_parameters_(prediction_adaptive_step_parameters),
      parent_(parent),
      ephemeris_(ephemeris),
      psychohistory_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()),
      prediction_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()) {}

not_null<MasslessBody const*> Vessel::body() const {
  return &body_;
}

not_null<Celestial const*> Vessel::parent() const {
  return parent_;
}

void Vessel::set_parent(not_null<Celestial const*> const parent) {
  parent_ = parent;
}

void Vessel::AddPart(not_null<std::unique_ptr<Part>> part) {
  PartId const id = part->part_id();
  LOG(INFO) << "Adding part " << id << " to vessel at " << this;
  kept_parts_.insert(part.get());
  parts_.emplace(id, std::move(part));
}

not_null<std::unique_ptr<Part>> Vessel::ExtractPart(PartId const id) {
  LOG(INFO) << "Extracting part " << id << " from vessel at " << this;
  auto const it = parts_.find(id);
  CHECK(it != parts_.end()) << id;
  auto result = std::move(it->second);
  parts_.erase(it);
  return result;
}

void Vessel::KeepPart(PartId const id) {
  kept_parts_.insert(FindOrDie(parts_, id).get());
}

void Vessel::FreeParts() {
  dummy_part_.reset();
  for (auto it = parts_.begin(); it != parts_.end();) {
    not_null<Part const*> part = it->second.get();
    if (kept_parts_.count(part) == 0) {
      it = parts_.erase(it);
    } else {
      ++it;
    }
  }
  CHECK(!parts_.empty());
  kept_parts_.clear();
}

not_null<Part*> Vessel::part(PartId const id) const {
  return FindOrDie(parts_, id).get();
}

void Vessel::ForSomePart(std::function<void(Part&)> action) const {
  CHECK(!parts_.empty());
  action(*parts_.begin()->second);
}

void Vessel::ForAllParts(std::function<void(Part&)> action) const {
  for (auto const& pair : parts_) {
    action(*pair.second);
  }
}

DiscreteTrajectory<Barycentric> const& Vessel::prediction() const {
  return *prediction_;
}

void Vessel::set_prediction_adaptive_step_parameters(
    Ephemeris<Barycentric>::AdaptiveStepParameters const&
        prediction_adaptive_step_parameters) {
  prediction_adaptive_step_parameters_ = prediction_adaptive_step_parameters;
}

Ephemeris<Barycentric>::AdaptiveStepParameters const&
Vessel::prediction_adaptive_step_parameters() const {
  return prediction_adaptive_step_parameters_;
}

FlightPlan& Vessel::flight_plan() const {
  CHECK(has_flight_plan());
  return *flight_plan_;
}

bool Vessel::has_flight_plan() const {
  return flight_plan_ != nullptr;
}

void Vessel::AdvanceTime(Instant const& time) {
  std::vector<DiscreteTrajectory<Barycentric>::Iterator> its;
  for (auto const& pair : parts_) {
    Part const& part = *pair.second;
    its.push_back(part.tail().Begin());
  }
  for (;;) {
    Part const& first_part = *parts_.begin()->second;
    Instant const time = its[0].time();
    bool const at_end_of_tail = its[0] == first_part.tail().last();
    bool const tail_is_authoritative = first_part.tail_is_authoritative();

    BarycentreCalculator<DegreesOfFreedom<Barycentric>, Mass> calculator;
    int i = 0;
    for (auto const& pair : parts_) {
      Part& part = *pair.second;
      auto& it = its[i];
      calculator.Add(it.degrees_of_freedom(), part.mass());
      CHECK_EQ(time, it.time());
      CHECK_EQ(at_end_of_tail, it == part.tail().last());
      CHECK_EQ(tail_is_authoritative, part.tail_is_authoritative());
      if (at_end_of_tail) {
        part.tail().ForgetBefore(astronomy::InfiniteFuture);
      } else {
        ++it;
      }
      ++i;
    }
    DegreesOfFreedom<Barycentric> const vessel_degrees_of_freedom =
        calculator.Get();
    AppendToPsychohistory(
        time,
        vessel_degrees_of_freedom,
        /*authoritative=*/!at_end_of_tail || tail_is_authoritative);

    if (at_end_of_tail) {
      return;
    }
  }
}

void Vessel::ForgetBefore(Instant const& time) {
  // TODO(egg): do something about the psychohistory. I think bad things happen
  // if it becomes empty though.
  prediction_->ForgetBefore(time);
  if (flight_plan_ != nullptr) {
    flight_plan_->ForgetBefore(time, [this]() { flight_plan_.reset(); });
  }
}

void Vessel::CreateFlightPlan(
    Instant const& final_time,
    Mass const& initial_mass,
    Ephemeris<Barycentric>::AdaptiveStepParameters const&
        flight_plan_adaptive_step_parameters) {
  auto const last = last_authoritative();
  flight_plan_ = std::make_unique<FlightPlan>(
      initial_mass,
      /*initial_time=*/last.time(),
      /*initial_degrees_of_freedom=*/last.degrees_of_freedom(),
      final_time,
      ephemeris_,
      flight_plan_adaptive_step_parameters);
}

void Vessel::DeleteFlightPlan() {
  flight_plan_.reset();
}

void Vessel::UpdatePrediction(Instant const& last_time) {
  prediction_ = make_not_null_unique<DiscreteTrajectory<Barycentric>>();
  auto const last = psychohistory_->last();
  prediction_->Append(last.time(), last.degrees_of_freedom());
  FlowPrediction(last_time);
}

DiscreteTrajectory<Barycentric> const& Vessel::psychohistory() const {
  return *psychohistory_;
}

bool Vessel::psychohistory_is_authoritative() const {
  return psychohistory_is_authoritative_;
}

void Vessel::WriteToMessage(
    not_null<serialization::Vessel*> const message) const {
  // TODO(phl): Implement.
  body_.WriteToMessage(message->mutable_body());
  prediction_adaptive_step_parameters_.WriteToMessage(
      message->mutable_prediction_adaptive_step_parameters());
  if (dummy_part_ != nullptr) {
    dummy_part_->WriteToMessage(message->mutable_dummy_part());
  }
  for (auto const& pair : parts_) {
    auto const& part = pair.second;
    part->WriteToMessage(message->add_parts());
  }
  for (auto const& part : kept_parts_) {
    message->add_kept_parts(part->part_id());
  }
  psychohistory_->WriteToMessage(message->mutable_psychohistory(),
                                 /*forks=*/{});
  message->set_psychohistory_is_authoritative(psychohistory_is_authoritative_);
  prediction_->WriteToMessage(message->mutable_prediction(),
                              /*forks=*/{});
  if (flight_plan_ != nullptr) {
    flight_plan_->WriteToMessage(message->mutable_flight_plan());
  }
}

not_null<std::unique_ptr<Vessel>> Vessel::ReadFromMessage(
    serialization::Vessel const& message,
    not_null<Celestial const*> const parent,
    not_null<Ephemeris<Barycentric>*> const ephemeris,
    std::function<void(PartId)> const& deletion_callback) {
  // NOTE(egg): for now we do not read the |MasslessBody| as it can contain no
  // information.
  auto vessel = make_not_null_unique<Vessel>(
      parent,
      ephemeris,
      Ephemeris<Barycentric>::AdaptiveStepParameters::ReadFromMessage(
          message.prediction_adaptive_step_parameters()));
  if (message.has_dummy_part()) {
    vessel->dummy_part_ = Part::ReadFromMessage(message.dummy_part(),
                                                /*deletion_callback=*/nullptr);
  }
  for (auto const& serialized_part : message.parts()) {
    PartId const part_id = serialized_part.part_id();
    auto part = Part::ReadFromMessage(
        serialized_part,
        [deletion_callback, part_id]() { deletion_callback(part_id); });
    vessel->parts_.emplace(part_id, std::move(part));
  }
  for (PartId const part_id : message.kept_parts()) {
    vessel->kept_parts_.insert(vessel->parts_.at(part_id).get());
  }
  vessel->psychohistory_ =
      DiscreteTrajectory<Barycentric>::ReadFromMessage(message.psychohistory(),
                                                       /*forks=*/{});
  vessel->psychohistory_is_authoritative_ =
      message.psychohistory_is_authoritative();
  vessel->prediction_ =
      DiscreteTrajectory<Barycentric>::ReadFromMessage(message.prediction(),
                                                       /*forks=*/{});
  if (message.has_flight_plan()) {
    // TODO(phl): Remove compatibility code.  The |root| is only needed
    // pre-Буняковский.
    vessel->flight_plan_ = FlightPlan::ReadFromMessage(
        message.flight_plan(), /*root=*/nullptr, ephemeris);
  }
  return std::move(vessel);
}

Vessel::Vessel()
    : body_(),
      prediction_adaptive_step_parameters_(DefaultPredictionParameters()),
      parent_(testing_utilities::make_not_null<Celestial const*>()),
      ephemeris_(testing_utilities::make_not_null<Ephemeris<Barycentric>*>()),
      psychohistory_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()),
      prediction_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()) {}

void Vessel::AppendToPsychohistory(
    Instant const& time,
    DegreesOfFreedom<Barycentric> const& degrees_of_freedom,
    bool const authoritative) {
  if (!psychohistory_is_authoritative_) {
    psychohistory_->ForgetAfter((--psychohistory_->last()).time());
  }
  psychohistory_->Append(time, degrees_of_freedom);
  psychohistory_is_authoritative_ = authoritative;
}

void Vessel::FlowPrediction(Instant const& time) {
  if (time > prediction_->last().time()) {
    bool const finite_time = IsFinite(time - prediction_->last().time());
    Instant const t = finite_time ? time : ephemeris_->t_max();
    // This will not prolong the ephemeris if |time| is infinite (but it may do
    // so if it is finite).
    bool const reached_t = ephemeris_->FlowWithAdaptiveStep(
        prediction_.get(),
        Ephemeris<Barycentric>::NoIntrinsicAcceleration,
        t,
        prediction_adaptive_step_parameters_,
        FlightPlan::max_ephemeris_steps_per_frame,
        /*last_point_only=*/false);
    if (!finite_time && reached_t) {
      // This will prolong the ephemeris by |max_ephemeris_steps_per_frame|.
      ephemeris_->FlowWithAdaptiveStep(
        prediction_.get(),
        Ephemeris<Barycentric>::NoIntrinsicAcceleration,
        time,
        prediction_adaptive_step_parameters_,
        FlightPlan::max_ephemeris_steps_per_frame,
        /*last_point_only=*/false);
    }
  }
}

DiscreteTrajectory<Barycentric>::Iterator Vessel::last_authoritative() const {
  auto it = psychohistory_->last();
  if (!psychohistory_is_authoritative_) {
    --it;
  }
  return it;
}

Ephemeris<Barycentric>::FixedStepParameters DefaultHistoryParameters() {
  return Ephemeris<Barycentric>::FixedStepParameters(
             McLachlanAtela1992Order5Optimal<Position<Barycentric>>(),
             /*step=*/10 * Second);
}

Ephemeris<Barycentric>::AdaptiveStepParameters DefaultProlongationParameters() {
  return Ephemeris<Barycentric>::AdaptiveStepParameters(
             DormandElMikkawyPrince1986RKN434FM<Position<Barycentric>>(),
             /*max_steps=*/std::numeric_limits<std::int64_t>::max(),
             /*length_integration_tolerance=*/1 * Milli(Metre),
             /*speed_integration_tolerance=*/1 * Milli(Metre) / Second);
}

Ephemeris<Barycentric>::AdaptiveStepParameters DefaultPredictionParameters() {
  return Ephemeris<Barycentric>::AdaptiveStepParameters(
             DormandElMikkawyPrince1986RKN434FM<Position<Barycentric>>(),
             /*max_steps=*/1000,
             /*length_integration_tolerance=*/1 * Metre,
             /*speed_integration_tolerance=*/1 * Metre / Second);
}

}  // namespace internal_vessel
}  // namespace ksp_plugin
}  // namespace principia
<|endoftext|>


<commit_before>//-----------------------------------------------------------------------
//    $Id$
//    Version: $Name$
//
//    Copyright (C) 1998, 1999, 2000, 2001, 2002, 2003, 2004 by the deal.II authors
//
//    This file is subject to QPL and may not be  distributed
//    without copyright and license information. Please refer
//    to the file deal.II/doc/license.html for the  text  and
//    further information on this license.
//
//-----------------------------------------------------------------------

#include <base/logstream.h>

#include <lac/vector.h>
#include <lac/block_vector.h>
#include <lac/sparse_matrix.h>
#include <lac/block_sparse_matrix.h>
#include <grid/tria.h>
#include <grid/tria_iterator.h>
#include <dofs/dof_tools.h>
#include <fe/fe.h>
#include <multigrid/mg_dof_handler.h>
#include <multigrid/mg_dof_accessor.h>
#include <multigrid/mg_transfer.h>
#include <multigrid/mg_transfer.templates.h>

template <typename number>
template <int dim>
void MGTransferPrebuilt<number>::build_matrices (
  const MGDoFHandler<dim> &mg_dof)
{
  const unsigned int n_levels      = mg_dof.get_tria().n_levels();
  const unsigned int dofs_per_cell = mg_dof.get_fe().dofs_per_cell;

  sizes.resize(n_levels);
  for (unsigned int l=0;l<n_levels;++l)
    sizes[l] = mg_dof.n_dofs(l);

				   // reset the size of the array of
				   // matrices. call resize(0) first,
				   // in order to delete all elements
				   // and clear their memory. then
				   // repopulate these arrays
				   //
				   // note that on resize(0), the
				   // shared_ptr class takes care of
				   // deleting the object it points to
				   // by itself
  prolongation_matrices.resize (0);
#ifndef DEAL_PREFER_MATRIX_EZ
  prolongation_sparsities.resize (0);
#endif

  for (unsigned int i=0; i<n_levels-1; ++i)
    {
#ifndef DEAL_PREFER_MATRIX_EZ
      prolongation_sparsities
	.push_back (boost::shared_ptr<SparsityPattern> (new SparsityPattern));
      prolongation_matrices
	.push_back (boost::shared_ptr<SparseMatrix<double> > (new SparseMatrix<double>));
#else
      prolongation_matrices
	.push_back (boost::shared_ptr<SparseMatrix<double> > (new SparseMatrixEZ<double>));
#endif
    }

				   // two fields which will store the
				   // indices of the multigrid dofs
				   // for a cell and one of its children
  std::vector<unsigned int> dof_indices_parent (dofs_per_cell);
  std::vector<unsigned int> dof_indices_child (dofs_per_cell);

				   // for each level: first build the sparsity
				   // pattern of the matrices and then build the
				   // matrices themselves. note that we only
				   // need to take care of cells on the coarser
				   // level which have children
  for (unsigned int level=0; level<n_levels-1; ++level)
    {
				       // reset the dimension of the structure.
				       // note that for the number of entries
				       // per row, the number of parent dofs
				       // coupling to a child dof is
				       // necessary. this, of course, is the
				       // number of degrees of freedom per
				       // cell
#ifdef DEAL_PREFER_MATRIX_EZ
      prolongation_matrices[level]->reinit (sizes[level+1],
					    sizes[level],
					    dofs_per_cell);
#else
				       // increment dofs_per_cell
				       // since a useless diagonal
				       // element will be stored
      prolongation_sparsities[level]->reinit (sizes[level+1],
					      sizes[level],
					      dofs_per_cell+1);

      for (typename MGDoFHandler<dim>::cell_iterator cell=mg_dof.begin(level);
	   cell != mg_dof.end(level); ++cell)
	if (cell->has_children())
	  {
	    cell->get_mg_dof_indices (dof_indices_parent);

	    for (unsigned int child=0;
		 child<GeometryInfo<dim>::children_per_cell; ++child)
	      {
						 // set an alias to the
						 // prolongation matrix for
						 // this child
		const FullMatrix<double> &prolongation
		  = mg_dof.get_fe().get_prolongation_matrix (child);

		Assert (prolongation.n() != 0, ExcNoProlongation());

		cell->child(child)->get_mg_dof_indices (dof_indices_child);

						 // now tag the entries in the
						 // matrix which will be used
						 // for this pair of parent/child
		for (unsigned int i=0; i<dofs_per_cell; ++i)
		  for (unsigned int j=0; j<dofs_per_cell; ++j)
		    if (prolongation(i,j) != 0)
		      {
			prolongation_sparsities[level]->add (dof_indices_child[i],
							     dof_indices_parent[j]);
		      };
	      };
	  };
      prolongation_sparsities[level]->compress ();

      prolongation_matrices[level]->reinit (*prolongation_sparsities[level]);
#endif

				       // now actually build the matrices
      for (typename MGDoFHandler<dim>::cell_iterator cell=mg_dof.begin(level);
	   cell != mg_dof.end(level); ++cell)
	if (cell->has_children())
	  {
	    cell->get_mg_dof_indices (dof_indices_parent);

	    for (unsigned int child=0;
		 child<GeometryInfo<dim>::children_per_cell; ++child)
	      {
						 // set an alias to the
						 // prolongation matrix for
						 // this child
		const FullMatrix<double> &prolongation
		  = mg_dof.get_fe().get_prolongation_matrix (child);

		cell->child(child)->get_mg_dof_indices (dof_indices_child);

						 // now set the entries in the
						 // matrix
		for (unsigned int i=0; i<dofs_per_cell; ++i)
		  for (unsigned int j=0; j<dofs_per_cell; ++j)
		    if (prolongation(i,j) != 0)
		      prolongation_matrices[level]->set (dof_indices_child[i],
							 dof_indices_parent[j],
							 prolongation(i,j));
	      };
	  };
    };
}

template
void MGTransferPrebuilt<float>::build_matrices<deal_II_dimension>
(const MGDoFHandler<deal_II_dimension> &mg_dof);

template
void MGTransferPrebuilt<double>::build_matrices<deal_II_dimension>
(const MGDoFHandler<deal_II_dimension> &mg_dof);

template void
MGTransferPrebuilt<float>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<float> >&,
  const Vector<double>&) const;
template void
MGTransferPrebuilt<float>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<float> >&,
  const BlockVector<double>&) const;
template void
MGTransferPrebuilt<float>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  Vector<double>&,
  const MGLevelObject<Vector<float> >&) const;
template void
MGTransferPrebuilt<float>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  BlockVector<double>&,
  const MGLevelObject<Vector<float> >&) const;

template void
MGTransferPrebuilt<double>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<double> >&,
  const Vector<double>&) const;
template void
MGTransferPrebuilt<double>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<double> >&,
  const BlockVector<double>&) const;
template void
MGTransferPrebuilt<double>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  Vector<double>&,
  const MGLevelObject<Vector<double> >&) const;
template void
MGTransferPrebuilt<double>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  BlockVector<double>&,
  const MGLevelObject<Vector<double> >&) const;

template void
MGTransferPrebuilt<double>::copy_from_mg_add (
  const MGDoFHandler<deal_II_dimension>&,
  Vector<double>&,
  const MGLevelObject<Vector<double> >&) const;
template void
MGTransferPrebuilt<double>::copy_from_mg_add (
  const MGDoFHandler<deal_II_dimension>&,
  BlockVector<double>&,
  const MGLevelObject<Vector<double> >&) const;
<commit_msg>Add missing instantiations.<commit_after>//-----------------------------------------------------------------------
//    $Id$
//    Version: $Name$
//
//    Copyright (C) 1998, 1999, 2000, 2001, 2002, 2003, 2004 by the deal.II authors
//
//    This file is subject to QPL and may not be  distributed
//    without copyright and license information. Please refer
//    to the file deal.II/doc/license.html for the  text  and
//    further information on this license.
//
//-----------------------------------------------------------------------

#include <base/logstream.h>

#include <lac/vector.h>
#include <lac/block_vector.h>
#include <lac/sparse_matrix.h>
#include <lac/block_sparse_matrix.h>
#include <grid/tria.h>
#include <grid/tria_iterator.h>
#include <dofs/dof_tools.h>
#include <fe/fe.h>
#include <multigrid/mg_dof_handler.h>
#include <multigrid/mg_dof_accessor.h>
#include <multigrid/mg_transfer.h>
#include <multigrid/mg_transfer.templates.h>

template <typename number>
template <int dim>
void MGTransferPrebuilt<number>::build_matrices (
  const MGDoFHandler<dim> &mg_dof)
{
  const unsigned int n_levels      = mg_dof.get_tria().n_levels();
  const unsigned int dofs_per_cell = mg_dof.get_fe().dofs_per_cell;

  sizes.resize(n_levels);
  for (unsigned int l=0;l<n_levels;++l)
    sizes[l] = mg_dof.n_dofs(l);

				   // reset the size of the array of
				   // matrices. call resize(0) first,
				   // in order to delete all elements
				   // and clear their memory. then
				   // repopulate these arrays
				   //
				   // note that on resize(0), the
				   // shared_ptr class takes care of
				   // deleting the object it points to
				   // by itself
  prolongation_matrices.resize (0);
#ifndef DEAL_PREFER_MATRIX_EZ
  prolongation_sparsities.resize (0);
#endif

  for (unsigned int i=0; i<n_levels-1; ++i)
    {
#ifndef DEAL_PREFER_MATRIX_EZ
      prolongation_sparsities
	.push_back (boost::shared_ptr<SparsityPattern> (new SparsityPattern));
      prolongation_matrices
	.push_back (boost::shared_ptr<SparseMatrix<double> > (new SparseMatrix<double>));
#else
      prolongation_matrices
	.push_back (boost::shared_ptr<SparseMatrix<double> > (new SparseMatrixEZ<double>));
#endif
    }

				   // two fields which will store the
				   // indices of the multigrid dofs
				   // for a cell and one of its children
  std::vector<unsigned int> dof_indices_parent (dofs_per_cell);
  std::vector<unsigned int> dof_indices_child (dofs_per_cell);

				   // for each level: first build the sparsity
				   // pattern of the matrices and then build the
				   // matrices themselves. note that we only
				   // need to take care of cells on the coarser
				   // level which have children
  for (unsigned int level=0; level<n_levels-1; ++level)
    {
				       // reset the dimension of the structure.
				       // note that for the number of entries
				       // per row, the number of parent dofs
				       // coupling to a child dof is
				       // necessary. this, of course, is the
				       // number of degrees of freedom per
				       // cell
#ifdef DEAL_PREFER_MATRIX_EZ
      prolongation_matrices[level]->reinit (sizes[level+1],
					    sizes[level],
					    dofs_per_cell);
#else
				       // increment dofs_per_cell
				       // since a useless diagonal
				       // element will be stored
      prolongation_sparsities[level]->reinit (sizes[level+1],
					      sizes[level],
					      dofs_per_cell+1);

      for (typename MGDoFHandler<dim>::cell_iterator cell=mg_dof.begin(level);
	   cell != mg_dof.end(level); ++cell)
	if (cell->has_children())
	  {
	    cell->get_mg_dof_indices (dof_indices_parent);

	    for (unsigned int child=0;
		 child<GeometryInfo<dim>::children_per_cell; ++child)
	      {
						 // set an alias to the
						 // prolongation matrix for
						 // this child
		const FullMatrix<double> &prolongation
		  = mg_dof.get_fe().get_prolongation_matrix (child);

		Assert (prolongation.n() != 0, ExcNoProlongation());

		cell->child(child)->get_mg_dof_indices (dof_indices_child);

						 // now tag the entries in the
						 // matrix which will be used
						 // for this pair of parent/child
		for (unsigned int i=0; i<dofs_per_cell; ++i)
		  for (unsigned int j=0; j<dofs_per_cell; ++j)
		    if (prolongation(i,j) != 0)
		      {
			prolongation_sparsities[level]->add (dof_indices_child[i],
							     dof_indices_parent[j]);
		      };
	      };
	  };
      prolongation_sparsities[level]->compress ();

      prolongation_matrices[level]->reinit (*prolongation_sparsities[level]);
#endif

				       // now actually build the matrices
      for (typename MGDoFHandler<dim>::cell_iterator cell=mg_dof.begin(level);
	   cell != mg_dof.end(level); ++cell)
	if (cell->has_children())
	  {
	    cell->get_mg_dof_indices (dof_indices_parent);

	    for (unsigned int child=0;
		 child<GeometryInfo<dim>::children_per_cell; ++child)
	      {
						 // set an alias to the
						 // prolongation matrix for
						 // this child
		const FullMatrix<double> &prolongation
		  = mg_dof.get_fe().get_prolongation_matrix (child);

		cell->child(child)->get_mg_dof_indices (dof_indices_child);

						 // now set the entries in the
						 // matrix
		for (unsigned int i=0; i<dofs_per_cell; ++i)
		  for (unsigned int j=0; j<dofs_per_cell; ++j)
		    if (prolongation(i,j) != 0)
		      prolongation_matrices[level]->set (dof_indices_child[i],
							 dof_indices_parent[j],
							 prolongation(i,j));
	      };
	  };
    };
}

template
void MGTransferPrebuilt<float>::build_matrices<deal_II_dimension>
(const MGDoFHandler<deal_II_dimension> &mg_dof);

template
void MGTransferPrebuilt<double>::build_matrices<deal_II_dimension>
(const MGDoFHandler<deal_II_dimension> &mg_dof);

template void
MGTransferPrebuilt<float>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<float> >&,
  const Vector<double>&) const;
template void
MGTransferPrebuilt<float>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<float> >&,
  const BlockVector<double>&) const;
template void
MGTransferPrebuilt<float>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<float> >&,
  const Vector<float>&) const;
template void
MGTransferPrebuilt<float>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<float> >&,
  const BlockVector<float>&) const;
template void
MGTransferPrebuilt<float>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  Vector<double>&,
  const MGLevelObject<Vector<float> >&) const;
template void
MGTransferPrebuilt<float>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  BlockVector<double>&,
  const MGLevelObject<Vector<float> >&) const;
template void
MGTransferPrebuilt<float>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  Vector<float>&,
  const MGLevelObject<Vector<float> >&) const;
template void
MGTransferPrebuilt<float>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  BlockVector<float>&,
  const MGLevelObject<Vector<float> >&) const;

template void
MGTransferPrebuilt<double>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<double> >&,
  const Vector<double>&) const;
template void
MGTransferPrebuilt<double>::copy_to_mg (
  const MGDoFHandler<deal_II_dimension>&,
  MGLevelObject<Vector<double> >&,
  const BlockVector<double>&) const;
template void
MGTransferPrebuilt<double>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  Vector<double>&,
  const MGLevelObject<Vector<double> >&) const;
template void
MGTransferPrebuilt<double>::copy_from_mg (
  const MGDoFHandler<deal_II_dimension>&,
  BlockVector<double>&,
  const MGLevelObject<Vector<double> >&) const;

template void
MGTransferPrebuilt<double>::copy_from_mg_add (
  const MGDoFHandler<deal_II_dimension>&,
  Vector<double>&,
  const MGLevelObject<Vector<double> >&) const;
template void
MGTransferPrebuilt<double>::copy_from_mg_add (
  const MGDoFHandler<deal_II_dimension>&,
  BlockVector<double>&,
  const MGLevelObject<Vector<double> >&) const;
<|endoftext|>


<commit_before>// Chemfiles, a modern library for chemistry file reading and writing
// Copyright (C) Guillaume Fraux and contributors -- BSD license

#include <cmath>

#include "chemfiles/ErrorFmt.hpp"
#include "chemfiles/unreachable.hpp"
#include "chemfiles/UnitCell.hpp"
using namespace chemfiles;

// Sinus and Cosine for degree values
constexpr double pi = 3.141592653589793238463;
inline double deg2rad(double x) {
    return x * pi / 180.0;
}

inline double rad2deg(double x) {
    return x * 180.0 / pi;
}

inline double cosd(double theta) {
    return cos(deg2rad(theta));
}

inline double sind(double theta) {
    return sin(deg2rad(theta));
}

UnitCell::UnitCell(): UnitCell(0) {
    shape_ = INFINITE;
}

UnitCell::UnitCell(double a): UnitCell(a, a, a) {}

UnitCell::UnitCell(double a, double b, double c): UnitCell(a, b, c, 90, 90, 90) {}

UnitCell::UnitCell(double a, double b, double c, double alpha, double beta, double gamma):
    h_(Matrix3D::unit()),
    h_inv_(Matrix3D::unit()),
    a_(a),
    b_(b),
    c_(c),
    alpha_(alpha),
    beta_(beta),
    gamma_(gamma)
{
    if (alpha_ == 90 && beta_ == 90 && gamma_ == 90) {
        shape_ = ORTHORHOMBIC;
    } else {
        shape_ = TRICLINIC;
    }
    update_matrix();
}

UnitCell::UnitCell(const Matrix3D& matrix): h_(Matrix3D::unit()), h_inv_(Matrix3D::unit()) {
    if (matrix[1][0] != 0 || matrix[2][0] != 0 || matrix[2][1] != 0) {
        throw error("Matrix supplied to UnitCell is not an upper triangular matrix");
    }

    if (matrix[0][0] == 0 && matrix[1][1] == 0 && matrix[2][2] == 0 &&
        matrix[0][1] == 0 && matrix[0][2] == 0 && matrix[1][2] == 0) {

        shape_ = INFINITE;
        a_ = b_ = c_ = 0;
        alpha_ = beta_ = gamma_ = 90.0;

        return;
    }

    if (matrix[0][1] == 0 && matrix[0][2] == 0 && matrix[1][2] == 0) {
        shape_ = ORTHORHOMBIC;

        a_ = matrix[0][0];
        b_ = matrix[1][1];
        c_ = matrix[2][2];

        alpha_ = beta_ = gamma_ = 90.0;

        return;
    }

    shape_ = TRICLINIC;

    Vector3D v1 = {matrix[0][0], matrix[1][0], matrix[2][0]};
    Vector3D v2 = {matrix[0][1], matrix[1][1], matrix[2][1]};
    Vector3D v3 = {matrix[0][2], matrix[1][2], matrix[2][2]};

    a_ = v1.norm();
    b_ = v2.norm();
    c_ = v3.norm();

    alpha_ = rad2deg(acos(dot(v2, v3) / (b_ * c_)));
    beta_  = rad2deg(acos(dot(v1, v3) / (a_ * c_)));
    gamma_ = rad2deg(acos(dot(v1, v2) / (a_ * b_)));

    update_matrix();

    assert(fabs(h_[0][0] - matrix[0][0]) < 1e-5);
    assert(fabs(h_[1][0] - matrix[1][0]) < 1e-5);
    assert(fabs(h_[2][0] - matrix[2][0]) < 1e-5);

    assert(fabs(h_[0][1] - matrix[0][1]) < 1e-5);
    assert(fabs(h_[1][1] - matrix[1][1]) < 1e-5);
    assert(fabs(h_[2][1] - matrix[2][1]) < 1e-5);

    assert(fabs(h_[0][2] - matrix[0][2]) < 1e-5);
    assert(fabs(h_[1][2] - matrix[1][2]) < 1e-5);
    assert(fabs(h_[2][2] - matrix[2][2]) < 1e-5);
}

double UnitCell::volume() const {
    switch (shape_) {
    case INFINITE:
        return 0;
    case ORTHORHOMBIC:
        return a_ * b_ * c_;
    case TRICLINIC:
        break; // The computation is too complexe to take place in a switch
    }
    double cos_alpha = cos(deg2rad(alpha_));
    double cos_beta = cos(deg2rad(beta_));
    double cos_gamma = cos(deg2rad(gamma_));

    double factor = sqrt(
        1 - cos_alpha * cos_alpha - cos_beta * cos_beta -
        cos_gamma * cos_gamma + 2 * cos_alpha * cos_beta * cos_gamma
    );
    return a_ * b_ * c_ * factor;
}

void UnitCell::update_matrix() {
    h_[0][0] = a_;
    h_[1][0] = 0;
    h_[2][0] = 0;

    h_[0][1] = cosd(gamma_) * b_;
    h_[1][1] = sind(gamma_) * b_;
    h_[2][1] = 0;

    h_[0][2] = cosd(beta_);
    h_[1][2] = (cosd(alpha_) - cosd(beta_) * cosd(gamma_)) / sind(gamma_);
    h_[2][2] = sqrt(1 - h_[0][2] * h_[0][2] - h_[1][2] * h_[1][2]);
    h_[0][2] *= c_;
    h_[1][2] *= c_;
    h_[2][2] *= c_;

    // Do not try to invert a cell with a 0 volume
    if (volume() == 0.0) {
        h_inv_ = Matrix3D::unit();
    } else {
        h_inv_ = h_.invert();
    }
}

void UnitCell::set_shape(CellShape shape) {
    if (shape == ORTHORHOMBIC) {
        if (!(alpha_ == 90 && beta_ == 90 && gamma_ == 90)) {
            throw error(
                "can not be set cell shape to ORTHORHOMBIC: some angles are not 90°"
            );
        }
    } else if (shape == INFINITE) {
        if (!(alpha_ == 90 && beta_ == 90 && gamma_ == 90)) {
            throw error(
                "can not be set cell shape to INFINITE: some angles are not 90°"
            );
        }
        if (!(a_ == 0.0 && b_ == 0.0 && c_ == 0.0)) {
            throw error(
                "can not be set cell shape to INFINITE: some lengths are not 0"
            );
        }
    }

    shape_ = shape;
}

void UnitCell::set_a(double val) {
    if (shape_ == INFINITE) {
        throw error("can not set 'a' on infinite cell");
    }
    a_ = val;
    update_matrix();
}

void UnitCell::set_b(double val) {
    if (shape_ == INFINITE) {
        throw error("can not set 'b' on infinite cell");
    }
    b_ = val;
    update_matrix();
}

void UnitCell::set_c(double val) {
    if (shape_ == INFINITE) {
        throw error("can not set 'c' on infinite cell");
    }
    c_ = val;
    update_matrix();
}

void UnitCell::set_alpha(double val) {
    if (shape_ != TRICLINIC) {
        throw error("can not set 'alpha' on non triclinic cell");
    }
    alpha_ = val;
    update_matrix();
}

void UnitCell::set_beta(double val) {
    if (shape_ != TRICLINIC) {
        throw error("can not set 'beta' on non triclinic cell");
    }
    beta_ = val;
    update_matrix();
}

void UnitCell::set_gamma(double val) {
    if (shape_ != TRICLINIC) {
        throw error("can not set 'gamma' on non triclinic cell");
    }
    gamma_ = val;
    update_matrix();
}

// Wrap a vector in an Orthorombic UnitCell
Vector3D UnitCell::wrap_orthorombic(const Vector3D& vector) const {
    return {
        vector[0] - round(vector[0] / a_) * a_,
        vector[1] - round(vector[1] / b_) * b_,
        vector[2] - round(vector[2] / c_) * c_
    };
}

// Wrap a vector in an Orthorombic UnitCell
Vector3D UnitCell::wrap_triclinic(const Vector3D& vector) const {
    auto fractional = h_inv_ * vector;
    fractional[0] -= round(fractional[0]);
    fractional[1] -= round(fractional[1]);
    fractional[2] -= round(fractional[2]);
    return h_ * fractional;
}

Vector3D UnitCell::wrap(const Vector3D& vector) const {
    switch (shape_) {
        case INFINITE:
            return vector;
        case ORTHORHOMBIC:
            return wrap_orthorombic(vector);
        case TRICLINIC:
            return wrap_triclinic(vector);
    }
    unreachable();
}

namespace chemfiles {
    bool operator==(const UnitCell& rhs, const UnitCell& lhs) {
        if (lhs.shape() != rhs.shape()) {
            return false;
        }

        return rhs.a() == lhs.a() &&
               rhs.b() == lhs.b() &&
               rhs.c() == lhs.c() &&
               rhs.alpha() == lhs.alpha() &&
               rhs.beta() == lhs.beta() &&
               rhs.gamma() == lhs.gamma();
    }

    bool operator!=(const UnitCell& rhs, const UnitCell& lhs) {
        return !(rhs == lhs);
    }
}
<commit_msg>Do not use float == when checking cell angles for shape<commit_after>// Chemfiles, a modern library for chemistry file reading and writing
// Copyright (C) Guillaume Fraux and contributors -- BSD license

#include <cmath>

#include "chemfiles/ErrorFmt.hpp"
#include "chemfiles/unreachable.hpp"
#include "chemfiles/UnitCell.hpp"
using namespace chemfiles;

// Sinus and Cosine for degree values
constexpr double pi = 3.141592653589793238463;
inline double deg2rad(double x) {
    return x * pi / 180.0;
}

inline double rad2deg(double x) {
    return x * 180.0 / pi;
}

inline double cosd(double theta) {
    return cos(deg2rad(theta));
}

inline double sind(double theta) {
    return sin(deg2rad(theta));
}

UnitCell::UnitCell(): UnitCell(0) {
    shape_ = INFINITE;
}

UnitCell::UnitCell(double a): UnitCell(a, a, a) {}

UnitCell::UnitCell(double a, double b, double c): UnitCell(a, b, c, 90, 90, 90) {}

UnitCell::UnitCell(double a, double b, double c, double alpha, double beta, double gamma):
    h_(Matrix3D::unit()),
    h_inv_(Matrix3D::unit()),
    a_(a),
    b_(b),
    c_(c),
    alpha_(alpha),
    beta_(beta),
    gamma_(gamma)
{
    auto is_90 = [](double angle) {
        // We think that 89.999° is close enough to 90°
        return fabs(angle - 90.0) < 1e-3;
    };
    if (is_90(alpha_) && is_90(beta_) && is_90(gamma_)) {
        shape_ = ORTHORHOMBIC;
        // Make sure alpha/beta/gamma are actually 90°, so that the matrix
        // update below does not create a non diagonal matrix.
        alpha_ = 90;
        beta_ = 90;
        gamma_ = 90;
    } else {
        shape_ = TRICLINIC;
    }
    update_matrix();
}

UnitCell::UnitCell(const Matrix3D& matrix): h_(Matrix3D::unit()), h_inv_(Matrix3D::unit()) {
    if (matrix[1][0] != 0 || matrix[2][0] != 0 || matrix[2][1] != 0) {
        throw error("Matrix supplied to UnitCell is not an upper triangular matrix");
    }

    if (matrix[0][0] == 0 && matrix[1][1] == 0 && matrix[2][2] == 0 &&
        matrix[0][1] == 0 && matrix[0][2] == 0 && matrix[1][2] == 0) {

        shape_ = INFINITE;
        a_ = b_ = c_ = 0;
        alpha_ = beta_ = gamma_ = 90.0;

        return;
    }

    if (matrix[0][1] == 0 && matrix[0][2] == 0 && matrix[1][2] == 0) {
        shape_ = ORTHORHOMBIC;

        a_ = matrix[0][0];
        b_ = matrix[1][1];
        c_ = matrix[2][2];

        alpha_ = beta_ = gamma_ = 90.0;

        return;
    }

    shape_ = TRICLINIC;

    Vector3D v1 = {matrix[0][0], matrix[1][0], matrix[2][0]};
    Vector3D v2 = {matrix[0][1], matrix[1][1], matrix[2][1]};
    Vector3D v3 = {matrix[0][2], matrix[1][2], matrix[2][2]};

    a_ = v1.norm();
    b_ = v2.norm();
    c_ = v3.norm();

    alpha_ = rad2deg(acos(dot(v2, v3) / (b_ * c_)));
    beta_  = rad2deg(acos(dot(v1, v3) / (a_ * c_)));
    gamma_ = rad2deg(acos(dot(v1, v2) / (a_ * b_)));

    update_matrix();

    assert(fabs(h_[0][0] - matrix[0][0]) < 1e-5);
    assert(fabs(h_[1][0] - matrix[1][0]) < 1e-5);
    assert(fabs(h_[2][0] - matrix[2][0]) < 1e-5);

    assert(fabs(h_[0][1] - matrix[0][1]) < 1e-5);
    assert(fabs(h_[1][1] - matrix[1][1]) < 1e-5);
    assert(fabs(h_[2][1] - matrix[2][1]) < 1e-5);

    assert(fabs(h_[0][2] - matrix[0][2]) < 1e-5);
    assert(fabs(h_[1][2] - matrix[1][2]) < 1e-5);
    assert(fabs(h_[2][2] - matrix[2][2]) < 1e-5);
}

double UnitCell::volume() const {
    switch (shape_) {
    case INFINITE:
        return 0;
    case ORTHORHOMBIC:
        return a_ * b_ * c_;
    case TRICLINIC:
        break; // The computation is too complexe to take place in a switch
    }
    double cos_alpha = cos(deg2rad(alpha_));
    double cos_beta = cos(deg2rad(beta_));
    double cos_gamma = cos(deg2rad(gamma_));

    double factor = sqrt(
        1 - cos_alpha * cos_alpha - cos_beta * cos_beta -
        cos_gamma * cos_gamma + 2 * cos_alpha * cos_beta * cos_gamma
    );
    return a_ * b_ * c_ * factor;
}

void UnitCell::update_matrix() {
    h_[0][0] = a_;
    h_[1][0] = 0;
    h_[2][0] = 0;

    h_[0][1] = cosd(gamma_) * b_;
    h_[1][1] = sind(gamma_) * b_;
    h_[2][1] = 0;

    h_[0][2] = cosd(beta_);
    h_[1][2] = (cosd(alpha_) - cosd(beta_) * cosd(gamma_)) / sind(gamma_);
    h_[2][2] = sqrt(1 - h_[0][2] * h_[0][2] - h_[1][2] * h_[1][2]);
    h_[0][2] *= c_;
    h_[1][2] *= c_;
    h_[2][2] *= c_;

    // Do not try to invert a cell with a 0 volume
    if (volume() == 0.0) {
        h_inv_ = Matrix3D::unit();
    } else {
        h_inv_ = h_.invert();
    }
}

void UnitCell::set_shape(CellShape shape) {
    if (shape == ORTHORHOMBIC) {
        if (!(alpha_ == 90 && beta_ == 90 && gamma_ == 90)) {
            throw error(
                "can not be set cell shape to ORTHORHOMBIC: some angles are not 90°"
            );
        }
    } else if (shape == INFINITE) {
        if (!(alpha_ == 90 && beta_ == 90 && gamma_ == 90)) {
            throw error(
                "can not be set cell shape to INFINITE: some angles are not 90°"
            );
        }
        if (!(a_ == 0.0 && b_ == 0.0 && c_ == 0.0)) {
            throw error(
                "can not be set cell shape to INFINITE: some lengths are not 0"
            );
        }
    }

    shape_ = shape;
}

void UnitCell::set_a(double val) {
    if (shape_ == INFINITE) {
        throw error("can not set 'a' on infinite cell");
    }
    a_ = val;
    update_matrix();
}

void UnitCell::set_b(double val) {
    if (shape_ == INFINITE) {
        throw error("can not set 'b' on infinite cell");
    }
    b_ = val;
    update_matrix();
}

void UnitCell::set_c(double val) {
    if (shape_ == INFINITE) {
        throw error("can not set 'c' on infinite cell");
    }
    c_ = val;
    update_matrix();
}

void UnitCell::set_alpha(double val) {
    if (shape_ != TRICLINIC) {
        throw error("can not set 'alpha' on non triclinic cell");
    }
    alpha_ = val;
    update_matrix();
}

void UnitCell::set_beta(double val) {
    if (shape_ != TRICLINIC) {
        throw error("can not set 'beta' on non triclinic cell");
    }
    beta_ = val;
    update_matrix();
}

void UnitCell::set_gamma(double val) {
    if (shape_ != TRICLINIC) {
        throw error("can not set 'gamma' on non triclinic cell");
    }
    gamma_ = val;
    update_matrix();
}

// Wrap a vector in an Orthorombic UnitCell
Vector3D UnitCell::wrap_orthorombic(const Vector3D& vector) const {
    return {
        vector[0] - round(vector[0] / a_) * a_,
        vector[1] - round(vector[1] / b_) * b_,
        vector[2] - round(vector[2] / c_) * c_
    };
}

// Wrap a vector in an Orthorombic UnitCell
Vector3D UnitCell::wrap_triclinic(const Vector3D& vector) const {
    auto fractional = h_inv_ * vector;
    fractional[0] -= round(fractional[0]);
    fractional[1] -= round(fractional[1]);
    fractional[2] -= round(fractional[2]);
    return h_ * fractional;
}

Vector3D UnitCell::wrap(const Vector3D& vector) const {
    switch (shape_) {
        case INFINITE:
            return vector;
        case ORTHORHOMBIC:
            return wrap_orthorombic(vector);
        case TRICLINIC:
            return wrap_triclinic(vector);
    }
    unreachable();
}

namespace chemfiles {
    bool operator==(const UnitCell& rhs, const UnitCell& lhs) {
        if (lhs.shape() != rhs.shape()) {
            return false;
        }

        return rhs.a() == lhs.a() &&
               rhs.b() == lhs.b() &&
               rhs.c() == lhs.c() &&
               rhs.alpha() == lhs.alpha() &&
               rhs.beta() == lhs.beta() &&
               rhs.gamma() == lhs.gamma();
    }

    bool operator!=(const UnitCell& rhs, const UnitCell& lhs) {
        return !(rhs == lhs);
    }
}
<|endoftext|>