<commit_before>#include <stdio.h>
#include <string.h>
#include <SFML/Network.hpp>
#include <iostream>
#include <iomanip>
#include <sstream>

#include "vectorUtils.h"
#include "resources.h"
#include "soundManager.h"
#include "random.h"

#define MAX_SOUNDS 16

SoundManager* soundManager;

SoundManager::SoundManager()
{
    //By creating a SoundBuffer we force SFML to load the sound subsystem. Else this is done when the first sound is loaded, causing a delay at that point.
    sf::SoundBuffer forceLoadBuffer;

    for(unsigned int n=0; n<MAX_SOUNDS; n++)
        activeSoundList.push_back(sf::Sound());

    music_volume = 100.0;
    positional_sound_enabled = false;
    primary_music.mode = None;
    secondary_music.mode = None;
}

SoundManager::~SoundManager()
{
}

void SoundManager::playMusic(string name)
{
    music_set.clear();
    P<ResourceStream> stream = getResourceStream(name);
    if (stream)
        startMusic(stream, true);
}

void SoundManager::playMusicSet(std::vector<string> filenames)
{
    music_set = filenames;
    if (music_set.size() > 0)
        startMusic(getResourceStream(music_set[irandom(0, music_set.size() - 1)]), false);
    else
        stopMusic();
}

void SoundManager::stopMusic()
{
    music_set.clear();
    primary_music.music.stop();
    secondary_music.music.stop();
}

void SoundManager::setMusicVolume(float volume)
{
    if (music_volume != volume)
    {
        music_volume = volume;
        if (primary_music.mode == None)
            primary_music.music.setVolume(music_volume);
        if (secondary_music.mode == None)
            secondary_music.music.setVolume(music_volume);
    }
}

float SoundManager::getMusicVolume()
{
    return music_volume;
}

void SoundManager::playSound(string name, float pitch, float volume)
{
    sf::SoundBuffer* data = soundMap[name];
    if (data == NULL)
        data = loadSound(name);

    playSoundData(data, pitch, volume);
}

void SoundManager::setListenerPosition(sf::Vector2f position, float angle)
{
    sf::Vector2f listen_vector = sf::vector2FromAngle(angle);
    sf::Listener::setPosition(position.x, 0, position.y);
    sf::Listener::setDirection(listen_vector.x, 0, listen_vector.y);
    positional_sound_enabled = true;
}

void SoundManager::disablePositionalSound()
{
    positional_sound_enabled = false;
}

void SoundManager::playSound(string name, sf::Vector2f position, float min_distance, float attenuation, float pitch, float volume)
{
    if (!positional_sound_enabled)
        return;
    sf::SoundBuffer* data = soundMap[name];
    if (data == NULL)
        data = loadSound(name);
    if (data->getChannelCount() > 1)
        LOG(WARNING) << name << ": Used as positional sound but has more then 1 channel.";

    for(unsigned int n=0; n<activeSoundList.size(); n++)
    {
        sf::Sound& sound = activeSoundList[n];
        if (sound.getStatus() == sf::Sound::Stopped)
        {
            sound.setBuffer(*data);
            sound.setRelativeToListener(false);
            sound.setMinDistance(min_distance);
            sound.setAttenuation(attenuation);
            sound.setPosition(position.x, 0, position.y);
            sound.setPitch(pitch);
            sound.setVolume(volume);
            sound.play();
            return;
        }
    }
}

void SoundManager::setTextToSpeachVoice(string name)
{
}

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

    for (string::const_iterator i = value.begin(), n = value.end(); i != n; ++i) {
        string::value_type c = (*i);
        if (isalnum(c) || c == '-' || c == '_' || c == '.' || c == '~') {
            escaped << c;
        }
        else if (c == ' ')  {
            escaped << '+';
        }
        else {
            escaped << '%' << std::setw(2) << ((int) c) << std::setw(0);
        }
    }

    return escaped.str();
}

void SoundManager::playTextToSpeech(string text)
{
    string name = "TTS:" + text;
    sf::SoundBuffer* data = soundMap[name];
    if (data != NULL)
    {
        playSoundData(data, 1.0, 100.0);
        return;
    }

    sf::Http http("localhost", 59125);
    sf::Http::Request request("process?INPUT_TEXT=" + url_encode(text) + "&INPUT_TYPE=TEXT&OUTPUT_TYPE=AUDIO&AUDIO=WAVE_FILE&LOCALE=en_US&VOICE=dfki-prudence");
    sf::Http::Response response = http.sendRequest(request);

    sf::Http::Response::Status status = response.getStatus();
    if (status == sf::Http::Response::Ok)
    {
        string wave = response.getBody();
        sf::SoundBuffer* data = new sf::SoundBuffer();
        data->loadFromMemory(wave.data(), wave.size());
        soundMap[name] = data;
        playSoundData(data, 1.0, 100.0);
    }
    else
    {
        std::cout << "Error requesting text to speech from Mary server: " << status << std::endl;
    }
}

void SoundManager::playSoundData(sf::SoundBuffer* data, float pitch, float volume)
{
    for(unsigned int n=0; n<activeSoundList.size(); n++)
    {
        sf::Sound& sound = activeSoundList[n];
        if (sound.getStatus() == sf::Sound::Stopped)
        {
            sound.setBuffer(*data);
            sound.setRelativeToListener(true);
            sound.setMinDistance(1);
            sound.setAttenuation(0);
            sound.setPitch(pitch);
            sound.setVolume(volume);
            sound.setPosition(0, 0, 0);
            sound.play();
            return;
        }
    }
}

sf::SoundBuffer* SoundManager::loadSound(string name)
{
    sf::SoundBuffer* data = soundMap[name];
    if (data)
        return data;

    data = new sf::SoundBuffer();

    P<ResourceStream> stream = getResourceStream(name);
    if (!stream) stream = getResourceStream(name + ".wav");
    if (!stream || !data->loadFromStream(**stream))
    {
        LOG(WARNING) << "Failed to load: " << name;
        soundMap[name] = data;
        return data;
    }

    LOG(INFO) << "Loaded: " << name << " of " << data->getDuration().asSeconds() << " seconds";
    soundMap[name] = data;
    return data;
}

void SoundManager::startMusic(P<ResourceStream> stream, bool loop)
{
    if (!stream)
        return;

    MusicChannel* channel = &primary_music;
    MusicChannel* other_channel = &secondary_music;

    if (primary_music.music.getStatus() != sf::Music::Stopped)
    {
        channel = &secondary_music;
        other_channel = &primary_music;
    }

    channel->music.stop();
    channel->mode = FadeIn;
    channel->fade_delay = fade_music_time;
    channel->music.openFromStream(**stream);
    channel->stream = stream;
    channel->music.setLoop(loop);
    channel->music.setVolume(0);
    channel->music.play();

    if (other_channel->music.getStatus() != sf::Music::Stopped)
    {
        other_channel->mode = FadeOut;
        other_channel->fade_delay = fade_music_time;
    }
}

void SoundManager::updateTick()
{
    float delta = clock.restart().asSeconds();
    updateChannel(primary_music, delta);
    updateChannel(secondary_music, delta);

    if (music_set.size() > 0)
    {
        if (primary_music.music.getStatus() == sf::Music::Playing && primary_music.mode == None)
        {
            if (secondary_music.music.getStatus() == sf::Music::Stopped)
            {
                if (primary_music.music.getPlayingOffset() > primary_music.music.getDuration() - sf::seconds(fade_music_time))
                {
                    startMusic(getResourceStream(music_set[irandom(0, music_set.size() - 1)]), false);
                }
            }
        }

        if (secondary_music.music.getStatus() == sf::Music::Playing && secondary_music.mode == None)
        {
            if (primary_music.music.getStatus() == sf::Music::Stopped)
            {
                if (secondary_music.music.getPlayingOffset() > secondary_music.music.getDuration() - sf::seconds(fade_music_time))
                {
                    startMusic(getResourceStream(music_set[irandom(0, music_set.size() - 1)]), false);
                }
            }
        }
    }
}

void SoundManager::updateChannel(MusicChannel& channel, float delta)
{
    switch(channel.mode)
    {
    case None:
        break;
    case FadeIn:
        channel.fade_delay -= delta;
        if (channel.fade_delay > 0.0)
        {
            channel.music.setVolume(music_volume * (1.0 - (channel.fade_delay / fade_music_time)));
        }else{
            channel.music.setVolume(music_volume);
            channel.mode = None;
        }
        break;
    case FadeOut:
        channel.fade_delay -= delta;
        if (channel.fade_delay > 0.0)
        {
            channel.music.setVolume(music_volume * (channel.fade_delay / fade_music_time));
        }else{
            channel.music.stop();
            channel.mode = None;
        }
        break;
    }
}
<commit_msg>Few extra checks in the music manager. Hopefully stopping the crashes.<commit_after>#include <stdio.h>
#include <string.h>
#include <SFML/Network.hpp>
#include <iostream>
#include <iomanip>
#include <sstream>

#include "vectorUtils.h"
#include "resources.h"
#include "soundManager.h"
#include "random.h"

#define MAX_SOUNDS 16

SoundManager* soundManager;

SoundManager::SoundManager()
{
    //By creating a SoundBuffer we force SFML to load the sound subsystem. Else this is done when the first sound is loaded, causing a delay at that point.
    sf::SoundBuffer forceLoadBuffer;

    for(unsigned int n=0; n<MAX_SOUNDS; n++)
        activeSoundList.push_back(sf::Sound());

    music_volume = 100.0;
    positional_sound_enabled = false;
    primary_music.mode = None;
    secondary_music.mode = None;
}

SoundManager::~SoundManager()
{
}

void SoundManager::playMusic(string name)
{
    music_set.clear();
    P<ResourceStream> stream = getResourceStream(name);
    if (stream)
        startMusic(stream, true);
}

void SoundManager::playMusicSet(std::vector<string> filenames)
{
    music_set = filenames;
    if (music_set.size() > 0)
        startMusic(getResourceStream(music_set[irandom(0, music_set.size() - 1)]), false);
    else
        stopMusic();
}

void SoundManager::stopMusic()
{
    music_set.clear();
    if (primary_music.music.getStatus() != sf::Music::Stopped)
        primary_music.music.stop();
    if (secondary_music.music.getStatus() != sf::Music::Stopped)
        secondary_music.music.stop();
}

void SoundManager::setMusicVolume(float volume)
{
    if (music_volume != volume)
    {
        music_volume = volume;
        if (primary_music.mode == None && primary_music.music.getStatus() != sf::Music::Stopped)
            primary_music.music.setVolume(music_volume);
        if (secondary_music.mode == None && secondary_music.music.getStatus() != sf::Music::Stopped)
            secondary_music.music.setVolume(music_volume);
    }
}

float SoundManager::getMusicVolume()
{
    return music_volume;
}

void SoundManager::playSound(string name, float pitch, float volume)
{
    sf::SoundBuffer* data = soundMap[name];
    if (data == NULL)
        data = loadSound(name);

    playSoundData(data, pitch, volume);
}

void SoundManager::setListenerPosition(sf::Vector2f position, float angle)
{
    sf::Vector2f listen_vector = sf::vector2FromAngle(angle);
    sf::Listener::setPosition(position.x, 0, position.y);
    sf::Listener::setDirection(listen_vector.x, 0, listen_vector.y);
    positional_sound_enabled = true;
}

void SoundManager::disablePositionalSound()
{
    positional_sound_enabled = false;
}

