<commit_before>#include <string>
#include <iostream>
#include <windows.h>
#include <conio.h>
#include "Hook.h"

using namespace std;

int main( int argc, char* argv[] )
{
    try
    {
        // TODO: Verify that console window can be hooked as well.

        // Parse parameters
        LPCSTR injectedDll = "Hook.dll";

        // Try hook call
        HWND handle = FindWindow( TEXT("TARGET"), TEXT("TARGET") );
        if( NULL == handle ) { throw error("GetForegroundWindow"); }

        char buffer[128];
        InjectDll_HookMessageQueue( handle, buffer );
    }
    catch( error &er )
    {
        cout << "ERROR. " << er.Message << ": " << er.ErrorCode << endl;
        system("pause");
        return 1;
    }
    catch( ... )
    {
        cout << "Unknown error. Run without parameters to see help." << endl;
        system("pause");
        return 1;
    }

    system("pause");
    return 0;
}

<commit_msg>Brought Windows Host under tests harness.<commit_after>#include <string>
#include <iostream>
#include <windows.h>
#include <conio.h>
#include <tchar.h>
#include <memory>
#include "Hook.h"

using namespace std;

class Process
{
public:
    Process( LPTSTR path )
    {

    }

    ~Process()
    {

    }

private:

};

HANDLE g_process = NULL;
HANDLE g_thread = NULL;

void MyCreateProcess( LPTSTR path )
{
    size_t length = _tcslen( path ) + 1;
    auto_ptr<TCHAR> path_copy( new TCHAR[ length ] );
    _tcscpy_s( path_copy.get(), length, path );

    STARTUPINFO si;
    PROCESS_INFORMATION pi;

    ZeroMemory( &si, sizeof(si) );
    ZeroMemory( &pi, sizeof(pi) );

    si.cb = sizeof(si);

    BOOL success = CreateProcess( NULL, path_copy.get(), NULL, NULL, FALSE, 0, NULL, NULL, &si, &pi );
    if( !success ) { throw error("CreateProcess"); }

    g_process = pi.hProcess;
    g_thread = pi.hThread;
}

int main( int argc, char* argv[] )
{
    try
    {
        // TODO: Verify that console window can be hooked as well.
        MyCreateProcess( TEXT("Host.Windows.exe") );

        // Parse parameters
        LPCSTR injectedDll = "Hook.dll";

        // Try hook call
        HWND handle = NULL;
        while( NULL == handle )
        {
            handle = FindWindow( TEXT("TARGET"), TEXT("TARGET") );
            Sleep(10);
        }
        if( NULL == handle ) { throw error("FindWindow"); }

        char buffer[128];
        InjectDll_HookMessageQueue( handle, buffer );
        cout << "Hook successfully deployed." << endl;

        BOOL success = TerminateProcess( g_process, 0 );
        if( !success ) { throw error("TerminateProcess"); }

        success = CloseHandle( g_process );
        if( !success ) { throw error("CloseHandle - process"); }

        success = CloseHandle( g_thread );
        if( !success ) { throw error("CloseHandle - thread"); }
    }
    catch( error &er )
    {
        cout << "ERROR. " << er.Message << ": " << er.ErrorCode << endl;
        system("pause");
        return 1;
    }
    catch( ... )
    {
        cout << "Unknown error. Run without parameters to see help." << endl;
        system("pause");
        return 1;
    }

    system("pause");
    return 0;
}

<|endoftext|>


<commit_before>#include <Math/Functions.h>
#include <DefaultFramebuffer.h>
#include <Renderer.h>
#include <Timeline.h>
#include <DebugTools/ForceRenderer.h>
#include <DebugTools/ObjectRenderer.h>
#include <DebugTools/ResourceManager.h>
#include <DebugTools/ShapeRenderer.h>
#ifndef CORRADE_TARGET_NACL
#include <Platform/Sdl2Application.h>
#else
#include <Platform/NaClApplication.h>
#endif
#include <SceneGraph/Camera2D.h>
#include <SceneGraph/DualComplexTransformation.h>
#include <SceneGraph/Object.h>
#include <SceneGraph/Scene.h>
#include <Physics/ObjectShape.h>
#include <Physics/ObjectShapeGroup.h>
#include <Physics/Box.h>
#include <Physics/LineSegment.h>
#include <Physics/Point.h>
#include <Physics/ShapeGroup.h>
#include <Physics/Sphere.h>

#ifdef MAGNUM_BUILD_STATIC
#include <Shaders/magnumShadersResourceImport.hpp>
#endif

#include "Kotel.h"

namespace Kotel { namespace Prototype {

class Forces2D: public Platform::Application {
    public:
        explicit Forces2D(const Arguments& arguments);

        void viewportEvent(const Vector2i& size) override;
        void drawEvent() override;
        void keyPressEvent(KeyEvent& event) override;
        void keyReleaseEvent(KeyEvent& event) override;

    private:
        void globalPhysicsStep(const Float time, const Float delta);
        void physicsStep(const Float time, const Float delta);
        void applyForce(const Vector2& position, const Vector2& force);
        void applyImpulse(const Vector2& position, const Vector2& impulse);

        DebugTools::ResourceManager debugResourceManager;

        Timeline timeline;
        Scene2D scene;
        Object2D cameraObject;
        SceneGraph::Camera2D<> *camera;
        SceneGraph::DrawableGroup<2> drawables;
        Physics::ObjectShapeGroup2D visualizationShapes, collisionShapes;

        Object2D *tube, *vehicle, *body, *armLeft, *armRight, *engineLeft, *engineRight;

        struct {
            Physics::Sphere2D *tubeMin, *tubeMax;
            Physics::Point2D *vehicleBody, *vehicleArmLeft, *vehicleArmRight;
        } shapes;

        struct {
            Vector2 weight,
                engineLeftArm, engineRightArm,
                frictionLeftArm, frictionRightArm,
                totalLeftArm, totalRightArm;
        } forces;

        struct {
            Float massInverted,
                powerArm,
                restitution,
                friction,
                momentOfInertiaInverted;

            Float physicsTimeDelta;
            Vector2 gravity;

            Deg armAngle, engineAngle;
            Float armRadius;
        } parameters;

        struct {
            Float currentPowerLeftArm,
                currentPowerRightArm,
                physicsTime,
                physicsTimeAccumulator,
                angularSpeed,
                torque;

            Vector2 linearVelocity,
                force;
        } state;

};

Forces2D::Forces2D(const Arguments& arguments): Platform::Application(arguments, nullptr), state() {
    /* Try to create MSAA context */
    auto conf = new Configuration();
    #ifndef CORRADE_TARGET_NACL
    conf->setTitle("Kotel::Prototype::Forces2D");
    #endif
    conf->setSampleCount(16);
    if(!tryCreateContext(conf)) {
        Warning() << "Cannot enable 16x MSAA, fallback to no-AA rendering";
        createContext(conf->setSampleCount(0));
    } else delete conf;

    Renderer::setClearColor(Color3<>(0.125f));
    Renderer::setFeature(Renderer::Feature::Blending, true);
    Renderer::setBlendFunction(Renderer::BlendFunction::SourceAlpha, Renderer::BlendFunction::OneMinusSourceAlpha);

    /* Camera setup */
    cameraObject.setParent(&scene);
    (camera = new SceneGraph::Camera2D<>(&cameraObject))
        ->setAspectRatioPolicy(SceneGraph::AspectRatioPolicy::Extend)
        ->setProjection(Vector2(3.0f));

    /* Debug draw setup */
    debugResourceManager.set("weight", (new DebugTools::ForceRendererOptions)
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(190.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("engines", (new DebugTools::ForceRendererOptions)
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(50.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("friction", (new DebugTools::ForceRendererOptions)
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(115.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("total", (new DebugTools::ForceRendererOptions)
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(245.0f), 0.75f, 0.9f, 0.75f)));
    debugResourceManager.set("collision", (new DebugTools::ShapeRendererOptions)
        ->setPointSize(0.1f)->setColor(Color4<>::fromHSV(Deg(25.0f), 0.75f, 0.9f, 0.75f)));
    debugResourceManager.set("vehicle", (new DebugTools::ShapeRendererOptions)
        ->setColor(Color3<>(0.5f)));
    debugResourceManager.set("parameters", (new DebugTools::ObjectRendererOptions)
        ->setSize(0.1f));

    /* Parameters */
    parameters.physicsTimeDelta = 1.0f/120.0f;
    parameters.gravity = Vector2::yAxis(-9.81);
    parameters.armAngle = Deg(110.0f);
    parameters.engineAngle = Deg(0.0f);
    parameters.armRadius = 1.0f;
    const Float massBody = 260.0f;
    const Float massArm = 80.0f;
    parameters.massInverted = 1.0f/(massBody + 2*massArm);
    parameters.powerArm = 1000.0f;
    parameters.restitution = 0.3f;
    parameters.friction = 0.16f;

    /* Object initialization */
    tube = new Object2D(&scene);
    vehicle = new Object2D(&scene);
    (body = new Object2D(vehicle))
        ->translate({});
    (armLeft = new Object2D(vehicle))
        ->translate(Vector2::yAxis(-parameters.armRadius))
        ->rotate(-parameters.armAngle/2);
    (armRight = new Object2D(vehicle))
        ->translate(Vector2::yAxis(-parameters.armRadius))
        ->rotate(parameters.armAngle/2);
    (engineLeft = new Object2D(vehicle))
        ->rotate(-parameters.engineAngle)
        ->translate(Vector2::yAxis(-parameters.armRadius))
        ->rotate(-parameters.armAngle/2);
    (engineRight = new Object2D(vehicle))
        ->rotate(parameters.engineAngle)
        ->translate(Vector2::yAxis(-parameters.armRadius))
        ->rotate(parameters.armAngle/2);

    /* Compute center of mass and move it to center of the object */
    const Vector2 centerOfMass = (body->transformation().translation()*massBody +
        (engineLeft->transformation().translation() + engineRight->transformation().translation())*
        massArm)*parameters.massInverted;
    vehicle->translate(centerOfMass);
    body->translate(-centerOfMass);
    armLeft->translate(-centerOfMass);
    armRight->translate(-centerOfMass);
    engineLeft->translate(-centerOfMass);
    engineRight->translate(-centerOfMass);

    /* Weight force */
    forces.weight = parameters.gravity/parameters.massInverted;

    /* Moment of inertia */
    parameters.momentOfInertiaInverted = 1.0f/(body->transformation().translation().dot()*massBody +
        engineLeft->transformation().translation().dot()*massArm +
        engineRight->transformation().translation().dot()*massArm);

    /* Tube and vehicle collision shapes */
    shapes.tubeMin = new Physics::Sphere2D({}, 0.99f);
    shapes.tubeMax = new Physics::Sphere2D({}, 1.01f);
    shapes.vehicleBody = new Physics::Point2D(body->transformation().translation()+Vector2::yAxis(0.15f));
    shapes.vehicleArmLeft = new Physics::Point2D(engineLeft->transformation().translation());
    shapes.vehicleArmRight = new Physics::Point2D(engineRight->transformation().translation());
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(tube, &collisionShapes))
        ->setShape(std::ref(*shapes.tubeMin) || std::ref(*shapes.tubeMax)),
        "collision", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(vehicle, &collisionShapes))
        ->setShape(std::ref(*shapes.vehicleBody) || std::ref(*shapes.vehicleArmLeft) || std::ref(*shapes.vehicleArmRight)),
        "collision", &drawables);

    /* Vehicle visualization */
    const auto armA = Vector2::yAxis(parameters.armRadius-0.2f);
    const auto armB = Vector2::yAxis(0.03f);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(body, &visualizationShapes))
        ->setShape(Physics::Sphere2D({}, 0.2f)), "vehicle", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(armLeft, &visualizationShapes))
        ->setShape(Physics::LineSegment2D(armA, armB)), "vehicle", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(armRight, &visualizationShapes))
        ->setShape(Physics::LineSegment2D(armA, armB)), "vehicle", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(engineLeft, &visualizationShapes))
        ->setShape(Physics::Box2D(Matrix3::scaling({0.1f, 0.03f}))), "vehicle", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(engineRight, &visualizationShapes))
        ->setShape(Physics::Box2D(Matrix3::scaling({0.1f, 0.03f}))), "vehicle", &drawables);

    /* Vehicle center-of-mass visualization */
    new DebugTools::ObjectRenderer2D(vehicle, "parameters", &drawables);

    /* Gravity force visualization */
    new DebugTools::ForceRenderer2D(vehicle, {}, &forces.weight, "weight", &drawables);

    /* Engine and friction forces visualization */
    new DebugTools::ForceRenderer2D(engineLeft, {}, &forces.engineLeftArm, "engines", &drawables);
    new DebugTools::ForceRenderer2D(engineRight, {}, &forces.engineRightArm, "engines", &drawables);
    new DebugTools::ForceRenderer2D(engineLeft, {}, &forces.frictionLeftArm, "friction", &drawables);
    new DebugTools::ForceRenderer2D(engineRight, {}, &forces.frictionRightArm, "friction", &drawables);

    /* Total forces visualization */
    new DebugTools::ForceRenderer2D(engineLeft, {}, &forces.totalLeftArm, "total", &drawables);
    new DebugTools::ForceRenderer2D(engineRight, {}, &forces.totalRightArm, "total", &drawables);

    /* Zero-time physics step */
    physicsStep(state.physicsTime, parameters.physicsTimeDelta);
    timeline.start();
}