void SoundManager::playSound(string name, sf::Vector2f position, float min_distance, float attenuation, float pitch, float volume)
{
    if (!positional_sound_enabled)
        return;
    sf::SoundBuffer* data = soundMap[name];
    if (data == NULL)
        data = loadSound(name);
    if (data->getChannelCount() > 1)
        LOG(WARNING) << name << ": Used as positional sound but has more then 1 channel.";

    for(unsigned int n=0; n<activeSoundList.size(); n++)
    {
        sf::Sound& sound = activeSoundList[n];
        if (sound.getStatus() == sf::Sound::Stopped)
        {
            sound.setBuffer(*data);
            sound.setRelativeToListener(false);
            sound.setMinDistance(min_distance);
            sound.setAttenuation(attenuation);
            sound.setPosition(position.x, 0, position.y);
            sound.setPitch(pitch);
            sound.setVolume(volume);
            sound.play();
            return;
        }
    }
}

void SoundManager::setTextToSpeachVoice(string name)
{
}

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

    for (string::const_iterator i = value.begin(), n = value.end(); i != n; ++i) {
        string::value_type c = (*i);
        if (isalnum(c) || c == '-' || c == '_' || c == '.' || c == '~') {
            escaped << c;
        }
        else if (c == ' ')  {
            escaped << '+';
        }
        else {
            escaped << '%' << std::setw(2) << ((int) c) << std::setw(0);
        }
    }

    return escaped.str();
}

void SoundManager::playTextToSpeech(string text)
{
    string name = "TTS:" + text;
    sf::SoundBuffer* data = soundMap[name];
    if (data != NULL)
    {
        playSoundData(data, 1.0, 100.0);
        return;
    }

    sf::Http http("localhost", 59125);
    sf::Http::Request request("process?INPUT_TEXT=" + url_encode(text) + "&INPUT_TYPE=TEXT&OUTPUT_TYPE=AUDIO&AUDIO=WAVE_FILE&LOCALE=en_US&VOICE=dfki-prudence");
    sf::Http::Response response = http.sendRequest(request);

    sf::Http::Response::Status status = response.getStatus();
    if (status == sf::Http::Response::Ok)
    {
        string wave = response.getBody();
        sf::SoundBuffer* data = new sf::SoundBuffer();
        data->loadFromMemory(wave.data(), wave.size());
        soundMap[name] = data;
        playSoundData(data, 1.0, 100.0);
    }
    else
    {
        std::cout << "Error requesting text to speech from Mary server: " << status << std::endl;
    }
}

void SoundManager::playSoundData(sf::SoundBuffer* data, float pitch, float volume)
{
    for(unsigned int n=0; n<activeSoundList.size(); n++)
    {
        sf::Sound& sound = activeSoundList[n];
        if (sound.getStatus() == sf::Sound::Stopped)
        {
            sound.setBuffer(*data);
            sound.setRelativeToListener(true);
            sound.setMinDistance(1);
            sound.setAttenuation(0);
            sound.setPitch(pitch);
            sound.setVolume(volume);
            sound.setPosition(0, 0, 0);
            sound.play();
            return;
        }
    }
}

sf::SoundBuffer* SoundManager::loadSound(string name)
{
    sf::SoundBuffer* data = soundMap[name];
    if (data)
        return data;

    data = new sf::SoundBuffer();

    P<ResourceStream> stream = getResourceStream(name);
    if (!stream) stream = getResourceStream(name + ".wav");
    if (!stream || !data->loadFromStream(**stream))
    {
        LOG(WARNING) << "Failed to load: " << name;
        soundMap[name] = data;
        return data;
    }

    LOG(INFO) << "Loaded: " << name << " of " << data->getDuration().asSeconds() << " seconds";
    soundMap[name] = data;
    return data;
}

void SoundManager::startMusic(P<ResourceStream> stream, bool loop)
{
    if (!stream)
        return;

    MusicChannel* channel = &primary_music;
    MusicChannel* other_channel = &secondary_music;

    if (primary_music.music.getStatus() != sf::Music::Stopped)
    {
        channel = &secondary_music;
        other_channel = &primary_music;
    }

    if (channel->music.getStatus() != sf::Music::Stopped)
        channel->music.stop();
    channel->mode = FadeIn;
    channel->fade_delay = fade_music_time;
    channel->music.openFromStream(**stream);
    channel->stream = stream;
    channel->music.setLoop(loop);
    channel->music.setVolume(0);
    channel->music.play();

    if (other_channel->music.getStatus() != sf::Music::Stopped)
    {
        other_channel->mode = FadeOut;
        other_channel->fade_delay = fade_music_time;
    }
}

void SoundManager::updateTick()
{
    float delta = clock.restart().asSeconds();
    updateChannel(primary_music, delta);
    updateChannel(secondary_music, delta);

    if (music_set.size() > 0)
    {
        if (primary_music.music.getStatus() == sf::Music::Playing && primary_music.mode == None)
        {
            if (secondary_music.music.getStatus() == sf::Music::Stopped)
            {
                if (primary_music.music.getPlayingOffset() > primary_music.music.getDuration() - sf::seconds(fade_music_time))
                {
                    startMusic(getResourceStream(music_set[irandom(0, music_set.size() - 1)]), false);
                }
            }
        }

        if (secondary_music.music.getStatus() == sf::Music::Playing && secondary_music.mode == None)
        {
            if (primary_music.music.getStatus() == sf::Music::Stopped)
            {
                if (secondary_music.music.getPlayingOffset() > secondary_music.music.getDuration() - sf::seconds(fade_music_time))
                {
                    startMusic(getResourceStream(music_set[irandom(0, music_set.size() - 1)]), false);
                }
            }
        }
    }
}

void SoundManager::updateChannel(MusicChannel& channel, float delta)
{
    switch(channel.mode)
    {
    case None:
        break;
    case FadeIn:
        channel.fade_delay -= delta;
        if (channel.fade_delay > 0.0)
        {
            channel.music.setVolume(music_volume * (1.0 - (channel.fade_delay / fade_music_time)));
        }else{
            channel.music.setVolume(music_volume);
            channel.mode = None;
        }
        break;
    case FadeOut:
        channel.fade_delay -= delta;
        if (channel.fade_delay > 0.0)
        {
            channel.music.setVolume(music_volume * (channel.fade_delay / fade_music_time));
        }else{
            channel.music.stop();
            channel.mode = None;
        }
        break;
    }
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2010 Justin Tulloss
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <v8.h>
#include <node.h>
#include <node_events.h>
#include <node_buffer.h>
#include <zmq.h>
#include <assert.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>

using namespace v8;
using namespace node;

namespace zmq {

class Context : public EventEmitter {
public:
    static void
    Initialize (v8::Handle<v8::Object> target) {
        HandleScope scope;

        Local<FunctionTemplate> t = FunctionTemplate::New(New);

        t->Inherit(EventEmitter::constructor_template);
        t->InstanceTemplate()->SetInternalFieldCount(1);

        NODE_SET_PROTOTYPE_METHOD(t, "close", Close);

        target->Set(String::NewSymbol("Context"), t->GetFunction());
    }

    void * getCContext() {
        return context_;
    }

    void Close(Local<Value> exception = Local<Value>()) {
        zmq_term(context_);
        context_ = NULL;
        Unref();
    }

protected:
    static Handle<Value>
    New (const Arguments& args) {
        HandleScope scope;

        Context *context = new Context();
        context->Wrap(args.This());

        return args.This();
    }

    static Handle<Value>
    Close (const Arguments& args) {
        zmq::Context *context = ObjectWrap::Unwrap<Context>(args.This());
        HandleScope scope;
        context->Close();
        return Undefined();
    }

    Context () : EventEmitter () {
        context_ = zmq_init(1);
    }

    ~Context () {
        assert(context_ == NULL);
    }

private:
    void * context_;
};

class Socket : public EventEmitter {
public:
    static void
    Initialize (v8::Handle<v8::Object> target) {
        HandleScope scope;

        Local<FunctionTemplate> t = FunctionTemplate::New(New);

        t->Inherit(EventEmitter::constructor_template);
        t->InstanceTemplate()->SetInternalFieldCount(1);

        NODE_DEFINE_CONSTANT(t, ZMQ_PUB);
        NODE_DEFINE_CONSTANT(t, ZMQ_SUB);
        NODE_DEFINE_CONSTANT(t, ZMQ_REQ);
        NODE_DEFINE_CONSTANT(t, ZMQ_REP);
        NODE_DEFINE_CONSTANT(t, ZMQ_UPSTREAM);
        NODE_DEFINE_CONSTANT(t, ZMQ_DOWNSTREAM);
        NODE_DEFINE_CONSTANT(t, ZMQ_PAIR);

        NODE_SET_PROTOTYPE_METHOD(t, "bind", Bind);
        NODE_SET_PROTOTYPE_METHOD(t, "connect", Connect);
        NODE_SET_PROTOTYPE_METHOD(t, "send", Send);
        NODE_SET_PROTOTYPE_METHOD(t, "recv", Recv);
        NODE_SET_PROTOTYPE_METHOD(t, "close", Close);

        target->Set(String::NewSymbol("Socket"), t->GetFunction());
    }

    int Bind(const char *address) {
        return zmq_bind(socket_, address) == 0;
    }

    int Connect(const char *address) {
        return zmq_connect(socket_, address) == 0;
    }

    int Send(char *msg, int length, int flags) {
        int rc;
        zmq_msg_t z_msg;
        rc = zmq_msg_init_data(&z_msg, msg, length, NULL, NULL);
        if (rc < 0) {
            return rc;
        }

        rc = zmq_send(socket_, &z_msg, flags);
        if (rc < 0) {
            return rc;
        }

        return zmq_msg_close(&z_msg);
    }

    int Recv(int flags, zmq_msg_t* z_msg) {
        int rc;
        rc = zmq_msg_init(z_msg);
        if (rc < 0) {
            return rc;
        }
        return zmq_recv(socket_, z_msg, flags);
    }

    void Close(Local<Value> exception = Local<Value>()) {
        zmq_close(socket_);
        socket_ = NULL;
        Unref();
    }

    const char * ErrorMessage() {
        return zmq_strerror(zmq_errno());
    }

protected:
    static Handle<Value>
    New (const Arguments &args) {
        HandleScope scope;
        if (args.Length() != 2) {
            return ThrowException(Exception::Error(
                String::New("Must pass a context and a type to constructor")));
        }
        Context *context = ObjectWrap::Unwrap<Context>(args[0]->ToObject());
        if (!args[1]->IsNumber()) {
            return ThrowException(Exception::TypeError(
                String::New("Type must be an integer")));
        }
        int type = (int) args[1]->ToInteger()->Value();

        Socket *socket = new Socket(context, type);
        socket->Wrap(args.This());

        return args.This();
    }

    static Handle<Value>
    Connect (const Arguments &args) {
        HandleScope scope;
        Socket *socket = getSocket(args);
        if (!args[0]->IsString()) {
            return ThrowException(Exception::TypeError(
                String::New("Address must be a string!")));
        }

        String::Utf8Value address(args[0]->ToString());
        if (!socket->Connect(*address)) {
            return ThrowException(Exception::Error(
                String::New(socket->ErrorMessage())));
        }
        return Undefined();
    }

    static Handle<Value>
    Bind (const Arguments &args) {
        HandleScope scope;
        Socket *socket = getSocket(args);
        if (!args[0]->IsString()) {
            return ThrowException(Exception::TypeError(
                String::New("Address must be a string!")));
        }

        String::Utf8Value address(args[0]->ToString());
        if (!socket->Bind(*address)) {
            return ThrowException(Exception::Error(
                String::New(socket->ErrorMessage())));
        }
        return Undefined();
    }

    static Handle<Value>
    Send (const Arguments &args) {
        HandleScope scope;
        Socket *socket = getSocket(args);
        if (!args.Length() == 1) {
            return ThrowException(Exception::TypeError(
                String::New("Must pass in a buffer or string to send")));
        }

        String::Utf8Value message(args[0]);
        if (socket->Send(*message, message.length(), 0)) {
            return ThrowException(Exception::Error(
                String::New(socket->ErrorMessage())));
        }
        return Undefined();
    }

    static Handle<Value>
    Recv (const Arguments &args) {
        HandleScope scope;

        Socket *socket = getSocket(args);
        zmq_msg_t z_msg;
        if (socket->Recv(0, &z_msg)) {
            return ThrowException(Exception::Error(
                String::New(socket->ErrorMessage())));
        }
        Local <String>js_msg = String::New(
            (char *) zmq_msg_data(&z_msg),
            zmq_msg_size(&z_msg));
        return scope.Close(js_msg);
    }

    static Handle<Value>
    Close (const Arguments &args) {
        HandleScope scope;

        Socket *socket = getSocket(args);
        socket->Close();
        return Undefined();
    }

    Socket (Context *context, int type) : EventEmitter () {
        socket_ = zmq_socket(context->getCContext(), type);
    }

    ~Socket () {
        assert(socket_ == NULL);
    }

private:
    static Socket * getSocket(const Arguments &args) {
        return ObjectWrap::Unwrap<Socket>(args.This());
    }
    void *socket_;
};

}

extern "C" void
init (Handle<Object> target) {
    HandleScope scope;
    zmq::Context::Initialize(target);
    zmq::Socket::Initialize(target);
}
<commit_msg>Close message before returning to JS<commit_after>/*
 * Copyright (c) 2010 Justin Tulloss
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <v8.h>
#include <node.h>
#include <node_events.h>
#include <node_buffer.h>
#include <zmq.h>
#include <assert.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>

using namespace v8;
using namespace node;

namespace zmq {

class Context : public EventEmitter {
public:
    static void
    Initialize (v8::Handle<v8::Object> target) {
        HandleScope scope;

        Local<FunctionTemplate> t = FunctionTemplate::New(New);

        t->Inherit(EventEmitter::constructor_template);
        t->InstanceTemplate()->SetInternalFieldCount(1);

        NODE_SET_PROTOTYPE_METHOD(t, "close", Close);

        target->Set(String::NewSymbol("Context"), t->GetFunction());
    }

    void * getCContext() {
        return context_;
    }

    void Close(Local<Value> exception = Local<Value>()) {
        zmq_term(context_);
        context_ = NULL;
        Unref();
    }

protected:
    static Handle<Value>
    New (const Arguments& args) {
        HandleScope scope;

        Context *context = new Context();
        context->Wrap(args.This());

        return args.This();
    }

    static Handle<Value>
    Close (const Arguments& args) {
        zmq::Context *context = ObjectWrap::Unwrap<Context>(args.This());
        HandleScope scope;
        context->Close();
        return Undefined();
    }

    Context () : EventEmitter () {
        context_ = zmq_init(1);
    }

    ~Context () {
        assert(context_ == NULL);
    }

private:
    void * context_;
};

class Socket : public EventEmitter {
public:
    static void
    Initialize (v8::Handle<v8::Object> target) {
        HandleScope scope;

        Local<FunctionTemplate> t = FunctionTemplate::New(New);

        t->Inherit(EventEmitter::constructor_template);
        t->InstanceTemplate()->SetInternalFieldCount(1);

        NODE_DEFINE_CONSTANT(t, ZMQ_PUB);
        NODE_DEFINE_CONSTANT(t, ZMQ_SUB);
        NODE_DEFINE_CONSTANT(t, ZMQ_REQ);
        NODE_DEFINE_CONSTANT(t, ZMQ_REP);
        NODE_DEFINE_CONSTANT(t, ZMQ_UPSTREAM);
        NODE_DEFINE_CONSTANT(t, ZMQ_DOWNSTREAM);
        NODE_DEFINE_CONSTANT(t, ZMQ_PAIR);

        NODE_SET_PROTOTYPE_METHOD(t, "bind", Bind);
        NODE_SET_PROTOTYPE_METHOD(t, "connect", Connect);
        NODE_SET_PROTOTYPE_METHOD(t, "send", Send);
        NODE_SET_PROTOTYPE_METHOD(t, "recv", Recv);
        NODE_SET_PROTOTYPE_METHOD(t, "close", Close);

        target->Set(String::NewSymbol("Socket"), t->GetFunction());
    }

    int Bind(const char *address) {
        return zmq_bind(socket_, address) == 0;
    }

    int Connect(const char *address) {
        return zmq_connect(socket_, address) == 0;
    }

    int Send(char *msg, int length, int flags) {
        int rc;
        zmq_msg_t z_msg;
        rc = zmq_msg_init_data(&z_msg, msg, length, NULL, NULL);
        if (rc < 0) {
            return rc;
        }

        rc = zmq_send(socket_, &z_msg, flags);
        if (rc < 0) {
            return rc;
        }

        return zmq_msg_close(&z_msg);
    }

    int Recv(int flags, zmq_msg_t* z_msg) {
        int rc;
        rc = zmq_msg_init(z_msg);
        if (rc < 0) {
            return rc;
        }
        return zmq_recv(socket_, z_msg, flags);
    }

    void Close(Local<Value> exception = Local<Value>()) {
        zmq_close(socket_);
        socket_ = NULL;
        Unref();
    }

    const char * ErrorMessage() {
        return zmq_strerror(zmq_errno());
    }

protected:
    static Handle<Value>
    New (const Arguments &args) {
        HandleScope scope;
        if (args.Length() != 2) {
            return ThrowException(Exception::Error(
                String::New("Must pass a context and a type to constructor")));
        }
        Context *context = ObjectWrap::Unwrap<Context>(args[0]->ToObject());
        if (!args[1]->IsNumber()) {
            return ThrowException(Exception::TypeError(
                String::New("Type must be an integer")));
        }
        int type = (int) args[1]->ToInteger()->Value();

        Socket *socket = new Socket(context, type);
        socket->Wrap(args.This());

        return args.This();
    }

    static Handle<Value>
    Connect (const Arguments &args) {
        HandleScope scope;
        Socket *socket = getSocket(args);
        if (!args[0]->IsString()) {
            return ThrowException(Exception::TypeError(
                String::New("Address must be a string!")));
        }

        String::Utf8Value address(args[0]->ToString());
        if (!socket->Connect(*address)) {
            return ThrowException(Exception::Error(
                String::New(socket->ErrorMessage())));
        }
        return Undefined();
    }

    static Handle<Value>
    Bind (const Arguments &args) {
        HandleScope scope;
        Socket *socket = getSocket(args);
        if (!args[0]->IsString()) {
            return ThrowException(Exception::TypeError(
                String::New("Address must be a string!")));
        }

        String::Utf8Value address(args[0]->ToString());
        if (!socket->Bind(*address)) {
            return ThrowException(Exception::Error(
                String::New(socket->ErrorMessage())));
        }
        return Undefined();
    }

    static Handle<Value>
    Send (const Arguments &args) {
        HandleScope scope;
        Socket *socket = getSocket(args);
        if (!args.Length() == 1) {
            return ThrowException(Exception::TypeError(
                String::New("Must pass in a string to send")));
        }

        String::Utf8Value message(args[0]);
        if (socket->Send(*message, message.length(), 0)) {
            return ThrowException(Exception::Error(
                String::New(socket->ErrorMessage())));
        }
        return Undefined();
    }

    static Handle<Value>
    Recv (const Arguments &args) {
        HandleScope scope;

        Socket *socket = getSocket(args);
        zmq_msg_t z_msg;
        if (socket->Recv(0, &z_msg)) {
            return ThrowException(Exception::Error(
                String::New(socket->ErrorMessage())));
        }
        Local <String>js_msg = String::New(
            (char *) zmq_msg_data(&z_msg),
            zmq_msg_size(&z_msg));
        zmq_msg_close(&z_msg);
        return scope.Close(js_msg);
    }

    static Handle<Value>
    Close (const Arguments &args) {
        HandleScope scope;

        Socket *socket = getSocket(args);
        socket->Close();
        return Undefined();
    }

    Socket (Context *context, int type) : EventEmitter () {
        socket_ = zmq_socket(context->getCContext(), type);
    }

    ~Socket () {
        assert(socket_ == NULL);
    }

private:
    static Socket * getSocket(const Arguments &args) {
        return ObjectWrap::Unwrap<Socket>(args.This());
    }
    void *socket_;
};

}

extern "C" void
init (Handle<Object> target) {
    HandleScope scope;
    zmq::Context::Initialize(target);
    zmq::Socket::Initialize(target);
}
<|endoftext|>


<commit_before>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>

#include <sstream>
#include <iostream>
#include <iomanip>
#include <atomic>
#include <thread>
#include <chrono>

#include "jimi/http_all.h"
#include "stop_watch.h"

using namespace jimi;
using namespace jimi::http;

#ifdef NDEBUG
static const std::size_t kIterations = 3000000;
#else
static const std::size_t kIterations = 10000;
#endif

#if 1
    static const char * http_header =
        "GET /cookies HTTP/1.1\r\n"
        "Host: 127.0.0.1:8090\r\n"
        "Connection: keep-alive\r\n"
        "Cache-Control: max-age=0\r\n"
        "Accept: text/html,application/xhtml+xml,application/xml;q=0.9,*/*;q=0.8\r\n"
        "User-Agent: Mozilla/5.0 (Windows NT 6.1; WOW64) AppleWebKit/537.17 (KHTML, like Gecko) Chrome/24.0.1312.56 Safari/537.17\r\n"
        "Accept-Encoding: gzip,deflate,sdch\r\n"
        "Accept-Language: en-US,en;q=0.8\r\n"
        "Accept-Charset: ISO-8859-1,utf-8;q=0.7,*;q=0.3\r\n"
        "Cookie: name=wookie\r\n"
        "\r\n";
#else
    // Reference from /h2o/picohttpparser/benchmark.c
    static const char * http_header =
        "GET /wp-content/uploads/2010/03/hello-kitty-darth-vader-pink.jpg HTTP/1.1\r\n"
        "Host: www.kittyhell.com\r\n"
        "User-Agent: Mozilla/5.0 (Macintosh; U; Intel Mac OS X 10.6; ja-JP-mac; rv:1.9.2.3) Gecko/20100401 Firefox/3.6.3 Pathtraq/0.9\r\n"
        "Accept: text/html,application/xhtml+xml,application/xml;q=0.9,*/*;q=0.8\r\n"
        "Accept-Language: ja,en-us;q=0.7,en;q=0.3\r\n"
        "Accept-Encoding: gzip,deflate\r\n"
        "Accept-Charset: Shift_JIS,utf-8;q=0.7,*;q=0.7\r\n"
        "Keep-Alive: 115\r\n"
        "Connection: keep-alive\r\n"
        "Cookie: wp_ozh_wsa_visits=2; wp_ozh_wsa_visit_lasttime=xxxxxxxxxx; "
        "__utma=xxxxxxxxx.xxxxxxxxxx.xxxxxxxxxx.xxxxxxxxxx.xxxxxxxxxx.x; "
        "__utmz=xxxxxxxxx.xxxxxxxxxx.x.x.utmccn=(referral)|utmcsr=reader.livedoor.com|utmcct=/reader/|utmcmd=referral\r\n"
        "\r\n";
#endif

void stop_watch_test()
{
    StopWatch sw;
    getTickCountStopWatch swTickCount;
    static const std::size_t kSleepIterations = 20;

    StopWatch::time_stamp_t start_time, end_time;
    StopWatch::time_point_t start_point, end_point;
    start_time = StopWatch::timestamp();
    start_point = StopWatch::now();
    sw.start();
    swTickCount.start();
    for (std::size_t i = 0; i < kSleepIterations; ++i) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    swTickCount.stop();
    sw.stop();
    end_time = StopWatch::timestamp();
    end_point = StopWatch::now();

    std::cout << "Iterations:        " << kSleepIterations << std::endl;
    if (sw.getMillisec() != 0.0) {
        std::cout << "Time spent:        " << sw.getMillisec() << " ms" << std::endl;
        std::cout << "Time spent(*):     " << StopWatch::duration(end_time, start_time).millisecs() << " ms" << std::endl;
        std::cout << "Time spent(**):    " << StopWatch::duration(end_point, start_point).millisecs() << " ms" << std::endl;
        std::cout << "Time spent(Tick):  " << swTickCount.getMillisec() << " ms" << std::endl;
    }
    else {
        std::cout << "Time spent:        0.0 ms" << std::endl;
    }
    std::cout << std::endl;
}

void http_parser_test()
{
    StopWatch sw;
    int sum = 0;
    std::size_t request_len = ::strlen(http_header);

    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  http_parser_test()" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;

    sw.start();
    for (std::size_t i = 0; i < kIterations; ++i) {
        http::Parser<1024> http_parser;
        sum += http_parser.parseRequest(http_header, request_len);
    }
    sw.stop();

    std::cout << std::endl;
    std::cout << "Sum:               " << sum << std::endl;
    std::cout << "Length:            " << ::strlen(http_header) << std::endl;
    std::cout << "Iterations:        " << kIterations << std::endl;
    if (sw.getMillisec() != 0.0) {
        std::cout << "Time spent:        " << sw.getMillisec() << " ms" << std::endl;
        std::cout << "Parse speed:       " << (uint64_t)((double)kIterations / sw.getSecond()) << " Parse/Sec" << std::endl;
        std::cout << "Parse throughput:  " << (double)(kIterations * request_len) / sw.getSecond() / (1024.0 * 1024.0) << " MB/Sec" << std::endl;
    }
    else {
        std::cout << "Time spent:        0.0 ms" << std::endl;
        std::cout << "Parse speed:       0   Parse/Sec" << std::endl;
        std::cout << "Parse throughput:  0.0 MB/Sec" << std::endl;
    }
    std::cout << std::endl;
}

void http_parser_ref_test()
{
    StopWatch sw;
    int sum = 0;
    std::size_t request_len = ::strlen(http_header);

    StringRefHelper helper;
    static int mark = 0;

    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  http_parser_ref_test()" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << std::endl;

    sw.start();
    for (std::size_t i = 0; i < kIterations; ++i) {
        http::ParserRef<1024> http_parser;
        sum += http_parser.parseRequest(http_header, request_len);
        if (mark < 3) {
            if (helper.attach(http_parser.getMethodStr())) {
                helper.truncate();
                std::cout << "method = " << http_parser.getMethodStr().c_str() << std::endl;
                helper.recover();
            }
            mark++;
        }
    }
    sw.stop();

    std::cout << std::endl;
    std::cout << "Sum:               " << sum << std::endl;
    std::cout << "Length:            " << ::strlen(http_header) << std::endl;
    std::cout << "Iterations:        " << kIterations << std::endl;
    if (sw.getMillisec() != 0.0) {
        std::cout << "Time spent:        " << sw.getMillisec() << " ms" << std::endl;
        std::cout << "Parse speed:       " << (uint64_t)((double)kIterations / sw.getSecond()) << " Parse/Sec" << std::endl;
        std::cout << "Parse throughput:  " << (double)(kIterations * request_len) / sw.getSecond() / (1024.0 * 1024.0) << " MB/Sec" << std::endl;
    }
    else {
        std::cout << "Time spent:        0.0 ms" << std::endl;
        std::cout << "Parse speed:       0   Parse/Sec" << std::endl;
        std::cout << "Parse throughput:  0.0 MB/Sec" << std::endl;
    }
    std::cout << std::endl;
}

void http_parser_benchmark()
{
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  http_parser_benchmark()" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << std::endl;

    static atomic<int> loop_cnt(0);
	auto request_len = ::strlen(http_header);
	volatile int64_t count = 0;
    volatile int64_t dummy = 0;
	std::thread counter([&] {
		auto last_count = count;
		auto count_ = count;
        auto dummy_ = dummy;
		do {
            std::atomic_thread_fence(std::memory_order_acquire);
			count_ = count;
            dummy_ = dummy;
            std::atomic_thread_fence(std::memory_order_release);
#if 1
            std::cout << std::right << std::setw(10) << std::setfill(' ') << std::dec;
            std::cout << (count_ - last_count);
            std::cout << ", ";
			std::cout << std::right << std::setw(9) << std::setfill(' ') << std::fixed << std::setprecision(3);
            std::cout << (double)((count_ - last_count) * request_len) / 1024.0 / 1024.0 << " MB/Sec";
            std::cout << ",  ";
            std::cout << std::left << std::dec << ::strlen(http_header);
            std::cout << " bytes,  dummy = ";
            std::cout << std::left << std::dec << dummy_;
            std::cout << std::endl;
#else
            printf("%lld,  %0.3f MB/Sec,  %llu bytes,  %lld\n", (count_ - last_count), (double)((count_ - last_count) * request_len) / 1024.0 / 1024.0,
                ::strlen(http_header), dummy);
#endif
			last_count = count_;
			std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            loop_cnt++;
            if (loop_cnt > 10) {
                break;
            }
		} while (1);
	});

    http::Parser<1024> http_parser;
	do {
        int dummy_tmp = http_parser.parseRequest(http_header, request_len);
        //dummy_tmp += parser.getEntrySize();
        http_parser.reset();
        std::atomic_thread_fence(std::memory_order_acquire);
        dummy += dummy_tmp;
		count++;
        if (loop_cnt > 10) {
            std::atomic_thread_fence(std::memory_order_release);
            if (counter.joinable()) {
                counter.join();
            }
            break;
        }
        std::atomic_thread_fence(std::memory_order_release);
	} while (1);

    std::cout << std::endl;
}

void http_parser_ref_benchmark()
{
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  http_parser_ref_benchmark()" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << std::endl;

    static atomic<int> loop_cnt(0);
	auto request_len = ::strlen(http_header);
	volatile int64_t count = 0;
    volatile int64_t dummy = 0;
	std::thread counter([&] {
		auto last_count = count;
		auto count_ = count;
        auto dummy_ = dummy;
		do {
            std::atomic_thread_fence(std::memory_order_acquire);
			count_ = count;
            dummy_ = dummy;
            std::atomic_thread_fence(std::memory_order_release);
#if 1
            std::cout << std::right << std::setw(10) << std::setfill(' ') << std::dec;
            std::cout << (count_ - last_count);
            std::cout << ", ";
			std::cout << std::right << std::setw(9) << std::setfill(' ') << std::fixed << std::setprecision(3);
            std::cout << (double)((count_ - last_count) * request_len) / 1024.0 / 1024.0 << " MB/Sec";
            std::cout << ",  ";
            std::cout << std::left << std::dec << ::strlen(http_header);
            std::cout << " bytes,  dummy = ";
            std::cout << std::left << std::dec << dummy_;
            std::cout << std::endl;
#else
            printf("%lld,  %0.3f MB/Sec,  %llu bytes,  %lld\n", (count_ - last_count), (double)((count_ - last_count) * request_len) / 1024.0 / 1024.0,
                ::strlen(http_header), dummy);
#endif
			last_count = count_;
			std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            loop_cnt++;
            if (loop_cnt > 10) {
                break;
            }
		} while (1);
	});

    http::ParserRef<1024> http_parser;
	do {
        int dummy_tmp = http_parser.parseRequest(http_header, request_len);
        //dummy_tmp += parser.getEntrySize();
        http_parser.reset();
        std::atomic_thread_fence(std::memory_order_acquire);
        dummy += dummy_tmp;
		count++;
        if (loop_cnt > 10) {
            std::atomic_thread_fence(std::memory_order_release);
            if (counter.joinable()) {
                counter.join();
            }
            break;
        }
        std::atomic_thread_fence(std::memory_order_release);
	} while (1);

    std::cout << std::endl;
}

int main(int argn, char * argv[])
{
    std::cout << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  Program: http_parser_test" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << std::endl;

    http::Parser<1024> http_parser;
    http_parser.parseRequest(http_header, ::strlen(http_header));
    printf("http_parser.getVersion() = %u\n", http_parser.getVersion());
    printf("http_parser.getMethod() = %u\n", http_parser.getMethod());
    printf("\n");
    http_parser.displayFields();
    printf("\n");

#if 1
    //stop_watch_test();
    http_parser_test();
    http_parser_ref_test();
#endif

    http_parser_benchmark();
    http_parser_ref_benchmark();

#ifdef _WIN32
    ::system("pause");
#endif
    return 0;
}
<commit_msg>小改动;<commit_after>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>

#include <sstream>
#include <iostream>
#include <iomanip>
#include <atomic>
#include <thread>
#include <chrono>

#include "jimi/http_all.h"
#include "stop_watch.h"

using namespace jimi;
using namespace jimi::http;

#ifdef NDEBUG
static const std::size_t kIterations = 3000000;
#else
static const std::size_t kIterations = 10000;
#endif

#if 1
    static const char * http_header =
        "GET /cookies HTTP/1.1\r\n"
        "Host: 127.0.0.1:8090\r\n"
        "Connection: keep-alive\r\n"
        "Cache-Control: max-age=0\r\n"
        "Accept: text/html,application/xhtml+xml,application/xml;q=0.9,*/*;q=0.8\r\n"
        "User-Agent: Mozilla/5.0 (Windows NT 6.1; WOW64) AppleWebKit/537.17 (KHTML, like Gecko) Chrome/24.0.1312.56 Safari/537.17\r\n"
        "Accept-Encoding: gzip,deflate,sdch\r\n"
        "Accept-Language: en-US,en;q=0.8\r\n"
        "Accept-Charset: ISO-8859-1,utf-8;q=0.7,*;q=0.3\r\n"
        "Cookie: name=wookie\r\n"
        "\r\n";
#else
    // Reference from /h2o/picohttpparser/benchmark.c
    static const char * http_header =
        "GET /wp-content/uploads/2010/03/hello-kitty-darth-vader-pink.jpg HTTP/1.1\r\n"
        "Host: www.kittyhell.com\r\n"
        "User-Agent: Mozilla/5.0 (Macintosh; U; Intel Mac OS X 10.6; ja-JP-mac; rv:1.9.2.3) Gecko/20100401 Firefox/3.6.3 Pathtraq/0.9\r\n"
        "Accept: text/html,application/xhtml+xml,application/xml;q=0.9,*/*;q=0.8\r\n"
        "Accept-Language: ja,en-us;q=0.7,en;q=0.3\r\n"
        "Accept-Encoding: gzip,deflate\r\n"
        "Accept-Charset: Shift_JIS,utf-8;q=0.7,*;q=0.7\r\n"
        "Keep-Alive: 115\r\n"
        "Connection: keep-alive\r\n"
        "Cookie: wp_ozh_wsa_visits=2; wp_ozh_wsa_visit_lasttime=xxxxxxxxxx; "
        "__utma=xxxxxxxxx.xxxxxxxxxx.xxxxxxxxxx.xxxxxxxxxx.xxxxxxxxxx.x; "
        "__utmz=xxxxxxxxx.xxxxxxxxxx.x.x.utmccn=(referral)|utmcsr=reader.livedoor.com|utmcct=/reader/|utmcmd=referral\r\n"
        "\r\n";
#endif

void stop_watch_test()
{
    StopWatch sw;
    getTickCountStopWatch swTickCount;
    static const std::size_t kSleepIterations = 20;

    StopWatch::time_stamp_t start_time, end_time;
    StopWatch::time_point_t start_point, end_point;
    start_time = StopWatch::timestamp();
    start_point = StopWatch::now();
    sw.start();
    swTickCount.start();
    for (std::size_t i = 0; i < kSleepIterations; ++i) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    swTickCount.stop();
    sw.stop();
    end_time = StopWatch::timestamp();
    end_point = StopWatch::now();

    std::cout << "Iterations:        " << kSleepIterations << std::endl;
    if (sw.getMillisec() != 0.0) {
        std::cout << "Time spent:        " << sw.getMillisec() << " ms" << std::endl;
        std::cout << "Time spent(*):     " << StopWatch::duration(end_time, start_time).millisecs() << " ms" << std::endl;
        std::cout << "Time spent(**):    " << StopWatch::duration(end_point, start_point).millisecs() << " ms" << std::endl;
        std::cout << "Time spent(Tick):  " << swTickCount.getMillisec() << " ms" << std::endl;
    }
    else {
        std::cout << "Time spent:        0.0 ms" << std::endl;
    }
    std::cout << std::endl;
}

void http_parser_test()
{
    StopWatch sw;
    int sum = 0;
    std::size_t request_len = ::strlen(http_header);

    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  http_parser_test()" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;

    sw.start();
    for (std::size_t i = 0; i < kIterations; ++i) {
        http::Parser<1024> http_parser;
        sum += http_parser.parseRequest(http_header, request_len);
    }
    sw.stop();

    std::cout << std::endl;
    std::cout << "Sum:               " << sum << std::endl;
    std::cout << "Length:            " << ::strlen(http_header) << std::endl;
    std::cout << "Iterations:        " << kIterations << std::endl;
    if (sw.getMillisec() != 0.0) {
        std::cout << "Time spent:        " << sw.getMillisec() << " ms" << std::endl;
        std::cout << "Parse speed:       " << (uint64_t)((double)kIterations / sw.getSecond()) << " Parse/Sec" << std::endl;
        std::cout << "Parse throughput:  " << (double)(kIterations * request_len) / sw.getSecond() / (1024.0 * 1024.0) << " MB/Sec" << std::endl;
    }
    else {
        std::cout << "Time spent:        0.0 ms" << std::endl;
        std::cout << "Parse speed:       0   Parse/Sec" << std::endl;
        std::cout << "Parse throughput:  0.0 MB/Sec" << std::endl;
    }
    std::cout << std::endl;
}

void http_parser_ref_test()
{
    StopWatch sw;
    int sum = 0;
    std::size_t request_len = ::strlen(http_header);

    StringRefHelper helper;
    static int mark = 0;

    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  http_parser_ref_test()" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << std::endl;

    sw.start();
    for (std::size_t i = 0; i < kIterations; ++i) {
        http::ParserRef<1024> http_parser;
        sum += http_parser.parseRequest(http_header, request_len);
        if (mark < 3) {
            if (helper.attach(http_parser.getMethodStr())) {
                helper.truncate();
                std::cout << "method = " << http_parser.getMethodStr().c_str() << std::endl;
                helper.recover();
            }
            mark++;
        }
    }
    sw.stop();

    std::cout << std::endl;
    std::cout << "Sum:               " << sum << std::endl;
    std::cout << "Length:            " << ::strlen(http_header) << std::endl;
    std::cout << "Iterations:        " << kIterations << std::endl;
    if (sw.getMillisec() != 0.0) {
        std::cout << "Time spent:        " << sw.getMillisec() << " ms" << std::endl;
        std::cout << "Parse speed:       " << (uint64_t)((double)kIterations / sw.getSecond()) << " Parse/Sec" << std::endl;
        std::cout << "Parse throughput:  " << (double)(kIterations * request_len) / sw.getSecond() / (1024.0 * 1024.0) << " MB/Sec" << std::endl;
    }
    else {
        std::cout << "Time spent:        0.0 ms" << std::endl;
        std::cout << "Parse speed:       0   Parse/Sec" << std::endl;
        std::cout << "Parse throughput:  0.0 MB/Sec" << std::endl;
    }
    std::cout << std::endl;
}

void http_parser_benchmark()
{
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  http_parser_benchmark()" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << std::endl;

    static const int kMaxLoop = 11;
    static std::atomic<int> loop_cnt(0);
	auto request_len = ::strlen(http_header);
	volatile int64_t count = 0;
    volatile int64_t dummy = 0;
	std::thread counter([&] {
		auto last_count = count;
		auto count_ = count;
        auto dummy_ = dummy;
		do {
            std::atomic_thread_fence(std::memory_order_acquire);
			count_ = count;
            dummy_ = dummy;
            std::atomic_thread_fence(std::memory_order_release);
#if 1
            std::cout << std::right << std::setw(10) << std::setfill(' ') << std::dec;
            std::cout << (count_ - last_count);
            std::cout << ", ";
			std::cout << std::right << std::setw(9) << std::setfill(' ') << std::fixed << std::setprecision(3);
            std::cout << (double)((count_ - last_count) * request_len) / 1024.0 / 1024.0 << " MB/Sec";
            std::cout << ",  ";
            std::cout << std::left << std::dec << ::strlen(http_header);
            std::cout << " bytes,  dummy = ";
            std::cout << std::left << std::dec << dummy_;
            std::cout << std::endl;
#else
            printf("%lld,  %0.3f MB/Sec,  %llu bytes,  %lld\n", (count_ - last_count), (double)((count_ - last_count) * request_len) / 1024.0 / 1024.0,
                ::strlen(http_header), dummy);
#endif
			last_count = count_;
			std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            loop_cnt++;
            if (loop_cnt > kMaxLoop) {
                break;
            }
		} while (1);
	});

    http::Parser<1024> http_parser;
	do {
        int dummy_tmp = http_parser.parseRequest(http_header, request_len);
        //dummy_tmp += parser.getEntrySize();
        http_parser.reset();
        std::atomic_thread_fence(std::memory_order_acquire);
        dummy += dummy_tmp;
		count++;
        if (loop_cnt > kMaxLoop) {
            std::atomic_thread_fence(std::memory_order_release);
            if (counter.joinable()) {
                counter.join();
            }
            break;
        }
        std::atomic_thread_fence(std::memory_order_release);
	} while (1);

    std::cout << std::endl;
}

void http_parser_ref_benchmark()
{
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  http_parser_ref_benchmark()" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << std::endl;

    static const int kMaxLoop = 11;
    static std::atomic<int> loop_cnt(0);
	auto request_len = ::strlen(http_header);
	volatile int64_t count = 0;
    volatile int64_t dummy = 0;
	std::thread counter([&] {
		auto last_count = count;
		auto count_ = count;
        auto dummy_ = dummy;
		do {
            std::atomic_thread_fence(std::memory_order_acquire);
			count_ = count;
            dummy_ = dummy;
            std::atomic_thread_fence(std::memory_order_release);
#if 1
            std::cout << std::right << std::setw(10) << std::setfill(' ') << std::dec;
            std::cout << (count_ - last_count);
            std::cout << ", ";
			std::cout << std::right << std::setw(9) << std::setfill(' ') << std::fixed << std::setprecision(3);
            std::cout << (double)((count_ - last_count) * request_len) / 1024.0 / 1024.0 << " MB/Sec";
            std::cout << ",  ";
            std::cout << std::left << std::dec << ::strlen(http_header);
            std::cout << " bytes,  dummy = ";
            std::cout << std::left << std::dec << dummy_;
            std::cout << std::endl;
#else
            printf("%lld,  %0.3f MB/Sec,  %llu bytes,  %lld\n", (count_ - last_count), (double)((count_ - last_count) * request_len) / 1024.0 / 1024.0,
                ::strlen(http_header), dummy);
#endif
			last_count = count_;
			std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            loop_cnt++;
            if (loop_cnt > kMaxLoop) {
                break;
            }
		} while (1);
	});

    http::ParserRef<1024> http_parser;
	do {
        int dummy_tmp = http_parser.parseRequest(http_header, request_len);
        //dummy_tmp += parser.getEntrySize();
        http_parser.reset();
        std::atomic_thread_fence(std::memory_order_acquire);
        dummy += dummy_tmp;
		count++;
        if (loop_cnt > kMaxLoop) {
            std::atomic_thread_fence(std::memory_order_release);
            if (counter.joinable()) {
                counter.join();
            }
            break;
        }
        std::atomic_thread_fence(std::memory_order_release);
	} while (1);

    std::cout << std::endl;
}

int main(int argn, char * argv[])
{
    std::cout << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << "  Program: http_parser_test" << std::endl;
    std::cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << std::endl;
    std::cout << std::endl;

    http::Parser<1024> http_parser;
    http_parser.parseRequest(http_header, ::strlen(http_header));
    printf("http_parser.getVersion() = %u\n", http_parser.getVersion());
    printf("http_parser.getMethod() = %u\n", http_parser.getMethod());
    printf("\n");
    http_parser.displayFields();
    printf("\n");

#if 1
    //stop_watch_test();
    http_parser_test();
    http_parser_ref_test();
#endif

    http_parser_benchmark();
    http_parser_ref_benchmark();

#ifdef _WIN32
    ::system("pause");
#endif
    return 0;
}
<|endoftext|>


<commit_before>#pragma once

#include <vector>
#include <algorithm>
#include <tuple>

namespace ssmpack {
namespace filter {

template <class TFilter>
class RecursiveBayesianBase {
  public:
  template <class TMeasurement, class... TArgs>
  decltype(auto) step(const TMeasurement &measurement, const TArgs &... args) {
    static_cast<TFilter *>(this)->predict();
    return static_cast<TFilter *>(this)->correct(measurement);
  }

  template <class TMeasurement, class TControl = std::tuple<>>
  decltype(auto)
  filter(const std::vector<TMeasurement> &measurements,
         const std::vector<TControl> &controls = std::vector<std::tuple<>>()) {

    using TState = decltype(step(measurements.at(0)));
    std::vector<TState> state_seq(measurements.size() + 1);
    state_seq[0] = static_cast<TFilter *>(this)->initialize();
    auto pm = measurements.cbegin();

    if (controls.size() == 0) {
      std::generate_n(state_seq.begin() + 1, state_seq.size(),
                      [this, &pm]() { return step(*(pm++)); });
    } else {
      // else what?
    }

    return state_seq;

  }

};

} // namespace filter
} // namespace ssmpack
<commit_msg>resolving out of range issue<commit_after>#pragma once

#include <vector>
#include <algorithm>
#include <tuple>

namespace ssmpack {
namespace filter {

template <class TFilter>
class RecursiveBayesianBase {
  public:
  template <class TMeasurement, class... TArgs>
  decltype(auto) step(const TMeasurement &measurement, const TArgs &... args) {
    static_cast<TFilter *>(this)->predict();
    return static_cast<TFilter *>(this)->correct(measurement);
  }

  template <class TMeasurement, class TControl = std::tuple<>>
  decltype(auto)
  filter(const std::vector<TMeasurement> &measurements,
         const std::vector<TControl> &controls = std::vector<std::tuple<>>()) {

    using TState = decltype(step(measurements.at(0)));
    std::vector<TState> state_seq(measurements.size() + 1);
    state_seq[0] = static_cast<TFilter *>(this)->initialize();
    auto pm = measurements.cbegin();

    if (controls.size() == 0) {
      std::generate_n(state_seq.begin() + 1, state_seq.size()-1,
                      [this, &pm]() { return step(*(pm++)); });
    } else {
      // else what?
    }

    return state_seq;

  }

};

} // namespace filter
} // namespace ssmpack
<|endoftext|>


<commit_before>

#include <iostream>
#include <cstdlib>
#include "drake/systems/plants/RigidBodyTree.h"

using namespace std;

int main(int argc, char* argv[]) {
  if (argc < 2) {
    cerr << "Usage: urdfKinTest urdf_filename" << endl;
    exit(-1);
  }
  RigidBodyTree* model = new RigidBodyTree(argv[1]);
  cout << "=======" << endl;

  // run kinematics with second derivatives 100 times
  Eigen::VectorXd q = model->getZeroConfiguration();
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model->num_velocities);
  int i;

  if (argc >= 2 + model->num_positions) {
    for (i = 0; i < model->num_positions; i++)
      sscanf(argv[2 + i], "%lf", &q(i));
  }

  // for (i=0; i<model->num_dof; i++)
  // 	 q(i)=(double)rand() / RAND_MAX;
  KinematicsCache<double> cache = model->doKinematics(q, v);
  //  }

  //  const Vector4d zero(0, 0, 0, 1);
  Eigen::Vector3d zero = Eigen::Vector3d::Zero();
  Eigen::Matrix<double, 6, 1> pt;

  for (i = 0; i < model->bodies.size(); i++) {
    //    model->forwardKin(i, zero, 1, pt);
    auto pt = model->transformPoints(cache, zero, i, 0);
    auto rpy = model->relativeRollPitchYaw(cache, i, 0);
    Eigen::Matrix<double, 6, 1> x;
    x << pt, rpy;
    //    cout << i << ": forward kin: " << model->bodies[i].linkname << " is at
    //    " << pt.transpose() << endl;
    cout << model->bodies[i]->linkname << " ";
    cout << x.transpose() << endl;
    //    for (int j=0; j<pt.size(); j++)
    //    	cout << pt(j) << " ";
  }

  auto phi = model->positionConstraints<double>(cache);
  cout << "phi = " << phi.transpose() << endl;

  delete model;
  return 0;
}
<commit_msg>Remove literal tabs from urdfKinTest.cpp.<commit_after>