void Forces2D::viewportEvent(const Vector2i& size) {
    defaultFramebuffer.setViewport({{}, size});

    camera->setViewport(size);
}

void Forces2D::drawEvent() {
    defaultFramebuffer.bind(DefaultFramebuffer::Target::Draw);
    defaultFramebuffer.clear(DefaultFramebuffer::Clear::Color);

    /* Do physics steps in elapsed time */
    state.physicsTimeAccumulator += timeline.previousFrameDuration();
    while(state.physicsTimeAccumulator >= parameters.physicsTimeDelta) {
        globalPhysicsStep(state.physicsTime, parameters.physicsTimeDelta);
        state.physicsTimeAccumulator -= parameters.physicsTimeDelta;
        state.physicsTime += parameters.physicsTimeDelta;
    }

    visualizationShapes.setClean();
    collisionShapes.setClean();
    camera->draw(drawables);

    swapBuffers();
    timeline.nextFrame();
    redraw();
}

void Forces2D::keyPressEvent(KeyEvent& event) {
    if(event.key() == KeyEvent::Key::Left) {
        state.currentPowerLeftArm = parameters.powerArm;
        state.currentPowerRightArm = parameters.powerArm;
    } else if(event.key() == KeyEvent::Key::Right) {
        state.currentPowerLeftArm = -parameters.powerArm;
        state.currentPowerRightArm = -parameters.powerArm;
    } else return;

    event.setAccepted();
    redraw();
}

void Forces2D::keyReleaseEvent(KeyEvent& event) {
    state.currentPowerLeftArm = state.currentPowerRightArm = {};

    event.setAccepted();
    redraw();
}

void Forces2D::globalPhysicsStep(const Float time, const Float delta) {
    /* Compute force and torque at original position */
    state.force = {};
    state.torque = {};
    physicsStep(time, delta);

    state.linearVelocity += 0.5f*state.force*parameters.massInverted*delta;
    state.angularSpeed += 0.5f*state.torque*parameters.momentOfInertiaInverted*delta;

    /* Check penetration */
    collisionShapes.setClean();
    for(Physics::Point2D* shape: {shapes.vehicleBody, shapes.vehicleArmLeft, shapes.vehicleArmRight}) {
        if(*shape % *shapes.tubeMax) continue;

        const Vector2 normal = -shape->transformedPosition();
        const Vector2 position = shape->transformedPosition() - vehicle->absoluteTransformation().translation();
        const Vector2 velocity = state.linearVelocity + state.angularSpeed*position.perpendicular();
        const Vector2 impulse = (-(1.0f + parameters.restitution)*Vector2::dot(velocity, normal)/
            (normal.dot()*parameters.massInverted + Math::pow<2>(Vector2::cross(position, normal))*parameters.momentOfInertiaInverted))*normal;

        applyImpulse(shape->transformedPosition(), impulse);
    }

    /* New position and rotation (around COM) */
    vehicle->translate(state.linearVelocity*delta)
        ->rotate(Rad(state.angularSpeed*delta), SceneGraph::TransformationType::Local)
        ->normalizeRotation();

    /* Compute force at new position */
    state.force = {};
    state.torque = {};
    physicsStep(time+delta, delta);

    /* New velocity */
    state.linearVelocity += 0.5f*state.force*parameters.massInverted*delta;
    state.angularSpeed += 0.5f*state.torque*parameters.momentOfInertiaInverted*delta;
}

void Forces2D::physicsStep(const Float, const Float) {
    /* Compute tangent and normal vectors */
    const Vector2 engineDirectionLeft(engineLeft->absoluteTransformation().rotation());
    const Vector2 engineDirectionRight(engineRight->absoluteTransformation().rotation());

    /* Reset forces */
    forces.totalLeftArm = forces.totalRightArm =
        forces.frictionLeftArm = forces.frictionRightArm = {};

    /* Add engine forces*/
    forces.engineLeftArm = engineDirectionLeft*state.currentPowerLeftArm;
    forces.engineRightArm = engineDirectionRight*state.currentPowerRightArm;
    forces.totalLeftArm += forces.engineLeftArm;
    forces.totalRightArm += forces.engineRightArm;

    /* Apply forces */
    applyForce(vehicle->absoluteTransformation().translation(), forces.weight);
    applyForce(engineLeft->absoluteTransformation().translation(), forces.totalLeftArm);
    applyForce(engineLeft->absoluteTransformation().translation(), forces.totalRightArm);
}

void Forces2D::applyForce(const Vector2& position, const Vector2& force) {
    state.force += force;
    state.torque += Vector2::cross(position - vehicle->absoluteTransformation().translation(), force);
}

void Forces2D::applyImpulse(const Vector2& position, const Vector2& impulse) {
    state.linearVelocity += impulse*parameters.massInverted;
    state.angularSpeed += Vector2::cross(position - vehicle->absoluteTransformation().translation(), impulse)*parameters.momentOfInertiaInverted;
}

}}

MAGNUM_APPLICATION_MAIN(Kotel::Prototype::Forces2D)
<commit_msg>prototypes: visualization and parameter tweaks.<commit_after>#include <Math/Functions.h>
#include <DefaultFramebuffer.h>
#include <Renderer.h>
#include <Timeline.h>
#include <DebugTools/ForceRenderer.h>
#include <DebugTools/ObjectRenderer.h>
#include <DebugTools/ResourceManager.h>
#include <DebugTools/ShapeRenderer.h>
#ifndef CORRADE_TARGET_NACL
#include <Platform/Sdl2Application.h>
#else
#include <Platform/NaClApplication.h>
#endif
#include <SceneGraph/Camera2D.h>
#include <SceneGraph/DualComplexTransformation.h>
#include <SceneGraph/Object.h>
#include <SceneGraph/Scene.h>
#include <Physics/ObjectShape.h>
#include <Physics/ObjectShapeGroup.h>
#include <Physics/Box.h>
#include <Physics/LineSegment.h>
#include <Physics/Point.h>
#include <Physics/ShapeGroup.h>
#include <Physics/Sphere.h>

#ifdef MAGNUM_BUILD_STATIC
#include <Shaders/magnumShadersResourceImport.hpp>
#endif

#include "Kotel.h"

namespace Kotel { namespace Prototype {

class Forces2D: public Platform::Application {
    public:
        explicit Forces2D(const Arguments& arguments);

        void viewportEvent(const Vector2i& size) override;
        void drawEvent() override;
        void keyPressEvent(KeyEvent& event) override;
        void keyReleaseEvent(KeyEvent& event) override;

    private:
        void globalPhysicsStep(const Float time, const Float delta);
        void physicsStep(const Float time, const Float delta);
        void applyForce(const Vector2& position, const Vector2& force);
        void applyImpulse(const Vector2& position, const Vector2& impulse);

        DebugTools::ResourceManager debugResourceManager;

        Timeline timeline;
        Scene2D scene;
        Object2D cameraObject;
        SceneGraph::Camera2D<> *camera;
        SceneGraph::DrawableGroup<2> drawables;
        Physics::ObjectShapeGroup2D visualizationShapes, collisionShapes;

        Object2D *tube, *vehicle, *body, *armLeft, *armRight, *engineLeft, *engineRight;

        struct {
            Physics::Sphere2D *tubeMin, *tubeMax;
            Physics::Point2D *vehicleBody, *vehicleArmLeft, *vehicleArmRight;
        } shapes;

        struct {
            Vector2 weight,
                engineLeftArm, engineRightArm,
                frictionLeftArm, frictionRightArm,
                totalLeftArm, totalRightArm;
        } forces;

        struct {
            Float massInverted,
                powerArm,
                restitution,
                friction,
                momentOfInertiaInverted;

            Float physicsTimeDelta;
            Vector2 gravity;

            Deg armAngle, engineAngle;
            Float armRadius;
        } parameters;

        struct {
            Float currentPowerLeftArm,
                currentPowerRightArm,
                physicsTime,
                physicsTimeAccumulator,
                angularSpeed,
                torque;