#include <iostream>
#include <cstdlib>
#include "drake/systems/plants/RigidBodyTree.h"

using namespace std;

int main(int argc, char* argv[]) {
  if (argc < 2) {
    cerr << "Usage: urdfKinTest urdf_filename" << endl;
    exit(-1);
  }
  RigidBodyTree* model = new RigidBodyTree(argv[1]);
  cout << "=======" << endl;

  // run kinematics with second derivatives 100 times
  Eigen::VectorXd q = model->getZeroConfiguration();
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model->num_velocities);
  int i;

  if (argc >= 2 + model->num_positions) {
    for (i = 0; i < model->num_positions; i++)
      sscanf(argv[2 + i], "%lf", &q(i));
  }

  // for (i=0; i<model->num_dof; i++)
  //   q(i)=(double)rand() / RAND_MAX;
  KinematicsCache<double> cache = model->doKinematics(q, v);
  //  }

  //  const Vector4d zero(0, 0, 0, 1);
  Eigen::Vector3d zero = Eigen::Vector3d::Zero();
  Eigen::Matrix<double, 6, 1> pt;

  for (i = 0; i < model->bodies.size(); i++) {
    //    model->forwardKin(i, zero, 1, pt);
    auto pt = model->transformPoints(cache, zero, i, 0);
    auto rpy = model->relativeRollPitchYaw(cache, i, 0);
    Eigen::Matrix<double, 6, 1> x;
    x << pt, rpy;
    //    cout << i << ": forward kin: " << model->bodies[i].linkname << " is at
    //    " << pt.transpose() << endl;
    cout << model->bodies[i]->linkname << " ";
    cout << x.transpose() << endl;
    //    for (int j=0; j<pt.size(); j++)
    //        cout << pt(j) << " ";
  }

  auto phi = model->positionConstraints<double>(cache);
  cout << "phi = " << phi.transpose() << endl;

  delete model;
  return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2010 ARM Limited
 * All rights reserved
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Ali Saidi
 */

#include "base/trace.hh"
#include "dev/arm/rv_ctrl.hh"
#include "mem/packet.hh"
#include "mem/packet_access.hh"

RealViewCtrl::RealViewCtrl(Params *p)
    : BasicPioDevice(p)
{
    pioSize = 0xD4;
}

Tick
RealViewCtrl::read(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);
    assert(pkt->getSize() == 4);
    Addr daddr = pkt->getAddr() - pioAddr;
    pkt->allocate();

    switch(daddr) {
      case ProcId:
        pkt->set(params()->proc_id);
        break;
      case Clock24:
        Tick clk;
        clk = (Tick)(curTick() / (24 * SimClock::Float::MHz));
        pkt->set((uint32_t)(clk));
        break;
      case Clock100:
        Tick clk100;
        clk100 = (Tick)(curTick() / (100 * SimClock::Float::MHz));
        pkt->set((uint32_t)(clk100));
        break;
      case Flash:
        pkt->set<uint32_t>(0);
        break;
      case Clcd:
        pkt->set<uint32_t>(0x00001F00);
        break;
      case Osc0:
        pkt->set<uint32_t>(0x00012C5C);
        break;
      case Osc1:
        pkt->set<uint32_t>(0x00002CC0);
        break;
      case Osc2:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Osc3:
        pkt->set<uint32_t>(0x00020211);
        break;
      case Osc4:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Lock:
        pkt->set<uint32_t>(sysLock);
        break;
      default:
        panic("Tried to read RealView I/O at offset %#x that doesn't exist\n", daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;

}

Tick
RealViewCtrl::write(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);

    Addr daddr = pkt->getAddr() - pioAddr;
    switch (daddr) {
      case Flash:
      case Clcd:
      case Osc0:
      case Osc1:
      case Osc2:
      case Osc3:
      case Osc4:
        break;
      case Lock:
        sysLock.lockVal = pkt->get<uint16_t>();
        break;
      default:
        panic("Tried to write RVIO at offset %#x that doesn't exist\n", daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;
}

void
RealViewCtrl::serialize(std::ostream &os)
{
}

void
RealViewCtrl::unserialize(Checkpoint *cp, const std::string &section)
{
}

RealViewCtrl *
RealViewCtrlParams::create()
{
    return new RealViewCtrl(this);
}
<commit_msg>RealView: Fix the 24 and 100MHz clocks which were providing incorrect values.<commit_after>/*
 * Copyright (c) 2010 ARM Limited
 * All rights reserved
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Ali Saidi
 */

#include "base/trace.hh"
#include "dev/arm/rv_ctrl.hh"
#include "mem/packet.hh"
#include "mem/packet_access.hh"

RealViewCtrl::RealViewCtrl(Params *p)
    : BasicPioDevice(p)
{
    pioSize = 0xD4;
}

Tick
RealViewCtrl::read(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);
    assert(pkt->getSize() == 4);
    Addr daddr = pkt->getAddr() - pioAddr;
    pkt->allocate();

    switch(daddr) {
      case ProcId:
        pkt->set(params()->proc_id);
        break;
      case Clock24:
        Tick clk;
        clk = (Tick)(curTick() / (24 * SimClock::Int::us));
        pkt->set((uint32_t)(clk));
        break;
      case Clock100:
        Tick clk100;
        clk100 = (Tick)(curTick() / (100 * SimClock::Int::us));
        pkt->set((uint32_t)(clk100));
        break;
      case Flash:
        pkt->set<uint32_t>(0);
        break;
      case Clcd:
        pkt->set<uint32_t>(0x00001F00);
        break;
      case Osc0:
        pkt->set<uint32_t>(0x00012C5C);
        break;
      case Osc1:
        pkt->set<uint32_t>(0x00002CC0);
        break;
      case Osc2:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Osc3:
        pkt->set<uint32_t>(0x00020211);
        break;
      case Osc4:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Lock:
        pkt->set<uint32_t>(sysLock);
        break;
      default:
        panic("Tried to read RealView I/O at offset %#x that doesn't exist\n", daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;

}

Tick
RealViewCtrl::write(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);

    Addr daddr = pkt->getAddr() - pioAddr;
    switch (daddr) {
      case Flash:
      case Clcd:
      case Osc0:
      case Osc1:
      case Osc2:
      case Osc3:
      case Osc4:
        break;
      case Lock:
        sysLock.lockVal = pkt->get<uint16_t>();
        break;
      default:
        panic("Tried to write RVIO at offset %#x that doesn't exist\n", daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;
}

void
RealViewCtrl::serialize(std::ostream &os)
{
}

void
RealViewCtrl::unserialize(Checkpoint *cp, const std::string &section)
{
}

RealViewCtrl *
RealViewCtrlParams::create()
{
    return new RealViewCtrl(this);
}
<|endoftext|>


<commit_before>// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// testing default transition sequence.
// This test requires that the transitions are set
// as depicted in design.ros2.org

#include <gtest/gtest.h>

extern "C"
{
#include "rclc_lifecycle/rclc_lifecycle.h"

#include <lifecycle_msgs/msg/state.h>
#include <lifecycle_msgs/msg/transition.h>
}

TEST(TestRclcLifecycle, lifecycle_node) {
  rcl_context_t context = rcl_get_zero_initialized_context();
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_allocator_t allocator = rcl_get_default_allocator();

  rcl_init_options_init(&init_options, allocator);
  rcl_init(0, nullptr, &init_options, &context);

  rcl_node_t my_node = rcl_get_zero_initialized_node();
  rcl_node_options_t node_ops = rcl_node_get_default_options();
  rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops);

  rclc_lifecycle_node_t lifecycle_node;
  rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();

  rcl_ret_t res = rclc_make_node_a_lifecycle_node(
    &lifecycle_node,
    &my_node,
    &state_machine_,
    &node_ops);

  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    RCL_RET_OK,
    rcl_lifecycle_state_machine_is_initialized(lifecycle_node.state_machine));
}

TEST(TestRclcLifecycle, lifecycle_node_transitions) {
  rcl_context_t context = rcl_get_zero_initialized_context();
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_allocator_t allocator = rcl_get_default_allocator();

  rcl_init_options_init(&init_options, allocator);
  rcl_init(0, nullptr, &init_options, &context);

  rcl_node_t my_node = rcl_get_zero_initialized_node();
  rcl_node_options_t node_ops = rcl_node_get_default_options();
  rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops);

  rclc_lifecycle_node_t lifecycle_node;
  rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();

  rcl_ret_t res = rclc_make_node_a_lifecycle_node(
    &lifecycle_node,
    &my_node,
    &state_machine_,
    &node_ops);

  // configure
  res = rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE,
    true);
  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE,
    lifecycle_node.state_machine->current_state->id);

  // activate
  res = rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE,
    true);
  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE,
    lifecycle_node.state_machine->current_state->id);

  // deactivate
  res = rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE,
    true);
  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE,
    lifecycle_node.state_machine->current_state->id);

  // cleanup
  res = rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP,
    true);
  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED,
    lifecycle_node.state_machine->current_state->id);
}
<commit_msg>Unit test for lifecycle change hooks/callbacks<commit_after>// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// testing default transition sequence.
// This test requires that the transitions are set
// as depicted in design.ros2.org