            Vector2 linearVelocity,
                force;
        } state;

};

Forces2D::Forces2D(const Arguments& arguments): Platform::Application(arguments, nullptr), state() {
    /* Try to create MSAA context */
    auto conf = new Configuration();
    #ifndef CORRADE_TARGET_NACL
    conf->setTitle("Kotel::Prototype::Forces2D");
    #endif
    conf->setSampleCount(16);
    if(!tryCreateContext(conf)) {
        Warning() << "Cannot enable 16x MSAA, fallback to no-AA rendering";
        createContext(conf->setSampleCount(0));
    } else delete conf;

    Renderer::setClearColor(Color3<>(0.125f));
    Renderer::setFeature(Renderer::Feature::Blending, true);
    Renderer::setBlendFunction(Renderer::BlendFunction::SourceAlpha, Renderer::BlendFunction::OneMinusSourceAlpha);

    /* Camera setup */
    cameraObject.setParent(&scene);
    (camera = new SceneGraph::Camera2D<>(&cameraObject))
        ->setAspectRatioPolicy(SceneGraph::AspectRatioPolicy::Extend)
        ->setProjection(Vector2(3.0f));

    /* Debug draw setup */
    debugResourceManager.set("weight", (new DebugTools::ForceRendererOptions)
        ->setSize(0.00025f)->setColor(Color4<>::fromHSV(Deg(190.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("engines", (new DebugTools::ForceRendererOptions)
        ->setSize(0.00025f)->setColor(Color4<>::fromHSV(Deg(50.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("friction", (new DebugTools::ForceRendererOptions)
        ->setSize(0.00025f)->setColor(Color4<>::fromHSV(Deg(115.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("total", (new DebugTools::ForceRendererOptions)
        ->setSize(0.00025f)->setColor(Color4<>::fromHSV(Deg(245.0f), 0.75f, 0.9f, 0.75f)));
    debugResourceManager.set("collision", (new DebugTools::ShapeRendererOptions)
        ->setPointSize(0.1f)->setColor(Color4<>::fromHSV(Deg(25.0f), 0.75f, 0.9f, 0.75f)));
    debugResourceManager.set("vehicle", (new DebugTools::ShapeRendererOptions)
        ->setColor(Color3<>(0.5f)));
    debugResourceManager.set("parameters", (new DebugTools::ObjectRendererOptions)
        ->setSize(0.1f));

    /* Parameters */
    parameters.physicsTimeDelta = 1.0f/120.0f;
    parameters.gravity = Vector2::yAxis(-9.81);
    parameters.armAngle = Deg(110.0f);
    parameters.engineAngle = Deg(0.0f);
    parameters.armRadius = 1.0f;
    const Float massBody = 260.0f;
    const Float massArm = 80.0f;
    parameters.massInverted = 1.0f/(massBody + 2*massArm);
    parameters.powerArm = 500.0f;
    parameters.restitution = 0.6f;
    parameters.friction = 0.16f;

    /* Object initialization */
    tube = new Object2D(&scene);
    vehicle = new Object2D(&scene);
    (body = new Object2D(vehicle))
        ->translate({});
    (armLeft = new Object2D(vehicle))
        ->translate(Vector2::yAxis(-parameters.armRadius))
        ->rotate(-parameters.armAngle/2);
    (armRight = new Object2D(vehicle))
        ->translate(Vector2::yAxis(-parameters.armRadius))
        ->rotate(parameters.armAngle/2);
    (engineLeft = new Object2D(vehicle))
        ->rotate(-parameters.engineAngle)
        ->translate(Vector2::yAxis(-parameters.armRadius))
        ->rotate(-parameters.armAngle/2);
    (engineRight = new Object2D(vehicle))
        ->rotate(parameters.engineAngle)
        ->translate(Vector2::yAxis(-parameters.armRadius))
        ->rotate(parameters.armAngle/2);

    /* Compute center of mass and move it to center of the object */
    const Vector2 centerOfMass = (body->transformation().translation()*massBody +
        (engineLeft->transformation().translation() + engineRight->transformation().translation())*
        massArm)*parameters.massInverted;
    vehicle->translate(centerOfMass);
    body->translate(-centerOfMass);
    armLeft->translate(-centerOfMass);
    armRight->translate(-centerOfMass);
    engineLeft->translate(-centerOfMass);
    engineRight->translate(-centerOfMass);

    /* Weight force */
    forces.weight = parameters.gravity/parameters.massInverted;

    /* Moment of inertia */
    parameters.momentOfInertiaInverted = 1.0f/(body->transformation().translation().dot()*massBody +
        engineLeft->transformation().translation().dot()*massArm +
        engineRight->transformation().translation().dot()*massArm);

    /* Tube and vehicle collision shapes */
    shapes.tubeMin = new Physics::Sphere2D({}, 0.99f);
    shapes.tubeMax = new Physics::Sphere2D({}, 1.01f);
    shapes.vehicleBody = new Physics::Point2D(body->transformation().translation()+Vector2::yAxis(0.15f));
    shapes.vehicleArmLeft = new Physics::Point2D(engineLeft->transformation().translation());
    shapes.vehicleArmRight = new Physics::Point2D(engineRight->transformation().translation());
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(tube, &collisionShapes))
        ->setShape(std::ref(*shapes.tubeMin) || std::ref(*shapes.tubeMax)),
        "collision", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(vehicle, &collisionShapes))
        ->setShape(std::ref(*shapes.vehicleBody) || std::ref(*shapes.vehicleArmLeft) || std::ref(*shapes.vehicleArmRight)),
        "collision", &drawables);

    /* Vehicle visualization */
    const auto armA = Vector2::yAxis(parameters.armRadius-0.2f);
    const auto armB = Vector2::yAxis(0.03f);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(body, &visualizationShapes))
        ->setShape(Physics::Sphere2D({}, 0.2f)), "vehicle", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(armLeft, &visualizationShapes))
        ->setShape(Physics::LineSegment2D(armA, armB)), "vehicle", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(armRight, &visualizationShapes))
        ->setShape(Physics::LineSegment2D(armA, armB)), "vehicle", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(engineLeft, &visualizationShapes))
        ->setShape(Physics::Box2D(Matrix3::scaling({0.1f, 0.03f}))), "vehicle", &drawables);
    new DebugTools::ShapeRenderer2D((new Physics::ObjectShape2D(engineRight, &visualizationShapes))
        ->setShape(Physics::Box2D(Matrix3::scaling({0.1f, 0.03f}))), "vehicle", &drawables);

    /* Vehicle center-of-mass visualization */
    new DebugTools::ObjectRenderer2D(vehicle, "parameters", &drawables);

    /* Gravity force visualization */
    new DebugTools::ForceRenderer2D(vehicle, {}, &forces.weight, "weight", &drawables);

    /* Engine and friction forces visualization */
    new DebugTools::ForceRenderer2D(engineLeft, {}, &forces.engineLeftArm, "engines", &drawables);
    new DebugTools::ForceRenderer2D(engineRight, {}, &forces.engineRightArm, "engines", &drawables);
    new DebugTools::ForceRenderer2D(engineLeft, {}, &forces.frictionLeftArm, "friction", &drawables);
    new DebugTools::ForceRenderer2D(engineRight, {}, &forces.frictionRightArm, "friction", &drawables);

    /* Total forces visualization */
    new DebugTools::ForceRenderer2D(engineLeft, {}, &forces.totalLeftArm, "total", &drawables);
    new DebugTools::ForceRenderer2D(engineRight, {}, &forces.totalRightArm, "total", &drawables);

    /* Zero-time physics step */
    physicsStep(state.physicsTime, parameters.physicsTimeDelta);
    timeline.start();
}

void Forces2D::viewportEvent(const Vector2i& size) {
    defaultFramebuffer.setViewport({{}, size});

    camera->setViewport(size);
}

void Forces2D::drawEvent() {
    defaultFramebuffer.bind(DefaultFramebuffer::Target::Draw);
    defaultFramebuffer.clear(DefaultFramebuffer::Clear::Color);

    /* Do physics steps in elapsed time */
    state.physicsTimeAccumulator += timeline.previousFrameDuration();
    while(state.physicsTimeAccumulator >= parameters.physicsTimeDelta) {
        globalPhysicsStep(state.physicsTime, parameters.physicsTimeDelta);
        state.physicsTimeAccumulator -= parameters.physicsTimeDelta;
        state.physicsTime += parameters.physicsTimeDelta;
    }

    visualizationShapes.setClean();
    collisionShapes.setClean();
    camera->draw(drawables);

    swapBuffers();
    timeline.nextFrame();
    redraw();
}

void Forces2D::keyPressEvent(KeyEvent& event) {
    if(event.key() == KeyEvent::Key::Left) {
        state.currentPowerLeftArm = parameters.powerArm;
        state.currentPowerRightArm = parameters.powerArm;
    } else if(event.key() == KeyEvent::Key::Right) {
        state.currentPowerLeftArm = -parameters.powerArm;
        state.currentPowerRightArm = -parameters.powerArm;
    } else return;

    event.setAccepted();
    redraw();
}

void Forces2D::keyReleaseEvent(KeyEvent& event) {
    state.currentPowerLeftArm = state.currentPowerRightArm = {};

    event.setAccepted();
    redraw();
}

void Forces2D::globalPhysicsStep(const Float time, const Float delta) {
    /* Compute force and torque at original position */
    state.force = {};
    state.torque = {};
    physicsStep(time, delta);

    state.linearVelocity += 0.5f*state.force*parameters.massInverted*delta;
    state.angularSpeed += 0.5f*state.torque*parameters.momentOfInertiaInverted*delta;

    /* Check penetration */
    collisionShapes.setClean();
    for(Physics::Point2D* shape: {shapes.vehicleBody, shapes.vehicleArmLeft, shapes.vehicleArmRight}) {
        if(*shape % *shapes.tubeMax) continue;

        const Vector2 normal = -shape->transformedPosition();
        const Vector2 position = shape->transformedPosition() - vehicle->absoluteTransformation().translation();
        const Vector2 velocity = state.linearVelocity + state.angularSpeed*position.perpendicular();
        const Vector2 impulse = (-(1.0f + parameters.restitution)*Vector2::dot(velocity, normal)/
            (normal.dot()*parameters.massInverted + Math::pow<2>(Vector2::cross(position, normal))*parameters.momentOfInertiaInverted))*normal;

        applyImpulse(shape->transformedPosition(), impulse);
    }

    /* New position and rotation (around COM) */
    vehicle->translate(state.linearVelocity*delta)
        ->rotate(Rad(state.angularSpeed*delta), SceneGraph::TransformationType::Local)
        ->normalizeRotation();

    /* Compute force at new position */
    state.force = {};
    state.torque = {};
    physicsStep(time+delta, delta);

    /* New velocity */
    state.linearVelocity += 0.5f*state.force*parameters.massInverted*delta;
    state.angularSpeed += 0.5f*state.torque*parameters.momentOfInertiaInverted*delta;
}

void Forces2D::physicsStep(const Float, const Float) {
    /* Compute tangent and normal vectors */
    const Vector2 engineDirectionLeft(engineLeft->absoluteTransformation().rotation());
    const Vector2 engineDirectionRight(engineRight->absoluteTransformation().rotation());

    /* Reset forces */
    forces.totalLeftArm = forces.totalRightArm =
        forces.frictionLeftArm = forces.frictionRightArm = {};

    /* Add engine forces*/
    forces.engineLeftArm = engineDirectionLeft*state.currentPowerLeftArm;
    forces.engineRightArm = engineDirectionRight*state.currentPowerRightArm;
    forces.totalLeftArm += forces.engineLeftArm;
    forces.totalRightArm += forces.engineRightArm;

    /* Apply forces */
    applyForce(vehicle->absoluteTransformation().translation(), forces.weight);
    applyForce(engineLeft->absoluteTransformation().translation(), forces.totalLeftArm);
    applyForce(engineLeft->absoluteTransformation().translation(), forces.totalRightArm);
}

void Forces2D::applyForce(const Vector2& position, const Vector2& force) {
    state.force += force;
    state.torque += Vector2::cross(position - vehicle->absoluteTransformation().translation(), force);
}

void Forces2D::applyImpulse(const Vector2& position, const Vector2& impulse) {
    state.linearVelocity += impulse*parameters.massInverted;
    state.angularSpeed += Vector2::cross(position - vehicle->absoluteTransformation().translation(), impulse)*parameters.momentOfInertiaInverted;
}

}}

MAGNUM_APPLICATION_MAIN(Kotel::Prototype::Forces2D)
<|endoftext|>


<commit_before>/*
 * File:   ZooKeeperRouter.cpp
 * Author: Paolo D'Apice
 *
 * Created on February 17, 2012, 2:32 PM
 */

#include "net/sf1r/distributed/ZooKeeperRouter.hpp"
#include "net/sf1r/distributed/Sf1Topology.hpp"
#include "../PoolFactory.hpp"
#include "../RawClient.hpp"
#include "RoundRobinPolicy.hpp"
#include "Sf1Watcher.hpp"
#include "ZooKeeperNamespace.hpp"
#include "util/kv2string.h"
#include <boost/foreach.hpp>
#include <boost/regex.hpp>
#include <glog/logging.h>

NS_IZENELIB_SF1R_BEGIN

using iz::ZooKeeper;
using std::string;
using std::vector;

namespace {

const bool AUTO_RECONNECT = true;
typedef vector<string> strvector;

}

ZooKeeperRouter::ZooKeeperRouter(PoolFactory* poolFactory,
        const string& hosts, int timeout, const std::string& match_master_name,
        int set_seq, int total_set_num)
    : factory(poolFactory), match_master_name_(match_master_name),
    set_seq_(set_seq), total_set_num_(total_set_num)
{
    assert(set_seq_ <= total_set_num_);
    // 0. connect to ZooKeeper
    LOG(INFO) << "Connecting to ZooKeeper servers: " << hosts;
    client.reset(new iz::ZooKeeper(hosts, timeout, AUTO_RECONNECT));

    while (not client->isConnected()) {
        timeout -= 100;
        if (timeout < 0)
        {
            // XXX: this should be done in the ZooKeeper client
            throw iz::ZooKeeperException("Not connected to (servers): " + hosts);
        }
        usleep(100*1000);
    }

    // 1. register a watcher that will control the Sf1Driver instances
    LOG(INFO) << "Registering watcher ...";
    watcher.reset(new Sf1Watcher(*this));
    client->registerEventHandler(watcher.get());

    // 2. obtain the list of actually running SF1 servers
    LOG(INFO) << "Getting actual topology ...";
    topology.reset(new Sf1Topology);
    loadTopology();

    // 3. set the routing policy
    LOG(INFO) << "Routing policy ...";
    policy.reset(new RoundRobinPolicy(*topology));

    LOG(INFO) << "ZooKeeperRouter ready";
}

void ZooKeeperRouter::reconnect()
{
    LOG(INFO) << "reconnecting....";
    if (!client)
        return;
    client->disconnect();
    client->connect();
    int timeout = 10*1000;
    while(not client->isConnected())
    {
        timeout -= 100;
        if (timeout < 0)
        {
            // XXX: this should be done in the ZooKeeper client
            throw iz::ZooKeeperException("Re-connect failed " );
        }
        usleep(100*1000);
    }
    loadTopology();
}

ZooKeeperRouter::~ZooKeeperRouter() {
    BOOST_FOREACH(PoolContainer::value_type& i, pools) {
        delete i.second;
    }

    LOG(INFO) << "ZooKeeperRouter closed";
}

void
ZooKeeperRouter::loadTopology() {
    strvector clusters;
    client->getZNodeChildren(ROOT_NODE, clusters, ZooKeeper::WATCH);

    LOG(INFO) << "Scanning root node ...";
    BOOST_FOREACH(const string& s, clusters) {
        if (boost::regex_match(s, SF1R_ROOT_REGEX)) { // this is a SF1 node
            LOG(INFO) << "node: " << s;
            addSearchTopology(s + TOPOLOGY);
        }
        else
            LOG(INFO) << "not sf1r node : " << s;
    }
    LOG(INFO) << "found (" << topology->count() << ") active nodes";
}

void
ZooKeeperRouter::addSearchTopology(const string& searchTopology) {
    if (client->isZNodeExists(searchTopology, ZooKeeper::WATCH)) {
        LOG(INFO) << "found search topology: " << searchTopology;

        DLOG(INFO) << "Searching for replicas ...";
        strvector replicas;
        client->getZNodeChildren(searchTopology, replicas, ZooKeeper::WATCH);

        if (replicas.empty()) {
            DLOG(INFO) << "no replica found";
            return;
        }

        BOOST_FOREACH(const string& replica, replicas) {
            LOG(INFO) << "replica: " << replica;

            DLOG(INFO) << "Searching for nodes ...";
            strvector nodes;
            client->getZNodeChildren(replica, nodes, ZooKeeper::WATCH);

            if (nodes.empty()) {
                DLOG(INFO) << "no node found";
                continue;
            }

            BOOST_FOREACH(const string& path, nodes) {
                addSf1Node(path);
            }
        }
    }
}

bool
ZooKeeperRouter::addSf1Node(const string& path) {
    boost::lock_guard<boost::mutex> lock(mutex);

    LOG(INFO) << "adding SF1 node: [" << path << "]";

    if (topology->isPresent(path)) {
        LOG(INFO) << "node already exists, skipping";
        return true;
    }

    string data;
    client->getZNodeData(path, data, ZooKeeper::WATCH);
    LOG(INFO) << "node data: [" << data << "]";

    izenelib::util::kv2string parser;
    parser.loadKvString(data);

    if (not parser.hasKey(MASTERPORT_KEY)) {
        LOG(INFO) << "Not a master node, skipping";
        return false;
    }

    if (!match_master_name_.empty())
    {
        std::string mastername = parser.getStrValue(MASTER_NAME_KEY);
        if(mastername != match_master_name_) { //match special master SF1 node
            LOG(INFO) << "ignore not matched master : " << mastername;
            return false;
        }
    }

    // replicaid and set_seq_ both start from 1.
    int32_t replicaid = parser.getUInt32Value(REPLICAID_KEY);
    if (((replicaid - 1) % total_set_num_) == (set_seq_ - 1))
    {
        LOG(INFO) << "current set_seq_ : " << set_seq_ << " connectting to node in replica : " << replicaid;
    }
    else
    {
        LOG(INFO) << "current set_seq_ : " << set_seq_ << " ignore node in replica : " << replicaid;
        client->isZNodeExists(path, ZooKeeper::NOT_WATCH);
        return false;
    }
    // check for search service
    std::string service_names;
    service_names = parser.getStrValue(SERVICE_NAMES_KEY);
    if (service_names.find(SearchService) == std::string::npos)
    {
        LOG(INFO) << "current node has no search service : " << path;
        return false;
    }
    // add node into topology
    topology->addNode(path, data);

#ifdef ENABLE_ZK_TEST
    if (factory == NULL) { // Should be NULL only in tests
        LOG(WARNING) << "factory is NULL, is this a test?";
        return false;
    }
#endif

    // add pool for the node
    const Sf1Node& node = topology->getNodeAt(path);
    DLOG(INFO) << "Getting connection pool for node: " << node.getPath();
    ConnectionPool* pool = factory->newConnectionPool(node);
    pools.insert(PoolContainer::value_type(node.getPath(), pool));
    return true;
}

void
ZooKeeperRouter::updateNodeData(const string& path) {
    boost::unique_lock<boost::mutex> lock(mutex);

    if (not topology->isPresent(path)) {
        LOG(INFO) << "Node not in topology, skipping";
        return;
    }

    LOG(INFO) << "updating SF1 node: [" << path << "]";

    string data;
    client->getZNodeData(path, data, iz::ZooKeeper::WATCH);
    LOG(INFO) << "node data: [" << data << "]";

    if (data.empty())
    {
        LOG(INFO) << "update node data is empty, remove it." << path;
        // remove node from topology
        topology->removeNode(path);

#ifdef ENABLE_ZK_TEST
        if (factory == NULL) { // Should be NULL only in tests
            LOG(WARNING) << "factory is NULL, is this a test?";
            return;
        }
#endif
        // remove its pool
        const PoolContainer::iterator& it = pools.find(path);
        CHECK(pools.end() != it) << "pool not found";
        ConnectionPool* pool = it->second;
        CHECK_NOTNULL(pool);

        while (pool->isBusy()) {
            DLOG(INFO) << "pool is currently in use, waiting";
            condition.wait(lock);
        }
        DLOG(INFO) << "removing pool";
        CHECK_EQ(1, pools.erase(path));
        delete pool;
    }
    else
        topology->updateNode(path, data);
}

void
ZooKeeperRouter::removeSf1Node(const string& path) {
    boost::unique_lock<boost::mutex> lock(mutex);

    if (not topology->isPresent(path)) {
        LOG(INFO) << "Node not in topology, skipping";
        return;
    }

    LOG(INFO) << "removing SF1 node: [" << path << "]";

    // remove node from topology
    topology->removeNode(path);

#ifdef ENABLE_ZK_TEST
    if (factory == NULL) { // Should be NULL only in tests
        LOG(WARNING) << "factory is NULL, is this a test?";
        return;
    }
#endif

    // remove its pool
    const PoolContainer::iterator& it = pools.find(path);
    CHECK(pools.end() != it) << "pool not found";
    ConnectionPool* pool = it->second;
    CHECK_NOTNULL(pool);

    while (pool->isBusy()) {
        DLOG(INFO) << "pool is currently in use, waiting";
        condition.wait(lock);
    }
    DLOG(INFO) << "removing pool";
    CHECK_EQ(1, pools.erase(path));
    delete pool;
}

void
ZooKeeperRouter::watchChildren(const string& path) {
    if (not client->isZNodeExists(path, ZooKeeper::WATCH)) {
        LOG(INFO) << "node [" << path << "] does not exist";
        //boost::smatch what;
        //CHECK(boost::regex_search(path, what, SF1R_ROOT_REGEX));
        //CHECK(not what.empty());
        //
        //DLOG(INFO) << "recurse to [" << what[0] << "] ...";
        //watchChildren(what[0]);
        return;
    }

    if (!boost::regex_search(path, TOPOLOGY_REGEX) && !boost::regex_match(path, SF1R_ROOT_REGEX))
    {
        client->isZNodeExists(path, ZooKeeper::NOT_WATCH);
        LOG(INFO) << "Not Watching children of: " << path;
        return;
    }

    strvector children;
    client->getZNodeChildren(path, children, ZooKeeper::WATCH);
    DLOG(INFO) << "Watching children of: " << path;
    BOOST_FOREACH(const string& s, children) {
        DLOG(INFO) << "child: " << s;
        if (boost::regex_match(s, SF1R_NODE_REGEX)) {
            DLOG(INFO) << "Adding node: " << s;
            addSf1Node(s);
        } else if (boost::regex_search(s, SF1R_ROOT_REGEX)) {
            DLOG(INFO) << "recurse";
            watchChildren(s);
        }
    }
}

NodeListRange
ZooKeeperRouter::getSf1Nodes() const {
    DLOG(INFO) << "Getting all nodes ...";

    if (topology->count() == 0) {
        LOG(WARNING) << "Empty topology";
    }
    return topology->getNodes();
}

NodeCollectionsRange
ZooKeeperRouter::getSf1Nodes(const string& collection) const {
    DLOG(INFO) << "Getting all nodes for collection: " << collection << " ...";

    if (topology->count() == 0) {
        LOG(WARNING) << "Empty topology";
    }

    if (topology->count(collection) == 0) {
        LOG(WARNING) << "No routes for collection: " << collection;
    }
    return topology->getNodesFor(collection);
}

const Sf1Node&
ZooKeeperRouter::resolve(const string collection) const {
    if (topology->count() == 0) {
        LOG(WARNING) << "Empty topology, throwing RoutingError";
        throw RoutingError("Empty topology");
    }

    if (collection.empty()) {
        DLOG(INFO) << "No collection specified, resolving to all nodes ...";

        // choose a node according to the routing policy
        return policy->getNode();
    } else {
        DLOG(INFO) << "Resolving nodes for collection: " << collection << " ...";

        if (topology->count(collection) == 0) {
            LOG(WARNING) << "No routes for collection: " << collection
                         << ", throwing RoutingError";
            throw RoutingError("No routes for " + collection);
        }

        // choose a node according to the routing policy
        return policy->getNodeFor(collection);
    }
}

RawClient&
ZooKeeperRouter::getConnection(const string& collection) {
    boost::lock_guard<boost::mutex> lock(mutex);

    const Sf1Node& node = resolve(collection);
    LOG(INFO) << "Resolved to node: " << node.getPath();

    // get a connection from the node
    PoolContainer::const_iterator cit = pools.find(node.getPath());
    if (cit == pools.end())
    {
        LOG(INFO) << "no pools for the connection, add new node : " << node.getPath();
        //if(!addSf1Node(node.getPath()))
        {
            LOG(ERROR) << "get connection failed for : " << node.getPath();
            throw RoutingError("no ConnectionPool for " + collection);
        }
        //cit = pools.find(node.getPath());
        //if (cit == pools.end())
        //{
        //    LOG(ERROR) << "get connection failed for : " << node.getPath();
        //    throw RoutingError("no ConnectionPool for " + collection);
        //}
    }
    ConnectionPool* pool = cit->second;
    CHECK(pool) << "NULL pool";
    return pool->acquire();
}

void
ZooKeeperRouter::releaseConnection(const RawClient& connection) {
    boost::lock_guard<boost::mutex> lock(mutex);

    DLOG(INFO) << "releasing connection";
    ConnectionPool* pool = pools.find(connection.getPath())->second;
    pool->release(connection);

    if (not pool->isBusy()) {
        DLOG(INFO) << "notifying for condition";
        condition.notify_one();
    }
}

inline void
addConnection(vector<RawClient*>& list, const Sf1Node& node, PoolContainer& pools) {
    ConnectionPool* pool = pools.find(node.getPath())->second;
    CHECK(pool) << "NULL pool";
    list.push_back(&pool->acquire());
}

vector<RawClient*>
ZooKeeperRouter::getConnections(const string& collection) {
    if (topology->count() == 0) {
        LOG(WARNING) << "Empty topology, throwing RoutingError";
        throw RoutingError("Empty topology");
    }

    if (collection.empty()) {
        vector<RawClient*> list;
        NodeListRange nodes = topology->getNodes();
        BOOST_FOREACH(const Sf1Node& node, nodes) {
            addConnection(list, node, pools);
        }
        return list;
    } else {
        if (topology->count(collection) == 0) {
            LOG(WARNING) << "No routes for collection: " << collection
                         << ", throwing RoutingError";
            throw RoutingError("No routes for " + collection);
        }

        vector<RawClient*> list;
        NodeCollectionsRange range = topology->getNodesFor(collection);
        NodeCollectionsList nodes(range.first, range.second);
        BOOST_FOREACH(const NodeCollectionsList::value_type& value, nodes) {
            const Sf1Node& node = value.second;
            addConnection(list, node, pools);
        }
        return list;
    }
}

void
ZooKeeperRouter::releaseConnections(const vector<RawClient*> connections) {
    BOOST_FOREACH(RawClient* connection, connections) {
        releaseConnection(*connection);
    }
}

NS_IZENELIB_SF1R_END
<commit_msg>handle connection exception in nginx sf1r lib<commit_after>/*
 * File:   ZooKeeperRouter.cpp
 * Author: Paolo D'Apice
 *
 * Created on February 17, 2012, 2:32 PM
 */

#include "net/sf1r/distributed/ZooKeeperRouter.hpp"
#include "net/sf1r/distributed/Sf1Topology.hpp"
#include "../PoolFactory.hpp"
#include "../RawClient.hpp"
#include "RoundRobinPolicy.hpp"
#include "Sf1Watcher.hpp"
#include "ZooKeeperNamespace.hpp"
#include "util/kv2string.h"
#include <boost/foreach.hpp>
#include <boost/regex.hpp>
#include <glog/logging.h>

NS_IZENELIB_SF1R_BEGIN

using iz::ZooKeeper;
using std::string;
using std::vector;

namespace {

const bool AUTO_RECONNECT = true;
typedef vector<string> strvector;

}

ZooKeeperRouter::ZooKeeperRouter(PoolFactory* poolFactory,
        const string& hosts, int timeout, const std::string& match_master_name,
        int set_seq, int total_set_num)
    : factory(poolFactory), match_master_name_(match_master_name),
    set_seq_(set_seq), total_set_num_(total_set_num)
{
    assert(set_seq_ <= total_set_num_);
    // 0. connect to ZooKeeper
    LOG(INFO) << "Connecting to ZooKeeper servers: " << hosts;
    client.reset(new iz::ZooKeeper(hosts, timeout, AUTO_RECONNECT));

    while (not client->isConnected()) {
        timeout -= 100;
        if (timeout < 0)
        {
            // XXX: this should be done in the ZooKeeper client
            throw iz::ZooKeeperException("Not connected to (servers): " + hosts);
        }
        usleep(100*1000);
    }

    // 1. register a watcher that will control the Sf1Driver instances
    LOG(INFO) << "Registering watcher ...";
    watcher.reset(new Sf1Watcher(*this));
    client->registerEventHandler(watcher.get());

    // 2. obtain the list of actually running SF1 servers
    LOG(INFO) << "Getting actual topology ...";
    topology.reset(new Sf1Topology);
    loadTopology();

    // 3. set the routing policy
    LOG(INFO) << "Routing policy ...";
    policy.reset(new RoundRobinPolicy(*topology));

    LOG(INFO) << "ZooKeeperRouter ready";
}

void ZooKeeperRouter::reconnect()
{
    LOG(INFO) << "reconnecting....";
    if (!client)
        return;
    client->disconnect();
    client->connect();
    int timeout = 10*1000;
    while(not client->isConnected())
    {
        timeout -= 100;
        if (timeout < 0)
        {
            // XXX: this should be done in the ZooKeeper client
            throw iz::ZooKeeperException("Re-connect failed " );
        }
        usleep(100*1000);
    }
    loadTopology();
}

ZooKeeperRouter::~ZooKeeperRouter() {
    BOOST_FOREACH(PoolContainer::value_type& i, pools) {
        delete i.second;
    }

    LOG(INFO) << "ZooKeeperRouter closed";
}

void
ZooKeeperRouter::loadTopology() {
    strvector clusters;
    client->getZNodeChildren(ROOT_NODE, clusters, ZooKeeper::WATCH);

    LOG(INFO) << "Scanning root node ...";
    BOOST_FOREACH(const string& s, clusters) {
        if (boost::regex_match(s, SF1R_ROOT_REGEX)) { // this is a SF1 node
            LOG(INFO) << "node: " << s;
            addSearchTopology(s + TOPOLOGY);
        }
        else
            LOG(INFO) << "not sf1r node : " << s;
    }
    LOG(INFO) << "found (" << topology->count() << ") active nodes";
}

void
ZooKeeperRouter::addSearchTopology(const string& searchTopology) {
    if (client->isZNodeExists(searchTopology, ZooKeeper::WATCH)) {
        LOG(INFO) << "found search topology: " << searchTopology;

        DLOG(INFO) << "Searching for replicas ...";
        strvector replicas;
        client->getZNodeChildren(searchTopology, replicas, ZooKeeper::WATCH);

        if (replicas.empty()) {
            DLOG(INFO) << "no replica found";
            return;
        }

        BOOST_FOREACH(const string& replica, replicas) {
            LOG(INFO) << "replica: " << replica;

            DLOG(INFO) << "Searching for nodes ...";
            strvector nodes;
            client->getZNodeChildren(replica, nodes, ZooKeeper::WATCH);

            if (nodes.empty()) {
                DLOG(INFO) << "no node found";
                continue;
            }

            BOOST_FOREACH(const string& path, nodes) {
                addSf1Node(path);
            }
        }
    }
}

bool
ZooKeeperRouter::addSf1Node(const string& path) {
    boost::lock_guard<boost::mutex> lock(mutex);

    LOG(INFO) << "adding SF1 node: [" << path << "]";

    if (topology->isPresent(path)) {
        LOG(INFO) << "node already exists, skipping";
        return true;
    }

    string data;
    client->getZNodeData(path, data, ZooKeeper::WATCH);
    LOG(INFO) << "node data: [" << data << "]";

    izenelib::util::kv2string parser;
    parser.loadKvString(data);

    if (not parser.hasKey(MASTERPORT_KEY)) {
        LOG(INFO) << "Not a master node, skipping";
        return false;
    }

    if (!match_master_name_.empty())
    {
        std::string mastername = parser.getStrValue(MASTER_NAME_KEY);
        if(mastername != match_master_name_) { //match special master SF1 node
            LOG(INFO) << "ignore not matched master : " << mastername;
            return false;
        }
    }

    // replicaid and set_seq_ both start from 1.
    int32_t replicaid = parser.getUInt32Value(REPLICAID_KEY);
    if (((replicaid - 1) % total_set_num_) == (set_seq_ - 1))
    {
        LOG(INFO) << "current set_seq_ : " << set_seq_ << " connectting to node in replica : " << replicaid;
    }
    else
    {
        LOG(INFO) << "current set_seq_ : " << set_seq_ << " ignore node in replica : " << replicaid;
        client->isZNodeExists(path, ZooKeeper::NOT_WATCH);
        return false;
    }
    // check for search service
    std::string service_names;
    service_names = parser.getStrValue(SERVICE_NAMES_KEY);
    if (service_names.find(SearchService) == std::string::npos)
    {
        LOG(INFO) << "current node has no search service : " << path;
        return false;
    }
    // add node into topology
    topology->addNode(path, data);

#ifdef ENABLE_ZK_TEST
    if (factory == NULL) { // Should be NULL only in tests
        LOG(WARNING) << "factory is NULL, is this a test?";
        return false;
    }
#endif

    // add pool for the node
    const Sf1Node& node = topology->getNodeAt(path);
    DLOG(INFO) << "Getting connection pool for node: " << node.getPath();

    try
    {
        ConnectionPool* pool = factory->newConnectionPool(node);
        pools.insert(PoolContainer::value_type(node.getPath(), pool));
    }
    catch(...)
    {
        LOG(ERROR) << "connect failed.";
        // remove node from topology
        topology->removeNode(path);
        return false;
    }

    return true;
}

void
ZooKeeperRouter::updateNodeData(const string& path) {
    boost::unique_lock<boost::mutex> lock(mutex);

    if (not topology->isPresent(path)) {
        LOG(INFO) << "Node not in topology, skipping";
        return;
    }

    LOG(INFO) << "updating SF1 node: [" << path << "]";

    string data;
    client->getZNodeData(path, data, iz::ZooKeeper::WATCH);
    LOG(INFO) << "node data: [" << data << "]";

    if (data.empty())
    {
        LOG(INFO) << "update node data is empty, remove it." << path;
        // remove node from topology
        topology->removeNode(path);

#ifdef ENABLE_ZK_TEST
        if (factory == NULL) { // Should be NULL only in tests
            LOG(WARNING) << "factory is NULL, is this a test?";
            return;
        }
#endif
        // remove its pool
        const PoolContainer::iterator& it = pools.find(path);
        CHECK(pools.end() != it) << "pool not found";
        ConnectionPool* pool = it->second;
        CHECK_NOTNULL(pool);

        while (pool->isBusy()) {
            DLOG(INFO) << "pool is currently in use, waiting";
            condition.wait(lock);
        }
        DLOG(INFO) << "removing pool";
        CHECK_EQ(1, pools.erase(path));
        delete pool;
    }
    else
        topology->updateNode(path, data);
}

void
ZooKeeperRouter::removeSf1Node(const string& path) {
    boost::unique_lock<boost::mutex> lock(mutex);

    if (not topology->isPresent(path)) {
        LOG(INFO) << "Node not in topology, skipping";
        return;
    }

    LOG(INFO) << "removing SF1 node: [" << path << "]";

    // remove node from topology
    topology->removeNode(path);

#ifdef ENABLE_ZK_TEST
    if (factory == NULL) { // Should be NULL only in tests
        LOG(WARNING) << "factory is NULL, is this a test?";
        return;
    }
#endif

    // remove its pool
    const PoolContainer::iterator& it = pools.find(path);
    CHECK(pools.end() != it) << "pool not found";
    ConnectionPool* pool = it->second;
    CHECK_NOTNULL(pool);

    while (pool->isBusy()) {
        DLOG(INFO) << "pool is currently in use, waiting";
        condition.wait(lock);
    }
    DLOG(INFO) << "removing pool";
    CHECK_EQ(1, pools.erase(path));
    delete pool;
}

void
ZooKeeperRouter::watchChildren(const string& path) {
    if (not client->isZNodeExists(path, ZooKeeper::WATCH)) {
        LOG(INFO) << "node [" << path << "] does not exist";
        //boost::smatch what;
        //CHECK(boost::regex_search(path, what, SF1R_ROOT_REGEX));
        //CHECK(not what.empty());
        //
        //DLOG(INFO) << "recurse to [" << what[0] << "] ...";
        //watchChildren(what[0]);
        return;
    }

    if (!boost::regex_search(path, TOPOLOGY_REGEX) && !boost::regex_match(path, SF1R_ROOT_REGEX))
    {
        client->isZNodeExists(path, ZooKeeper::NOT_WATCH);
        LOG(INFO) << "Not Watching children of: " << path;
        return;
    }

    strvector children;
    client->getZNodeChildren(path, children, ZooKeeper::WATCH);
    DLOG(INFO) << "Watching children of: " << path;
    BOOST_FOREACH(const string& s, children) {
        DLOG(INFO) << "child: " << s;
        if (boost::regex_match(s, SF1R_NODE_REGEX)) {
            DLOG(INFO) << "Adding node: " << s;
            addSf1Node(s);
        } else if (boost::regex_search(s, SF1R_ROOT_REGEX)) {
            DLOG(INFO) << "recurse";
            watchChildren(s);
        }
    }
}

NodeListRange
ZooKeeperRouter::getSf1Nodes() const {
    DLOG(INFO) << "Getting all nodes ...";

    if (topology->count() == 0) {
        LOG(WARNING) << "Empty topology";
    }
    return topology->getNodes();
}

NodeCollectionsRange
ZooKeeperRouter::getSf1Nodes(const string& collection) const {
    DLOG(INFO) << "Getting all nodes for collection: " << collection << " ...";

    if (topology->count() == 0) {
        LOG(WARNING) << "Empty topology";
    }

    if (topology->count(collection) == 0) {
        LOG(WARNING) << "No routes for collection: " << collection;
    }
    return topology->getNodesFor(collection);
}

const Sf1Node&
ZooKeeperRouter::resolve(const string collection) const {
    if (topology->count() == 0) {
        LOG(WARNING) << "Empty topology, throwing RoutingError";
        throw RoutingError("Empty topology");
    }

    if (collection.empty()) {
        DLOG(INFO) << "No collection specified, resolving to all nodes ...";

        // choose a node according to the routing policy
        return policy->getNode();
    } else {
        DLOG(INFO) << "Resolving nodes for collection: " << collection << " ...";

        if (topology->count(collection) == 0) {
            LOG(WARNING) << "No routes for collection: " << collection
                         << ", throwing RoutingError";
            throw RoutingError("No routes for " + collection);
        }

        // choose a node according to the routing policy
        return policy->getNodeFor(collection);
    }
}

RawClient&
ZooKeeperRouter::getConnection(const string& collection) {
    boost::lock_guard<boost::mutex> lock(mutex);

    const Sf1Node& node = resolve(collection);
    LOG(INFO) << "Resolved to node: " << node.getPath();

    // get a connection from the node
    PoolContainer::const_iterator cit = pools.find(node.getPath());
    if (cit == pools.end())
    {
        LOG(INFO) << "no pools for the connection, add new node : " << node.getPath();
        //if(!addSf1Node(node.getPath()))
        {
            LOG(ERROR) << "get connection failed for : " << node.getPath();
            throw RoutingError("no ConnectionPool for " + collection);
        }
        //cit = pools.find(node.getPath());
        //if (cit == pools.end())
        //{
        //    LOG(ERROR) << "get connection failed for : " << node.getPath();
        //    throw RoutingError("no ConnectionPool for " + collection);
        //}
    }
    ConnectionPool* pool = cit->second;
    CHECK(pool) << "NULL pool";
    return pool->acquire();
}

void
ZooKeeperRouter::releaseConnection(const RawClient& connection) {
    boost::lock_guard<boost::mutex> lock(mutex);

    DLOG(INFO) << "releasing connection";
    ConnectionPool* pool = pools.find(connection.getPath())->second;
    pool->release(connection);

    if (not pool->isBusy()) {
        DLOG(INFO) << "notifying for condition";
        condition.notify_one();
    }
}

inline void
addConnection(vector<RawClient*>& list, const Sf1Node& node, PoolContainer& pools) {
    ConnectionPool* pool = pools.find(node.getPath())->second;
    CHECK(pool) << "NULL pool";
    list.push_back(&pool->acquire());
}

vector<RawClient*>
ZooKeeperRouter::getConnections(const string& collection) {
    if (topology->count() == 0) {
        LOG(WARNING) << "Empty topology, throwing RoutingError";
        throw RoutingError("Empty topology");
    }

    if (collection.empty()) {
        vector<RawClient*> list;
        NodeListRange nodes = topology->getNodes();
        BOOST_FOREACH(const Sf1Node& node, nodes) {
            addConnection(list, node, pools);
        }
        return list;
    } else {
        if (topology->count(collection) == 0) {
            LOG(WARNING) << "No routes for collection: " << collection
                         << ", throwing RoutingError";
            throw RoutingError("No routes for " + collection);
        }

        vector<RawClient*> list;
        NodeCollectionsRange range = topology->getNodesFor(collection);
        NodeCollectionsList nodes(range.first, range.second);
        BOOST_FOREACH(const NodeCollectionsList::value_type& value, nodes) {
            const Sf1Node& node = value.second;
            addConnection(list, node, pools);
        }
        return list;
    }
}

void
ZooKeeperRouter::releaseConnections(const vector<RawClient*> connections) {
    BOOST_FOREACH(RawClient* connection, connections) {
        releaseConnection(*connection);
    }
}

NS_IZENELIB_SF1R_END
<|endoftext|>


<commit_before>#include <Math/Functions.h>
#include <Renderer.h>
#include <DefaultFramebuffer.h>
#include <DebugTools/ForceRenderer.h>
#include <DebugTools/ResourceManager.h>
#include <DebugTools/ShapeRenderer.h>
#ifndef CORRADE_TARGET_NACL
#include <Platform/Sdl2Application.h>
#else
#include <Platform/NaClApplication.h>
#endif
#include <SceneGraph/Camera2D.h>
#include <SceneGraph/DualComplexTransformation.h>
#include <SceneGraph/Object.h>
#include <SceneGraph/Scene.h>
#include <Physics/ObjectShape.h>
#include <Physics/ObjectShapeGroup.h>
#include <Physics/Box.h>
#include <Physics/Point.h>
#include <Physics/ShapeGroup.h>
#include <Physics/Sphere.h>

#ifdef MAGNUM_BUILD_STATIC
#include <Shaders/magnumShadersResourceImport.hpp>
#endif

#include "Kotel.h"

namespace Kotel { namespace Prototype {

class Forces2D: public Platform::Application {
    public:
        explicit Forces2D(const Arguments& arguments);

        void viewportEvent(const Vector2i& size) override;
        void drawEvent() override;
        void keyPressEvent(KeyEvent& event) override;
        void keyReleaseEvent(KeyEvent& event) override;

    private:
        DebugTools::ResourceManager debugResourceManager;

        Scene2D scene;
        Object2D cameraObject;
        SceneGraph::Camera2D<> *camera;
        SceneGraph::DrawableGroup<2> drawables;
        Physics::ObjectShapeGroup2D shapes;

        Object2D *tube, *vehicle;

        struct {
            Vector2 weightBody,
                weightLeftArm, weightRightArm,
                engineLeftArm, engineRightArm,
                frictionLeftArm, frictionRightArm,
                totalNormalLeftArm, totalNormalRightArm,
                totalTangentLeftArm, totalTangentRightArm;
        } forces;

        struct {
            Float massBody,
                massArm,
                powerArm,
                friction;

            Vector2 gravity,
                centerOfMass;

            Deg armAngle;
            DualComplex baseLeftArmTransformation,
                baseRightArmTransformation;
        } parameters;

        struct {
            Float currentPowerLeftArm,
                currentPowerRightArm;
        } engine;

};

Forces2D::Forces2D(const Arguments& arguments): Platform::Application(arguments, (new Configuration())
    #ifndef CORRADE_TARGET_NACL
    ->setTitle("Kotel::Prototype::Forces2D")
    ->setSampleCount(16)
    #endif
), engine{{}, {}} {
    Renderer::setClearColor(Color3<>(0.125f));
    Renderer::setFeature(Renderer::Feature::Blending, true);
    Renderer::setBlendFunction(Renderer::BlendFunction::SourceAlpha, Renderer::BlendFunction::OneMinusSourceAlpha);

    /* Parameters */
    parameters.gravity = Vector2::yAxis(-9.81);
    parameters.armAngle = Deg(110.0f);
    parameters.massBody = 260.0f;
    parameters.massArm = 80.0f;
    parameters.powerArm = 1000.0f;
    parameters.friction = 0.16f;

    /* Camera setup */
    cameraObject.setParent(&scene);
    (camera = new SceneGraph::Camera2D<>(&cameraObject))
        ->setAspectRatioPolicy(SceneGraph::AspectRatioPolicy::Extend)
        ->setProjection(Vector2(3.0f));

    /* Debug draw setup */
    debugResourceManager.set("gravity", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(190.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("engines", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(50.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("friction", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(115.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("tangent", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(245.0f), 0.75f, 0.9f, 0.75f)));
    debugResourceManager.set("normal", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(5.0f), 0.75f, 0.9f, 0.75f)));
    debugResourceManager.set("tube", (new DebugTools::ShapeRendererOptions())
        ->setColor(Color3<>(0.2f)));
    debugResourceManager.set("vehicle", (new DebugTools::ShapeRendererOptions())
        ->setColor(Color3<>(0.5f)));
    debugResourceManager.set("parameters", (new DebugTools::ShapeRendererOptions())
        ->setPointSize(0.1f)->setColor(Color3<>::fromHSV(Deg(25.0f), 0.75f, 0.9f)));

    /* Tube */
    tube = new Object2D(&scene);
    auto tubeShape = new Physics::ObjectShape2D(tube, &shapes);
    tubeShape->setShape(
        Physics::Sphere2D({}, 1.05f) ||
        Physics::Sphere2D({}, 1.15f)
    );
    new DebugTools::ShapeRenderer2D(tubeShape, "tube", &drawables);

    /* Vehicle parameters */
    parameters.baseLeftArmTransformation = DualComplex::rotation(-parameters.armAngle/2)*DualComplex::translation(Vector2::yAxis(-1.0f));
    parameters.baseRightArmTransformation = DualComplex::rotation(parameters.armAngle/2)*DualComplex::translation(Vector2::yAxis(-1.0f));
    parameters.centerOfMass = ((parameters.baseLeftArmTransformation.translation() +
        parameters.baseRightArmTransformation.translation())*
        parameters.massArm + Vector2()*parameters.massBody)/(parameters.massArm*2 + parameters.massBody);

    /* Vehicle */
    vehicle = new Object2D(&scene);
    Matrix3 baseArmTransformation = Matrix3::translation(Vector2::yAxis(-0.585f))*Matrix3::scaling({0.02f, 0.385f});
    auto vehicleShape = new Physics::ObjectShape2D(vehicle, &shapes);
    vehicleShape->setShape(
        Physics::Sphere2D({}, .2f) ||
        Physics::Box2D(Matrix3::rotation(-parameters.armAngle/2)*baseArmTransformation) ||
        Physics::Box2D(Matrix3::rotation(parameters.armAngle/2)*baseArmTransformation) ||
        Physics::Box2D(parameters.baseLeftArmTransformation.toMatrix()*Matrix3::scaling({0.1f, 0.03f})) ||
        Physics::Box2D(parameters.baseRightArmTransformation.toMatrix()*Matrix3::scaling({0.1f, 0.03f}))
    );
    new DebugTools::ShapeRenderer2D(vehicleShape, "vehicle", &drawables);

    /* Vehicle center-of-mass */
    auto vehicleCenterOfMassShape = new Physics::ObjectShape2D(vehicle, &shapes);
    vehicleCenterOfMassShape->setShape(Physics::Point2D(parameters.centerOfMass));
    new DebugTools::ShapeRenderer2D(vehicleCenterOfMassShape, "parameters", &drawables);

    /* Gravity forces */
    forces.weightBody = parameters.gravity*parameters.massBody;
    forces.weightLeftArm = parameters.gravity*parameters.massArm;
    forces.weightRightArm = parameters.gravity*parameters.massArm;
    new DebugTools::ForceRenderer2D(vehicle, {},
        &forces.weightBody, "gravity", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.weightLeftArm, "gravity", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.weightRightArm, "gravity", &drawables);

    /* Engine and friction forces */
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.engineLeftArm, "engines", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.engineRightArm, "engines", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.frictionLeftArm, "friction", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.frictionRightArm, "friction", &drawables);

    /* Tangent and normal forces */
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.totalTangentLeftArm, "tangent", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.totalTangentRightArm, "tangent", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.totalNormalLeftArm, "normal", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.totalNormalRightArm, "normal", &drawables);
}

void Forces2D::viewportEvent(const Vector2i& size) {
    defaultFramebuffer.setViewport({{}, size});

    camera->setViewport(size);
}

void Forces2D::drawEvent() {
    defaultFramebuffer.bind(DefaultFramebuffer::Target::Draw);
    defaultFramebuffer.clear(DefaultFramebuffer::Clear::Color);

    /* Compute tangent and normal vectors */
    const Vector2 tangentLeftArm = (vehicle->transformation().rotation()*
        parameters.baseLeftArmTransformation.rotation()).transformVector(Vector2::xAxis());
    const Vector2 tangentRightArm = (vehicle->transformation().rotation()*
        parameters.baseRightArmTransformation.rotation()).transformVector(Vector2::xAxis());
    const Vector2 normalLeftArm = (vehicle->transformation().rotation()*
        parameters.baseLeftArmTransformation.rotation()).transformVector(-Vector2::yAxis());
    const Vector2 normalRightArm = (vehicle->transformation().rotation()*
        parameters.baseRightArmTransformation.rotation()).transformVector(-Vector2::yAxis());

    /* Reset */
    forces.totalNormalLeftArm = forces.totalNormalRightArm =
        forces.totalTangentLeftArm = forces.totalTangentRightArm = {};

    /* Propagate body weight to arms */
    forces.totalNormalLeftArm += forces.weightBody.projectedOntoNormalized(normalLeftArm);
    forces.totalNormalRightArm += forces.weightBody.projectedOntoNormalized(normalRightArm);

    /* Decompose arm weight to normal and tangent */
    forces.totalNormalLeftArm += forces.weightLeftArm.projectedOntoNormalized(normalLeftArm);
    forces.totalNormalRightArm += forces.weightRightArm.projectedOntoNormalized(normalRightArm);
    forces.totalTangentLeftArm += forces.weightLeftArm.projectedOntoNormalized(tangentLeftArm);
    forces.totalTangentRightArm += forces.weightRightArm.projectedOntoNormalized(tangentRightArm);

    /* Add engine forces to tangent */
    forces.engineLeftArm = tangentLeftArm*engine.currentPowerLeftArm;
    forces.engineRightArm = tangentRightArm*engine.currentPowerRightArm;
    forces.totalTangentLeftArm += forces.engineLeftArm;
    forces.totalTangentRightArm += forces.engineRightArm;

    /* Add friction forces to tangent */
    forces.frictionLeftArm = -Math::sign(forces.totalTangentLeftArm)*Math::min(
        Math::abs(tangentLeftArm*forces.totalNormalLeftArm.length()*parameters.friction),
        Math::abs(forces.totalTangentLeftArm));
    forces.frictionRightArm = -Math::sign(forces.totalTangentRightArm)*Math::min(
        Math::abs(tangentRightArm*forces.totalNormalRightArm.length()*parameters.friction),
        Math::abs(forces.totalTangentRightArm));
    forces.totalTangentLeftArm += forces.frictionLeftArm;
    forces.totalTangentRightArm += forces.frictionRightArm;

    shapes.setClean();
    camera->draw(drawables);

    swapBuffers();
}

void Forces2D::keyPressEvent(KeyEvent& event) {
    if(event.key() == KeyEvent::Key::Left) {
        engine.currentPowerLeftArm = parameters.powerArm;
        engine.currentPowerRightArm = parameters.powerArm;
    } else if(event.key() == KeyEvent::Key::Right) {
        engine.currentPowerLeftArm = -parameters.powerArm;
        engine.currentPowerRightArm = -parameters.powerArm;
    } else if(event.key() == KeyEvent::Key::Up) {
        vehicle->rotate(Deg(5.0f))->normalizeRotation();
    } else if(event.key() == KeyEvent::Key::Down) {
        vehicle->rotate(Deg(-5.0f))->normalizeRotation();
    } else return;

    event.setAccepted();
    redraw();
}

void Forces2D::keyReleaseEvent(KeyEvent& event) {
    engine.currentPowerLeftArm = engine.currentPowerRightArm = {};

    event.setAccepted();
    redraw();
}

}}

MAGNUM_APPLICATION_MAIN(Kotel::Prototype::Forces2D)
<commit_msg>prototype: try to enable 16x MSAA and fallback to no-AA.<commit_after>#include <Math/Functions.h>
#include <Renderer.h>
#include <DefaultFramebuffer.h>
#include <DebugTools/ForceRenderer.h>
#include <DebugTools/ResourceManager.h>
#include <DebugTools/ShapeRenderer.h>
#ifndef CORRADE_TARGET_NACL
#include <Platform/Sdl2Application.h>
#else
#include <Platform/NaClApplication.h>
#endif
#include <SceneGraph/Camera2D.h>
#include <SceneGraph/DualComplexTransformation.h>
#include <SceneGraph/Object.h>
#include <SceneGraph/Scene.h>
#include <Physics/ObjectShape.h>
#include <Physics/ObjectShapeGroup.h>
#include <Physics/Box.h>
#include <Physics/Point.h>
#include <Physics/ShapeGroup.h>
#include <Physics/Sphere.h>

#ifdef MAGNUM_BUILD_STATIC
#include <Shaders/magnumShadersResourceImport.hpp>
#endif

#include "Kotel.h"

namespace Kotel { namespace Prototype {

class Forces2D: public Platform::Application {
    public:
        explicit Forces2D(const Arguments& arguments);

        void viewportEvent(const Vector2i& size) override;
        void drawEvent() override;
        void keyPressEvent(KeyEvent& event) override;
        void keyReleaseEvent(KeyEvent& event) override;

    private:
        DebugTools::ResourceManager debugResourceManager;

        Scene2D scene;
        Object2D cameraObject;
        SceneGraph::Camera2D<> *camera;
        SceneGraph::DrawableGroup<2> drawables;
        Physics::ObjectShapeGroup2D shapes;

        Object2D *tube, *vehicle;

        struct {
            Vector2 weightBody,
                weightLeftArm, weightRightArm,
                engineLeftArm, engineRightArm,
                frictionLeftArm, frictionRightArm,
                totalNormalLeftArm, totalNormalRightArm,
                totalTangentLeftArm, totalTangentRightArm;
        } forces;

        struct {
            Float massBody,
                massArm,
                powerArm,
                friction;

            Vector2 gravity,
                centerOfMass;

            Deg armAngle;
            DualComplex baseLeftArmTransformation,
                baseRightArmTransformation;
        } parameters;

        struct {
            Float currentPowerLeftArm,
                currentPowerRightArm;
        } engine;

};

Forces2D::Forces2D(const Arguments& arguments): Platform::Application(arguments, nullptr), engine{{}, {}} {
    /* Try to create MSAA context */
    auto conf = new Configuration();
    #ifndef CORRADE_TARGET_NACL
    conf->setTitle("Kotel::Prototype::Forces2D");
    #endif
    conf->setSampleCount(16);
    if(!tryCreateContext(conf)) {
        Warning() << "Cannot enable 16x MSAA, fallback to no-AA rendering";
        createContext(conf->setSampleCount(0));
    } else delete conf;

    Renderer::setClearColor(Color3<>(0.125f));
    Renderer::setFeature(Renderer::Feature::Blending, true);
    Renderer::setBlendFunction(Renderer::BlendFunction::SourceAlpha, Renderer::BlendFunction::OneMinusSourceAlpha);

    /* Parameters */
    parameters.gravity = Vector2::yAxis(-9.81);
    parameters.armAngle = Deg(110.0f);
    parameters.massBody = 260.0f;
    parameters.massArm = 80.0f;
    parameters.powerArm = 1000.0f;
    parameters.friction = 0.16f;

    /* Camera setup */
    cameraObject.setParent(&scene);
    (camera = new SceneGraph::Camera2D<>(&cameraObject))
        ->setAspectRatioPolicy(SceneGraph::AspectRatioPolicy::Extend)
        ->setProjection(Vector2(3.0f));

    /* Debug draw setup */
    debugResourceManager.set("gravity", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(190.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("engines", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(50.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("friction", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(115.0f), 0.75f, 0.9f, 0.5f)));
    debugResourceManager.set("tangent", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(245.0f), 0.75f, 0.9f, 0.75f)));
    debugResourceManager.set("normal", (new DebugTools::ForceRendererOptions())
        ->setSize(0.0005f)->setColor(Color4<>::fromHSV(Deg(5.0f), 0.75f, 0.9f, 0.75f)));
    debugResourceManager.set("tube", (new DebugTools::ShapeRendererOptions())
        ->setColor(Color3<>(0.2f)));
    debugResourceManager.set("vehicle", (new DebugTools::ShapeRendererOptions())
        ->setColor(Color3<>(0.5f)));
    debugResourceManager.set("parameters", (new DebugTools::ShapeRendererOptions())
        ->setPointSize(0.1f)->setColor(Color3<>::fromHSV(Deg(25.0f), 0.75f, 0.9f)));

    /* Tube */
    tube = new Object2D(&scene);
    auto tubeShape = new Physics::ObjectShape2D(tube, &shapes);
    tubeShape->setShape(
        Physics::Sphere2D({}, 1.05f) ||
        Physics::Sphere2D({}, 1.15f)
    );
    new DebugTools::ShapeRenderer2D(tubeShape, "tube", &drawables);

    /* Vehicle parameters */
    parameters.baseLeftArmTransformation = DualComplex::rotation(-parameters.armAngle/2)*DualComplex::translation(Vector2::yAxis(-1.0f));
    parameters.baseRightArmTransformation = DualComplex::rotation(parameters.armAngle/2)*DualComplex::translation(Vector2::yAxis(-1.0f));
    parameters.centerOfMass = ((parameters.baseLeftArmTransformation.translation() +
        parameters.baseRightArmTransformation.translation())*
        parameters.massArm + Vector2()*parameters.massBody)/(parameters.massArm*2 + parameters.massBody);

    /* Vehicle */
    vehicle = new Object2D(&scene);
    Matrix3 baseArmTransformation = Matrix3::translation(Vector2::yAxis(-0.585f))*Matrix3::scaling({0.02f, 0.385f});
    auto vehicleShape = new Physics::ObjectShape2D(vehicle, &shapes);
    vehicleShape->setShape(
        Physics::Sphere2D({}, .2f) ||
        Physics::Box2D(Matrix3::rotation(-parameters.armAngle/2)*baseArmTransformation) ||
        Physics::Box2D(Matrix3::rotation(parameters.armAngle/2)*baseArmTransformation) ||
        Physics::Box2D(parameters.baseLeftArmTransformation.toMatrix()*Matrix3::scaling({0.1f, 0.03f})) ||
        Physics::Box2D(parameters.baseRightArmTransformation.toMatrix()*Matrix3::scaling({0.1f, 0.03f}))
    );
    new DebugTools::ShapeRenderer2D(vehicleShape, "vehicle", &drawables);

    /* Vehicle center-of-mass */
    auto vehicleCenterOfMassShape = new Physics::ObjectShape2D(vehicle, &shapes);
    vehicleCenterOfMassShape->setShape(Physics::Point2D(parameters.centerOfMass));
    new DebugTools::ShapeRenderer2D(vehicleCenterOfMassShape, "parameters", &drawables);

    /* Gravity forces */
    forces.weightBody = parameters.gravity*parameters.massBody;
    forces.weightLeftArm = parameters.gravity*parameters.massArm;
    forces.weightRightArm = parameters.gravity*parameters.massArm;
    new DebugTools::ForceRenderer2D(vehicle, {},
        &forces.weightBody, "gravity", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.weightLeftArm, "gravity", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.weightRightArm, "gravity", &drawables);

    /* Engine and friction forces */
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.engineLeftArm, "engines", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.engineRightArm, "engines", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.frictionLeftArm, "friction", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.frictionRightArm, "friction", &drawables);

    /* Tangent and normal forces */
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.totalTangentLeftArm, "tangent", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.totalTangentRightArm, "tangent", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseLeftArmTransformation.translation(),
        &forces.totalNormalLeftArm, "normal", &drawables);
    new DebugTools::ForceRenderer2D(vehicle, parameters.baseRightArmTransformation.translation(),
        &forces.totalNormalRightArm, "normal", &drawables);
}

void Forces2D::viewportEvent(const Vector2i& size) {
    defaultFramebuffer.setViewport({{}, size});

    camera->setViewport(size);
}

void Forces2D::drawEvent() {
    defaultFramebuffer.bind(DefaultFramebuffer::Target::Draw);
    defaultFramebuffer.clear(DefaultFramebuffer::Clear::Color);

    /* Compute tangent and normal vectors */
    const Vector2 tangentLeftArm = (vehicle->transformation().rotation()*
        parameters.baseLeftArmTransformation.rotation()).transformVector(Vector2::xAxis());
    const Vector2 tangentRightArm = (vehicle->transformation().rotation()*
        parameters.baseRightArmTransformation.rotation()).transformVector(Vector2::xAxis());
    const Vector2 normalLeftArm = (vehicle->transformation().rotation()*
        parameters.baseLeftArmTransformation.rotation()).transformVector(-Vector2::yAxis());
    const Vector2 normalRightArm = (vehicle->transformation().rotation()*
        parameters.baseRightArmTransformation.rotation()).transformVector(-Vector2::yAxis());

    /* Reset */
    forces.totalNormalLeftArm = forces.totalNormalRightArm =
        forces.totalTangentLeftArm = forces.totalTangentRightArm = {};

    /* Propagate body weight to arms */
    forces.totalNormalLeftArm += forces.weightBody.projectedOntoNormalized(normalLeftArm);
    forces.totalNormalRightArm += forces.weightBody.projectedOntoNormalized(normalRightArm);

    /* Decompose arm weight to normal and tangent */
    forces.totalNormalLeftArm += forces.weightLeftArm.projectedOntoNormalized(normalLeftArm);
    forces.totalNormalRightArm += forces.weightRightArm.projectedOntoNormalized(normalRightArm);
    forces.totalTangentLeftArm += forces.weightLeftArm.projectedOntoNormalized(tangentLeftArm);
    forces.totalTangentRightArm += forces.weightRightArm.projectedOntoNormalized(tangentRightArm);

    /* Add engine forces to tangent */
    forces.engineLeftArm = tangentLeftArm*engine.currentPowerLeftArm;
    forces.engineRightArm = tangentRightArm*engine.currentPowerRightArm;
    forces.totalTangentLeftArm += forces.engineLeftArm;
    forces.totalTangentRightArm += forces.engineRightArm;

    /* Add friction forces to tangent */
    forces.frictionLeftArm = -Math::sign(forces.totalTangentLeftArm)*Math::min(
        Math::abs(tangentLeftArm*forces.totalNormalLeftArm.length()*parameters.friction),
        Math::abs(forces.totalTangentLeftArm));
    forces.frictionRightArm = -Math::sign(forces.totalTangentRightArm)*Math::min(
        Math::abs(tangentRightArm*forces.totalNormalRightArm.length()*parameters.friction),
        Math::abs(forces.totalTangentRightArm));
    forces.totalTangentLeftArm += forces.frictionLeftArm;
    forces.totalTangentRightArm += forces.frictionRightArm;

    shapes.setClean();
    camera->draw(drawables);

    swapBuffers();
}

void Forces2D::keyPressEvent(KeyEvent& event) {
    if(event.key() == KeyEvent::Key::Left) {
        engine.currentPowerLeftArm = parameters.powerArm;
        engine.currentPowerRightArm = parameters.powerArm;
    } else if(event.key() == KeyEvent::Key::Right) {
        engine.currentPowerLeftArm = -parameters.powerArm;
        engine.currentPowerRightArm = -parameters.powerArm;
    } else if(event.key() == KeyEvent::Key::Up) {
        vehicle->rotate(Deg(5.0f))->normalizeRotation();
    } else if(event.key() == KeyEvent::Key::Down) {
        vehicle->rotate(Deg(-5.0f))->normalizeRotation();
    } else return;

    event.setAccepted();
    redraw();
}

void Forces2D::keyReleaseEvent(KeyEvent& event) {
    engine.currentPowerLeftArm = engine.currentPowerRightArm = {};

    event.setAccepted();
    redraw();
}

}}

MAGNUM_APPLICATION_MAIN(Kotel::Prototype::Forces2D)
<|endoftext|>


<commit_before>
#include<iostream>
#include<ostream>
#include<fstream>

#define SHOULD(description, cond) {{\
    if(!(cond)){\
        std::clog << "FAILED SHOULD: " << description << "\n";\
        return 1;\
    }\
}}

#define SHOULD_THROW(desc, exc, code) {{\
    try{\
        code;\
        std::cerr << "FAILED SHOULD_THROW: " << desc << "\n";\
        return 1;\
    } catch (exc &e) {\
    } catch (...){\
        std::clog << "FAILED SHOULD_THROW: " << desc << ": wrong exception" \
               << "\n";\
        return 1;\
    }\
}}
<commit_msg>Exit instead of return when tests fail<commit_after>#include<iostream>
#include<ostream>
#include<fstream>
#include<cstdlib>

#define SHOULD(description, cond) {{\
    if(!(cond)){\
        std::clog << "FAILED SHOULD: " << description << "\n";\
        std::exit(EXIT_FAILURE);\
    }\
}}

#define SHOULD_THROW(desc, exc, code) {{\
    try{\
        code;\
        std::cerr << "FAILED SHOULD_THROW: " << desc << "\n";\
        std::exit(EXIT_FAILURE);\
    } catch (exc &e) {\
    } catch (...){\
        std::clog << "FAILED SHOULD_THROW: " << desc << ": wrong exception" \
               << "\n";\
        std::exit(EXIT_FAILURE);\
    }\
}}
<|endoftext|>