#include <gtest/gtest.h>

extern "C"
{
#include <lifecycle_msgs/msg/state.h>
#include <lifecycle_msgs/msg/transition.h>

#include "rclc_lifecycle/rclc_lifecycle.h"
}

static int callback_mockup_counter = 0;

rcl_ret_t callback_mockup_0()
{
  callback_mockup_counter += 1;
  return RCL_RET_OK;
}

rcl_ret_t callback_mockup_1()
{
  callback_mockup_counter += 2;
  return RCL_RET_OK;
}

rcl_ret_t callback_mockup_2()
{
  callback_mockup_counter += 4;
  return RCL_RET_OK;
}

rcl_ret_t callback_mockup_3()
{
  callback_mockup_counter += 8;
  return RCL_RET_OK;
}

TEST(TestRclcLifecycle, lifecycle_node) {
  rcl_context_t context = rcl_get_zero_initialized_context();
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_allocator_t allocator = rcl_get_default_allocator();

  rcl_ret_t res = rcl_init_options_init(&init_options, allocator);
  res += rcl_init(0, nullptr, &init_options, &context);

  rcl_node_t my_node = rcl_get_zero_initialized_node();
  rcl_node_options_t node_ops = rcl_node_get_default_options();
  res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops);

  rclc_lifecycle_node_t lifecycle_node;
  rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();

  res += rclc_make_node_a_lifecycle_node(
    &lifecycle_node,
    &my_node,
    &state_machine_,
    &node_ops);

  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    RCL_RET_OK,
    rcl_lifecycle_state_machine_is_initialized(lifecycle_node.state_machine));
}

TEST(TestRclcLifecycle, lifecycle_node_transitions) {
  rcl_context_t context = rcl_get_zero_initialized_context();
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_allocator_t allocator = rcl_get_default_allocator();

  rcl_ret_t res = rcl_init_options_init(&init_options, allocator);
  res += rcl_init(0, nullptr, &init_options, &context);

  rcl_node_t my_node = rcl_get_zero_initialized_node();
  rcl_node_options_t node_ops = rcl_node_get_default_options();
  res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops);

  rclc_lifecycle_node_t lifecycle_node;
  rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();

  res += rclc_make_node_a_lifecycle_node(
    &lifecycle_node,
    &my_node,
    &state_machine_,
    &node_ops);

  // configure
  res = rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE,
    true);
  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE,
    lifecycle_node.state_machine->current_state->id);

  // activate
  res = rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE,
    true);
  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    lifecycle_msgs__msg__State__PRIMARY_STATE_ACTIVE,
    lifecycle_node.state_machine->current_state->id);

  // deactivate
  res = rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE,
    true);
  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    lifecycle_msgs__msg__State__PRIMARY_STATE_INACTIVE,
    lifecycle_node.state_machine->current_state->id);

  // cleanup
  res = rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP,
    true);
  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(
    lifecycle_msgs__msg__State__PRIMARY_STATE_UNCONFIGURED,
    lifecycle_node.state_machine->current_state->id);
}

TEST(TestRclcLifecycle, lifecycle_node_callbacks) {
  rcl_context_t context = rcl_get_zero_initialized_context();
  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_allocator_t allocator = rcl_get_default_allocator();

  rcl_ret_t res = rcl_init_options_init(&init_options, allocator);
  res += rcl_init(0, nullptr, &init_options, &context);

  rcl_node_t my_node = rcl_get_zero_initialized_node();
  rcl_node_options_t node_ops = rcl_node_get_default_options();
  res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops);

  rclc_lifecycle_node_t lifecycle_node;
  rcl_lifecycle_state_machine_t state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();

  res += rclc_make_node_a_lifecycle_node(
    &lifecycle_node,
    &my_node,
    &state_machine_,
    &node_ops);

  // register callbacks
  rclc_lifecycle_register_on_configure(&lifecycle_node, &callback_mockup_0);
  rclc_lifecycle_register_on_activate(&lifecycle_node, &callback_mockup_1);
  rclc_lifecycle_register_on_deactivate(&lifecycle_node, &callback_mockup_2);
  rclc_lifecycle_register_on_cleanup(&lifecycle_node, &callback_mockup_3);

  // run through the lifecycle
  res += rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE,
    true);
  res += rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE,
    true);
  res += rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_DEACTIVATE,
    true);
  res += rclc_lifecycle_change_state(
    &lifecycle_node,
    lifecycle_msgs__msg__Transition__TRANSITION_CLEANUP,
    true);

  EXPECT_EQ(RCL_RET_OK, res);
  EXPECT_EQ(15, callback_mockup_counter);
}
<|endoftext|>


<commit_before>#include "physics/n_body_system.hpp"

#include <cmath>
#include <functional>
#include <vector>

#include "geometry/r3_element.hpp"
#ifndef _MANAGED
#include "glog/logging.h"
#endif
#include "integrators/symplectic_partitioned_runge_kutta_integrator.hpp"
#include "quantities/quantities.hpp"

using principia::geometry::R3Element;
using principia::integrators::SPRKIntegrator;
using principia::integrators::SymplecticIntegrator;
using principia::quantities::Acceleration;
using principia::quantities::Exponentiation;
using principia::quantities::GravitationalParameter;
using principia::quantities::Length;
using principia::quantities::Speed;

namespace principia {
namespace physics {

template<typename InertialFrame>
NBodySystem<InertialFrame>::NBodySystem(
    std::unique_ptr<Bodies>&& massive_bodies,
    std::unique_ptr<Bodies>&& massless_bodies,
    std::unique_ptr<Trajectories>&& trajectories)
    : massive_bodies_(massive_bodies == nullptr ?
                          std::unique_ptr<Bodies>(new Bodies) :
                          std::move(massive_bodies)),
      massless_bodies_(massless_bodies == nullptr ?
                           std::unique_ptr<Bodies>(new Bodies) :
                           std::move(massless_bodies)),
      trajectories_(std::move(trajectories)) {
  // Parameter checking.
  for (auto const& body : *massive_bodies_) {
#ifndef _MANAGED
    CHECK(!body->is_massless());
#endif
    bodies_.push_back(body.get());
  }
  for (auto const& body : *massless_bodies_) {
#ifndef _MANAGED
    CHECK(body->is_massless());
#endif
    bodies_.push_back(body.get());
  }
#ifndef _MANAGED
  CHECK_EQ(trajectories_->size(), bodies_.size());
#endif
}

template<typename InertialFrame>
std::vector<Body const*> NBodySystem<InertialFrame>::massless_bodies() const {
  std::vector<Body const*> result;
  for (auto const& body : *massless_bodies_) {
    result.push_back(body.get());
  }
  return result;
}

template<typename InertialFrame>
std::vector<Body const*> NBodySystem<InertialFrame>::massive_bodies() const {
  std::vector<Body const*> result;
  for (auto const& body : *massive_bodies_) {
    result.push_back(body.get());
  }
  return result;
}

template<typename InertialFrame>
std::vector<Body const*> NBodySystem<InertialFrame>::bodies() const {
  std::vector<Body const*> result;
  for (auto const& body : bodies_) {
    result.push_back(body);
  }
  return result;
}

template<typename InertialFrame>
std::vector<Trajectory<InertialFrame> const*>
NBodySystem<InertialFrame>::trajectories() const {
  std::vector<Trajectory<InertialFrame> const*> result;
  for (auto const& trajectory : *trajectories_) {
    result.push_back(trajectory.get());
  }
  return result;
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::Integrate(
    SymplecticIntegrator<Length, Speed> const& integrator,
    Time const& tmax,
    Time const& Δt,
    int const sampling_period) {
  SymplecticIntegrator<Length, Speed>::Parameters parameters;
  std::vector<SymplecticIntegrator<Length, Speed>::SystemState> solution;

  // Prepare the input data.
  std::unique_ptr<Time> reference_time;
  for (auto const& trajectory : *trajectories_) {
    // TODO(phl): Relation with bodies_?
    R3Element<Length> const& position =
        trajectory->last_position().coordinates();
    R3Element<Speed> const& velocity =
        trajectory->last_velocity().coordinates();
    Time const& time = trajectory->last_time();
    for (int i = 0; i < 3; ++i) {
      parameters.initial.positions.emplace_back(position[i]);
    }
    for (int i = 0; i < 3; ++i) {
      parameters.initial.momenta.emplace_back(velocity[i]);
    }
    parameters.initial.time = time;
    // All the positions/velocities must be for the same time.
    if (reference_time == nullptr) {
      reference_time.reset(new Time(time));
    } else {
#ifndef _MANAGED
      CHECK_EQ(*reference_time, time);
#endif
    }
  }

  parameters.tmax = tmax;
  parameters.Δt = Δt;
  parameters.sampling_period = sampling_period;
  dynamic_cast<const SPRKIntegrator<Length, Speed>*>(&integrator)->Solve(
      std::bind(&NBodySystem::ComputeGravitationalAccelerations, this,
                std::placeholders::_1,
                std::placeholders::_2,
                std::placeholders::_3),
      &ComputeGravitationalVelocities,
      parameters, &solution);

  // TODO(phl): Ignoring errors for now.
  // Loop over the time steps.
  for (std::size_t i = 0; i < solution.size(); ++i) {
    SymplecticIntegrator<Length, Speed>::SystemState const& state = solution[i];
    Time const& time = state.time.value;
#ifndef _MANAGED
    CHECK_EQ(state.positions.size(), state.momenta.size());
#endif
    // Loop over the dimensions.
    for (std::size_t k = 0, b = 0; k < state.positions.size(); k += 3, ++b) {
      // TODO(phl): bodies_ vs. trajectory.
      Trajectory<InertialFrame>* trajectory = (*trajectories_)[b].get();
      Vector<Length, InertialFrame> const position(
          R3Element<Length>(state.positions[k].value,
                            state.positions[k + 1].value,
                            state.positions[k + 2].value));
      Vector<Speed, InertialFrame> const velocity(
          R3Element<Speed>(state.momenta[k].value,
                           state.momenta[k + 1].value,
                           state.momenta[k + 2].value));
      trajectory->Append(time,
                         DegreesOfFreedom<InertialFrame>(position, velocity));
    }
  }
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::ComputeGravitationalAccelerations(
    Time const& t,
    std::vector<Length> const& q,
    std::vector<Acceleration>* result) const {
  result->assign(result->size(), Acceleration());

  // TODO(phl): Used to deal with proper accelerations here.
  for (std::size_t b1 = 0, three_b1 = 0;
       b1 < massive_bodies_->size();
       ++b1, three_b1 += 3) {
    GravitationalParameter const& body1_gravitational_parameter =
        (*massive_bodies_)[b1]->gravitational_parameter();
    for (std::size_t b2 = b1 + 1; b2 < massive_bodies_->size(); ++b2) {
      std::size_t const three_b2 = 3 * b2;
      Length const Δq0 = q[three_b1] - q[three_b2];
      Length const Δq1 = q[three_b1 + 1] - q[three_b2 + 1];
      Length const Δq2 = q[three_b1 + 2] - q[three_b2 + 2];

      Exponentiation<Length, 2> const squared_distance =
          Δq0 * Δq0 + Δq1 * Δq1 + Δq2 * Δq2;
      Exponentiation<Length, -3> const multiplier =
          Sqrt(squared_distance) / (squared_distance * squared_distance);

      auto const μ2OverRSquared =
          (*massive_bodies_)[b2]->gravitational_parameter() * multiplier;
      (*result)[three_b1] -= Δq0 * μ2OverRSquared;
      (*result)[three_b1 + 1] -= Δq1 * μ2OverRSquared;
      (*result)[three_b1 + 2] -= Δq2 * μ2OverRSquared;
      // Lex. III. Actioni contrariam semper & æqualem esse reactionem:
      // sive corporum duorum actiones in se mutuo semper esse æquales &
      // in partes contrarias dirigi.
      auto const μ1OverRSquared = body1_gravitational_parameter * multiplier;
      (*result)[three_b2] += Δq0 * μ1OverRSquared;
      (*result)[three_b2 + 1] += Δq1 * μ1OverRSquared;
      (*result)[three_b2 + 2] += Δq2 * μ1OverRSquared;
    }
    for (std::size_t b2 = 0; b2 < massless_bodies_->size(); ++b2) {
      std::size_t const three_b2 = 3 * b2;
      Length const Δq0 = q[three_b1] - q[three_b2];
      Length const Δq1 = q[three_b1 + 1] - q[three_b2 + 1];
      Length const Δq2 = q[three_b1 + 2] - q[three_b2 + 2];

      Exponentiation<Length, 2> const squared_distance =
          Δq0 * Δq0 + Δq1 * Δq1 + Δq2 * Δq2;
      Exponentiation<Length, -3> const multiplier =
          Sqrt(squared_distance) / (squared_distance * squared_distance);

      auto const μ1OverRSquared = body1_gravitational_parameter * multiplier;
      (*result)[three_b2] += Δq0 * μ1OverRSquared;
      (*result)[three_b2 + 1] += Δq1 * μ1OverRSquared;
      (*result)[three_b2 + 2] += Δq2 * μ1OverRSquared;
    }
  }
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::ComputeGravitationalVelocities(
    std::vector<Speed> const& p,
    std::vector<Speed>* result) {
  *result = p;
}

}  // namespace physics
}  // namespace principia
<commit_msg>Lint.<commit_after>#include "physics/n_body_system.hpp"

#include <cmath>
#include <functional>
#include <vector>

#include "geometry/r3_element.hpp"
#ifndef _MANAGED
#include "glog/logging.h"
#endif
#include "integrators/symplectic_partitioned_runge_kutta_integrator.hpp"
#include "quantities/quantities.hpp"

using principia::geometry::R3Element;
using principia::integrators::SPRKIntegrator;
using principia::integrators::SymplecticIntegrator;
using principia::quantities::Acceleration;
using principia::quantities::Exponentiation;
using principia::quantities::GravitationalParameter;
using principia::quantities::Length;
using principia::quantities::Speed;

namespace principia {
namespace physics {

template<typename InertialFrame>
NBodySystem<InertialFrame>::NBodySystem(
    std::unique_ptr<Bodies>&& massive_bodies,
    std::unique_ptr<Bodies>&& massless_bodies,
    std::unique_ptr<Trajectories>&& trajectories)
    : massive_bodies_(massive_bodies == nullptr ?
                          std::unique_ptr<Bodies>(new Bodies) :
                          std::move(massive_bodies)),
      massless_bodies_(massless_bodies == nullptr ?
                           std::unique_ptr<Bodies>(new Bodies) :
                           std::move(massless_bodies)),
      trajectories_(std::move(trajectories)) {
  // Parameter checking.
  for (auto const& body : *massive_bodies_) {
#ifndef _MANAGED
    CHECK(!body->is_massless());
#endif
    bodies_.push_back(body.get());
  }
  for (auto const& body : *massless_bodies_) {
#ifndef _MANAGED
    CHECK(body->is_massless());
#endif
    bodies_.push_back(body.get());
  }
#ifndef _MANAGED
  CHECK_EQ(trajectories_->size(), bodies_.size());
#endif
}

template<typename InertialFrame>
std::vector<Body const*> NBodySystem<InertialFrame>::massless_bodies() const {
  std::vector<Body const*> result;
  for (auto const& body : *massless_bodies_) {
    result.push_back(body.get());
  }
  return result;
}

template<typename InertialFrame>
std::vector<Body const*> NBodySystem<InertialFrame>::massive_bodies() const {
  std::vector<Body const*> result;
  for (auto const& body : *massive_bodies_) {
    result.push_back(body.get());
  }
  return result;
}

template<typename InertialFrame>
std::vector<Body const*> NBodySystem<InertialFrame>::bodies() const {
  std::vector<Body const*> result;
  for (auto const& body : bodies_) {
    result.push_back(body);
  }
  return result;
}

template<typename InertialFrame>
std::vector<Trajectory<InertialFrame> const*>
NBodySystem<InertialFrame>::trajectories() const {
  std::vector<Trajectory<InertialFrame> const*> result;
  for (auto const& trajectory : *trajectories_) {
    result.push_back(trajectory.get());
  }
  return result;
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::Integrate(
    SymplecticIntegrator<Length, Speed> const& integrator,
    Time const& tmax,
    Time const& Δt,
    int const sampling_period) {
  SymplecticIntegrator<Length, Speed>::Parameters parameters;
  std::vector<SymplecticIntegrator<Length, Speed>::SystemState> solution;

  // Prepare the input data.
  std::unique_ptr<Time> reference_time;
  for (auto const& trajectory : *trajectories_) {
    // TODO(phl): Relation with bodies_?
    R3Element<Length> const& position =
        trajectory->last_position().coordinates();
    R3Element<Speed> const& velocity =
        trajectory->last_velocity().coordinates();
    Time const& time = trajectory->last_time();
    for (int i = 0; i < 3; ++i) {
      parameters.initial.positions.emplace_back(position[i]);
    }
    for (int i = 0; i < 3; ++i) {
      parameters.initial.momenta.emplace_back(velocity[i]);
    }
    parameters.initial.time = time;
    // All the positions/velocities must be for the same time.
    if (reference_time == nullptr) {
      reference_time.reset(new Time(time));
    } else {
#ifndef _MANAGED
      CHECK_EQ(*reference_time, time);
#endif
    }
  }

  parameters.tmax = tmax;
  parameters.Δt = Δt;
  parameters.sampling_period = sampling_period;
  dynamic_cast<const SPRKIntegrator<Length, Speed>*>(&integrator)->Solve(
      std::bind(&NBodySystem::ComputeGravitationalAccelerations, this,
                std::placeholders::_1,
                std::placeholders::_2,
                std::placeholders::_3),
      &ComputeGravitationalVelocities,
      parameters, &solution);

  // TODO(phl): Ignoring errors for now.
  // Loop over the time steps.
  for (std::size_t i = 0; i < solution.size(); ++i) {
    SymplecticIntegrator<Length, Speed>::SystemState const& state = solution[i];
    Time const& time = state.time.value;
#ifndef _MANAGED
    CHECK_EQ(state.positions.size(), state.momenta.size());
#endif
    // Loop over the dimensions.
    for (std::size_t k = 0, b = 0; k < state.positions.size(); k += 3, ++b) {
      // TODO(phl): bodies_ vs. trajectory.
      Trajectory<InertialFrame>* trajectory = (*trajectories_)[b].get();
      Vector<Length, InertialFrame> const position(
          R3Element<Length>(state.positions[k].value,
                            state.positions[k + 1].value,
                            state.positions[k + 2].value));
      Vector<Speed, InertialFrame> const velocity(
          R3Element<Speed>(state.momenta[k].value,
                           state.momenta[k + 1].value,
                           state.momenta[k + 2].value));
      trajectory->Append(time,
                         DegreesOfFreedom<InertialFrame>(position, velocity));
    }
  }
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::ComputeGravitationalAccelerations(
    Time const& t,
    std::vector<Length> const& q,
    std::vector<Acceleration>* result) const {
  result->assign(result->size(), Acceleration());

  // TODO(phl): Used to deal with proper accelerations here.
  for (std::size_t b1 = 0, three_b1 = 0;
       b1 < massive_bodies_->size();
       ++b1, three_b1 += 3) {
    GravitationalParameter const& body1_gravitational_parameter =
        (*massive_bodies_)[b1]->gravitational_parameter();
    for (std::size_t b2 = b1 + 1; b2 < massive_bodies_->size(); ++b2) {
      std::size_t const three_b2 = 3 * b2;
      Length const Δq0 = q[three_b1] - q[three_b2];
      Length const Δq1 = q[three_b1 + 1] - q[three_b2 + 1];
      Length const Δq2 = q[three_b1 + 2] - q[three_b2 + 2];

      Exponentiation<Length, 2> const squared_distance =
          Δq0 * Δq0 + Δq1 * Δq1 + Δq2 * Δq2;
      Exponentiation<Length, -3> const multiplier =
          Sqrt(squared_distance) / (squared_distance * squared_distance);

      auto const μ2OverRSquared =
          (*massive_bodies_)[b2]->gravitational_parameter() * multiplier;
      (*result)[three_b1] -= Δq0 * μ2OverRSquared;
      (*result)[three_b1 + 1] -= Δq1 * μ2OverRSquared;
      (*result)[three_b1 + 2] -= Δq2 * μ2OverRSquared;
      // Lex. III. Actioni contrariam semper & æqualem esse reactionem:
      // sive corporum duorum actiones in se mutuo semper esse æquales &
      // in partes contrarias dirigi.
      auto const μ1OverRSquared = body1_gravitational_parameter * multiplier;
      (*result)[three_b2] += Δq0 * μ1OverRSquared;
      (*result)[three_b2 + 1] += Δq1 * μ1OverRSquared;
      (*result)[three_b2 + 2] += Δq2 * μ1OverRSquared;
    }
    for (std::size_t b2 = 0; b2 < massless_bodies_->size(); ++b2) {
      std::size_t const three_b2 = 3 * b2;
      Length const Δq0 = q[three_b1] - q[three_b2];
      Length const Δq1 = q[three_b1 + 1] - q[three_b2 + 1];
      Length const Δq2 = q[three_b1 + 2] - q[three_b2 + 2];

      Exponentiation<Length, 2> const squared_distance =
          Δq0 * Δq0 + Δq1 * Δq1 + Δq2 * Δq2;
      Exponentiation<Length, -3> const multiplier =
          Sqrt(squared_distance) / (squared_distance * squared_distance);

      auto const μ1OverRSquared = body1_gravitational_parameter * multiplier;
      (*result)[three_b2] += Δq0 * μ1OverRSquared;
      (*result)[three_b2 + 1] += Δq1 * μ1OverRSquared;
      (*result)[three_b2 + 2] += Δq2 * μ1OverRSquared;
    }
  }
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::ComputeGravitationalVelocities(
    std::vector<Speed> const& p,
    std::vector<Speed>* result) {
  *result = p;
}

}  // namespace physics
}  // namespace principia
<|endoftext|>