<commit_before>
/*
 * Copyright 2011 Google Inc.
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */
#include "SkBenchmark.h"
#include "SkBitmap.h"
#include "SkCanvas.h"
#include "SkColorPriv.h"
#include "SkPaint.h"
#include "SkShader.h"
#include "SkString.h"

static const char* gConfigName[] = {
    "ERROR", "a1", "a8", "index8", "565", "4444", "8888"
};

static void drawIntoBitmap(const SkBitmap& bm) {
    const int w = bm.width();
    const int h = bm.height();

    SkCanvas canvas(bm);
    SkPaint p;
    p.setAntiAlias(true);
    p.setColor(SK_ColorRED);
    canvas.drawCircle(SkIntToScalar(w)/2, SkIntToScalar(h)/2,
                      SkIntToScalar(SkMin32(w, h))*3/8, p);

    SkRect r;
    r.set(0, 0, SkIntToScalar(w), SkIntToScalar(h));
    p.setStyle(SkPaint::kStroke_Style);
    p.setStrokeWidth(SkIntToScalar(4));
    p.setColor(SK_ColorBLUE);
    canvas.drawRect(r, p);
}

static int conv6ToByte(int x) {
    return x * 0xFF / 5;
}

static int convByteTo6(int x) {
    return x * 5 / 255;
}

static uint8_t compute666Index(SkPMColor c) {
    int r = SkGetPackedR32(c);
    int g = SkGetPackedG32(c);
    int b = SkGetPackedB32(c);

    return convByteTo6(r) * 36 + convByteTo6(g) * 6 + convByteTo6(b);
}

static void convertToIndex666(const SkBitmap& src, SkBitmap* dst) {
    SkColorTable* ctable = new SkColorTable(216);
    SkPMColor* colors = ctable->lockColors();
    // rrr ggg bbb
    for (int r = 0; r < 6; r++) {
        int rr = conv6ToByte(r);
        for (int g = 0; g < 6; g++) {
            int gg = conv6ToByte(g);
            for (int b = 0; b < 6; b++) {
                int bb = conv6ToByte(b);
                *colors++ = SkPreMultiplyARGB(0xFF, rr, gg, bb);
            }
        }
    }
    ctable->unlockColors(true);
    dst->setConfig(SkBitmap::kIndex8_Config, src.width(), src.height());
    dst->allocPixels(ctable);
    ctable->unref();

    SkAutoLockPixels alps(src);
    SkAutoLockPixels alpd(*dst);

    for (int y = 0; y < src.height(); y++) {
        const SkPMColor* srcP = src.getAddr32(0, y);
        uint8_t* dstP = dst->getAddr8(0, y);
        for (int x = src.width() - 1; x >= 0; --x) {
            *dstP++ = compute666Index(*srcP++);
        }
    }
}

class RepeatTileBench : public SkBenchmark {
    SkPaint     fPaint;
    SkString    fName;
    enum { N = SkBENCHLOOP(20) };
public:
    RepeatTileBench(void* param, SkBitmap::Config c) : INHERITED(param) {
        const int w = 50;
        const int h = 50;
        SkBitmap bm;

        if (SkBitmap::kIndex8_Config == c) {
            bm.setConfig(SkBitmap::kARGB_8888_Config, w, h);
        } else {
            bm.setConfig(c, w, h);
        }
        bm.allocPixels();
        bm.eraseColor(0);

        drawIntoBitmap(bm);

        if (SkBitmap::kIndex8_Config == c) {
            SkBitmap tmp;
            convertToIndex666(bm, &tmp);
            bm = tmp;
        }

        SkShader* s = SkShader::CreateBitmapShader(bm,
                                                   SkShader::kRepeat_TileMode,
                                                   SkShader::kRepeat_TileMode);
        fPaint.setShader(s)->unref();
        fName.printf("repeatTile_%s", gConfigName[bm.config()]);
    }

protected:
    virtual const char* onGetName() {
        return fName.c_str();
    }

    virtual void onDraw(SkCanvas* canvas) {
        SkPaint paint(fPaint);
        this->setupPaint(&paint);

        for (int i = 0; i < N; i++) {
            canvas->drawPaint(paint);
        }
    }

private:
    typedef SkBenchmark INHERITED;
};

static SkBenchmark* Fact0(void* p) { return new RepeatTileBench(p, SkBitmap::kARGB_8888_Config); }
static SkBenchmark* Fact1(void* p) { return new RepeatTileBench(p, SkBitmap::kRGB_565_Config); }
static SkBenchmark* Fact2(void* p) { return new RepeatTileBench(p, SkBitmap::kARGB_4444_Config); }
static SkBenchmark* Fact3(void* p) { return new RepeatTileBench(p, SkBitmap::kIndex8_Config); }

static BenchRegistry gReg0(Fact0);
static BenchRegistry gReg1(Fact1);
static BenchRegistry gReg2(Fact2);
static BenchRegistry gReg3(Fact3);
<commit_msg>add opaque/alpha variants<commit_after>
/*
 * Copyright 2011 Google Inc.
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */
#include "SkBenchmark.h"
#include "SkBitmap.h"
#include "SkCanvas.h"
#include "SkColorPriv.h"
#include "SkPaint.h"
#include "SkShader.h"
#include "SkString.h"

static const char* gConfigName[] = {
    "ERROR", "a1", "a8", "index8", "565", "4444", "8888"
};

static void drawIntoBitmap(const SkBitmap& bm) {
    const int w = bm.width();
    const int h = bm.height();

    SkCanvas canvas(bm);
    SkPaint p;
    p.setAntiAlias(true);
    p.setColor(SK_ColorRED);
    canvas.drawCircle(SkIntToScalar(w)/2, SkIntToScalar(h)/2,
                      SkIntToScalar(SkMin32(w, h))*3/8, p);

    SkRect r;
    r.set(0, 0, SkIntToScalar(w), SkIntToScalar(h));
    p.setStyle(SkPaint::kStroke_Style);
    p.setStrokeWidth(SkIntToScalar(4));
    p.setColor(SK_ColorBLUE);
    canvas.drawRect(r, p);
}

static int conv6ToByte(int x) {
    return x * 0xFF / 5;
}

static int convByteTo6(int x) {
    return x * 5 / 255;
}

static uint8_t compute666Index(SkPMColor c) {
    int r = SkGetPackedR32(c);
    int g = SkGetPackedG32(c);
    int b = SkGetPackedB32(c);

    return convByteTo6(r) * 36 + convByteTo6(g) * 6 + convByteTo6(b);
}

static void convertToIndex666(const SkBitmap& src, SkBitmap* dst) {
    SkColorTable* ctable = new SkColorTable(216);
    SkPMColor* colors = ctable->lockColors();
    // rrr ggg bbb
    for (int r = 0; r < 6; r++) {
        int rr = conv6ToByte(r);
        for (int g = 0; g < 6; g++) {
            int gg = conv6ToByte(g);
            for (int b = 0; b < 6; b++) {
                int bb = conv6ToByte(b);
                *colors++ = SkPreMultiplyARGB(0xFF, rr, gg, bb);
            }
        }
    }
    ctable->unlockColors(true);
    dst->setConfig(SkBitmap::kIndex8_Config, src.width(), src.height());
    dst->allocPixels(ctable);
    ctable->unref();

    SkAutoLockPixels alps(src);
    SkAutoLockPixels alpd(*dst);

    for (int y = 0; y < src.height(); y++) {
        const SkPMColor* srcP = src.getAddr32(0, y);
        uint8_t* dstP = dst->getAddr8(0, y);
        for (int x = src.width() - 1; x >= 0; --x) {
            *dstP++ = compute666Index(*srcP++);
        }
    }
}

class RepeatTileBench : public SkBenchmark {
    SkPaint     fPaint;
    SkString    fName;
    enum { N = SkBENCHLOOP(20) };
public:
    RepeatTileBench(void* param, SkBitmap::Config c, bool isOpaque = false) : INHERITED(param) {
        const int w = 50;
        const int h = 50;
        SkBitmap bm;

        if (SkBitmap::kIndex8_Config == c) {
            bm.setConfig(SkBitmap::kARGB_8888_Config, w, h);
        } else {
            bm.setConfig(c, w, h);
        }
        bm.allocPixels();
        bm.eraseColor(isOpaque ? SK_ColorWHITE : 0);
        bm.setIsOpaque(isOpaque);

        drawIntoBitmap(bm);

        if (SkBitmap::kIndex8_Config == c) {
            SkBitmap tmp;
            convertToIndex666(bm, &tmp);
            bm = tmp;
        }

        SkShader* s = SkShader::CreateBitmapShader(bm,
                                                   SkShader::kRepeat_TileMode,
                                                   SkShader::kRepeat_TileMode);
        fPaint.setShader(s)->unref();
        fName.printf("repeatTile_%s_%c", gConfigName[bm.config()], isOpaque ? 'X' : 'A');
    }

protected:
    virtual const char* onGetName() {
        return fName.c_str();
    }

    virtual void onDraw(SkCanvas* canvas) {
        SkPaint paint(fPaint);
        this->setupPaint(&paint);

        for (int i = 0; i < N; i++) {
            canvas->drawPaint(paint);
        }
    }

private:
    typedef SkBenchmark INHERITED;
};

DEF_BENCH(return new RepeatTileBench(p, SkBitmap::kARGB_8888_Config, true))
DEF_BENCH(return new RepeatTileBench(p, SkBitmap::kARGB_8888_Config, false))
DEF_BENCH(return new RepeatTileBench(p, SkBitmap::kRGB_565_Config))
DEF_BENCH(return new RepeatTileBench(p, SkBitmap::kARGB_4444_Config))
DEF_BENCH(return new RepeatTileBench(p, SkBitmap::kIndex8_Config))

<|endoftext|>


<commit_before>//
//  CPlayer.cpp
//  SwingGame
//
//  Created by Tim Brier on 11/10/2014.
//  Copyright (c) 2014 tbrier. All rights reserved.
//

// =============================================================================
// Include Files
// -----------------------------------------------------------------------------
#include "SFMLIntegration/SFMLIntegration.hpp"
#include "CPlayer.hpp"
#include "SystemUtilities.hpp"
#include "CSwingGame.hpp"
#include "CLevel.hpp"
#include "CLine.hpp"
#include "CollisionHandler.hpp"
#include "CRigidSwing.hpp"
#include "CFlexibleSwing.hpp"
#include "CSpringSwing.hpp"

// =============================================================================
// Static members
// -----------------------------------------------------------------------------
CVector2f CPlayer::smJumpVelocity = CVector2f(0.0f, -250.0f);
CTime CPlayer::smJumpCooldown = CTime::Seconds(1.5f);

// =============================================================================
// SUVAT Helper Methods
// -----------------------------------------------------------------------------
CVector2f GetVectorMoved(CVector2f u, CTime t, CVector2f a)
{
    // s = ut + 0.5at^2
    float ts = t.asSeconds();
    return (u * ts) + (0.5f * a * ts * ts);
}

CVector2f GetFinalVelocity(CVector2f u, CVector2f a, CTime t)
{
    // v = u + at
    float ts = t.asSeconds();
    return u + (a * ts);
}

// =============================================================================
// CPlayer constructor/destrctor
// -----------------------------------------------------------------------------
CPlayer::CPlayer(CLevel *theParentLevel) :  mParentLevel(theParentLevel)
{
    // Create the shape
    mSmallestRadius = 10.0f;

    std::list<CVector2f> thePoints;
    thePoints.push_back(CVector2f(-mSmallestRadius, -mSmallestRadius));
    thePoints.push_back(CVector2f(mSmallestRadius,  -mSmallestRadius));
    thePoints.push_back(CVector2f(mSmallestRadius,  mSmallestRadius));
    thePoints.push_back(CVector2f(-mSmallestRadius, mSmallestRadius));

    SetShape(CConvexShape(thePoints));
    GetShape()->setFillColor(CColour(255, 0, 255));

    float mass = 1.0f; // Read from file?
    SetInverseMass(1.0f / mass);
}

CPlayer::~CPlayer()
{

}

// =============================================================================
// CPlayer::Update
// -----------------------------------------------------------------------------
void CPlayer::Update(CTime elapsedTime)
{
    HandleInput();

    // Now simulate the physics until we've simulated the entire elapsedTime
    while (elapsedTime > CTime::Zero)
    {
        // Find the acceleration applied by any physics items
        CVector2f acceleration = HandlePhysics();

        // Now try to move based on our velocity
        CTime timeSimulated = MoveUntilCollision(elapsedTime, acceleration);
        elapsedTime -= timeSimulated;
    }
}

// =============================================================================
// CPlayer::ShouldUpdateForState
// -----------------------------------------------------------------------------
bool CPlayer::ShouldUpdateForState(EGameState theState)
{
    // Only update if we're not paused
    if ((theState & kGameStatePaused) != 0)
    {
        return false;
    }

    return CUpdateable::ShouldUpdateForState(theState);
}

// =============================================================================
// CPlayer::Draw
// -----------------------------------------------------------------------------
void CPlayer::Draw(CWindow *theWindow)
{
    // Colour the player by the colour of the swing we will fire
    GetShape()->setFillColor(mSwings[mSwingToFire]->GetColour());
    theWindow->DrawShape(*GetShape());
}

// =============================================================================
// CPlayer::Init
// -----------------------------------------------------------------------------
void CPlayer::Init()
{
    // Register any update/renderables
    CSwingGame::RegisterUpdateable(this);
    CSwingGame::RegisterRenderable(this);

    mSwings[kSwingTypeRigid] = new CRigidSwing(this, mParentLevel);
    mSwings[kSwingTypeFlexible] = new CFlexibleSwing(this, mParentLevel);
    mSwings[kSwingTypeSpring] = new CSpringSwing(this, mParentLevel);
}

// =============================================================================
// CPlayer::Cleanup
// -----------------------------------------------------------------------------
void CPlayer::Cleanup()
{
    SAFE_DELETE(mSwings[kSwingTypeRigid]);
    SAFE_DELETE(mSwings[kSwingTypeFlexible]);
    SAFE_DELETE(mSwings[kSwingTypeSpring]);

    // Unregister update/renderables
    CSwingGame::UnregisterUpdateable(this);
    CSwingGame::UnregisterRenderable(this);
}

// =============================================================================
// CPlayer::StartLevel
// -----------------------------------------------------------------------------
void CPlayer::StartLevel(SStartPosition theStartPos)
{
    GetShape()->setPosition(theStartPos.mPosition);
    SetVelocity(theStartPos.mVelocity);

    for (int i = 0; i < kSwingTypeMax; i++)
    {
        mSwings[i]->Detach();
    }

    mSwingToFire = theStartPos.mSwingType;
    mCurrentSwing = mSwings[mSwingToFire];

    mCurrentSwing->AttemptToAttach(theStartPos.mSwingTarget);

    // Start the jump clock at the cooldown time so we can jump straight away
    mJumpClock.Restart(smJumpCooldown);
}

// =============================================================================
// CPlayer::HandleInput
// -----------------------------------------------------------------------------
void CPlayer::HandleInput()
{
#if SG_DEBUG    // In debug move the player to the mouse pos if r shift is down
    if (CKeyboard::isKeyPressed(CKeyboard::RShift))
    {
        GetShape()->setPosition(SystemUtilities::GetMousePosition());
        HandleCollisions();

        // Reset velocity
        SetVelocity(CVector2f(0.0f, 0.0f));
    }
#endif

    // Connect the swing on left mouse click
    CVector2f mousePos;
    if (SystemUtilities::WasButtonPressedThisCycle(CMouse::Left, &mousePos))
    {
        // Detach the current swing before we try to fire the next
        mCurrentSwing->Detach();

        // Update the current swing type and try to attach it
        mCurrentSwing = mSwings[mSwingToFire];
        mCurrentSwing->AttemptToAttach(mousePos);
    }

    // Disconnect on right click
    if (SystemUtilities::WasButtonPressedThisCycle(CMouse::Right, &mousePos))
    {
        // We specifically requested this detach so respond to it
        mCurrentSwing->Detach(true);
    }

    // Jump on space
    if (SystemUtilities::WasKeyPressedThisCycle(CKeyboard::Space))
    {
        // Only jump if we're attached to a swing, and detach it when we do
        if (mCurrentSwing->CanJumpFrom()
            && mJumpClock.GetElapsedTime() >= smJumpCooldown)
        {
            mCurrentSwing->Detach();
            SetVelocity(GetVelocity() + smJumpVelocity);
            mJumpClock.Restart();
        }
    }

    // Change swing type
    if (SystemUtilities::WasKeyPressedThisCycle(CKeyboard::Num1))
    {
        mSwingToFire = kSwingTypeRigid;
    }
    if (SystemUtilities::WasKeyPressedThisCycle(CKeyboard::Num2))
    {
        mSwingToFire = kSwingTypeFlexible;
    }
    if (SystemUtilities::WasKeyPressedThisCycle(CKeyboard::Num3))
    {
        mSwingToFire = kSwingTypeSpring;
    }
}

// =============================================================================
// CPlayer::HandlePhysics
// Return the resulting acceleration of the player
// -----------------------------------------------------------------------------
CVector2f CPlayer::HandlePhysics()
{
    CVector2f acceleration = mParentLevel->GetGravityAcceleration();

    acceleration = mCurrentSwing->AttenuateGravity(acceleration);

    return acceleration;
}

// =============================================================================
// CPlayer::HandleCollisions
// Return true if we resolved a collision
// -----------------------------------------------------------------------------
bool CPlayer::HandleCollisions(bool shouldResolve /* = false */)
{
    bool didCollide = false;

    // Check for collisions against all level obstacles
    std::list<CPhysicsBody *> theObstacles = mParentLevel->GetObstacles();

    CVector2f correctionVector;
    FOR_EACH_IN_LIST(CPhysicsBody *, theObstacles)
    {
        if (CollisionHandler::AreColliding(*GetShape(),
                                           *((*it)->GetShape()),
                                           &correctionVector))
        {
            didCollide = true;

            if (shouldResolve)
            {
                // Resolve the collision
                CollisionHandler::Resolve(this, (*it), correctionVector);
            }
        }
    }

    return didCollide;
}

// =============================================================================
// CPlayer::MoveUntilCollision
// Move for the given amount of time with the given acceleration or until
// we collide
// Return the time we actually simulated movement for
// -----------------------------------------------------------------------------
CTime CPlayer::MoveUntilCollision(CTime elapsedTime, CVector2f acceleration)
{
    // Find the total offset we've been requested to move
    CVector2f totalOffset;
    totalOffset = GetVectorMoved(GetVelocity(), elapsedTime, acceleration);
    float totalDistance = totalOffset.GetMagnitude();

    // If we don't need to move bail out
    if (totalDistance == 0)
    {
        return elapsedTime;
    }

    // Find how many timesteps we should use to make sure we don't move by
    // more than the shapes smallest "radius"
    int numTimesteps = ceil(totalDistance / mSmallestRadius);

    // Find how big this timesteps should be
    CTime timeStep = elapsedTime / (numTimesteps * 1.0f);

    // Now find how far we should move for each timestep
    CVector2f smallOffset = totalOffset / (numTimesteps * 1.0f);

    // And simulate these timesteps until we've done them all or we've collided
    bool haveCollided = false;
    CTime timeSimulatedSoFar = CTime::Zero;
    for (int i = 0; i < numTimesteps && !haveCollided; i++)
    {
        // Update velocity to what it will be at the end of this timestep
        SetVelocity(GetFinalVelocity(GetVelocity(), acceleration, timeStep));

        // Now move the shape and handle any collisions
        GetShape()->move(smallOffset);
        haveCollided = HandleCollisions();

        timeSimulatedSoFar += timeStep;
    }

    return timeSimulatedSoFar;
}

// =============================================================================
// CPlayer::MoveFixedDistanceUntilCollision
// -----------------------------------------------------------------------------
void CPlayer::MoveFixedDistanceUntilCollision(CVector2f offset)
{
    // Find out how far we want to move
    CVector2f totalOffset = offset;
    float totalDistance = totalOffset.GetMagnitude();

    // Break it down into a number of smaller offsets
    int numSteps = ceil(totalDistance / mSmallestRadius);
    CVector2f smallOffset = totalOffset / (numSteps * 1.0f);

    // Apply these until we collide
    bool haveCollided = false;
    for (int i = 0; i < numSteps && !haveCollided; i++)
    {
        // Move by the small offset
        GetShape()->move(smallOffset);
        haveCollided = HandleCollisions();
    }
}

// =============================================================================
// CPlayer::IsColliding
// -----------------------------------------------------------------------------
bool CPlayer::IsColliding()
{
    // Let HandleCollisions do this check
    return HandleCollisions(false);
}

// =============================================================================
// CPlayer::PauseClocks()
// -----------------------------------------------------------------------------
void CPlayer::PauseClocks(bool paused)
{
    if (paused)
    {
        mJumpClock.Pause();
    }
    else
    {
        mJumpClock.Resume();
    }
}<commit_msg>Draw the jumo cooldown<commit_after>//
//  CPlayer.cpp
//  SwingGame
//
//  Created by Tim Brier on 11/10/2014.
//  Copyright (c) 2014 tbrier. All rights reserved.
//

// =============================================================================
// Include Files
// -----------------------------------------------------------------------------
#include "SFMLIntegration/SFMLIntegration.hpp"
#include "CPlayer.hpp"
#include "SystemUtilities.hpp"
#include "CSwingGame.hpp"
#include "CLevel.hpp"
#include "CLine.hpp"
#include "CollisionHandler.hpp"
#include "CRigidSwing.hpp"
#include "CFlexibleSwing.hpp"
#include "CSpringSwing.hpp"

// =============================================================================
// Static members
// -----------------------------------------------------------------------------
CVector2f CPlayer::smJumpVelocity = CVector2f(0.0f, -250.0f);
CTime CPlayer::smJumpCooldown = CTime::Seconds(1.5f);

// =============================================================================
// SUVAT Helper Methods
// -----------------------------------------------------------------------------
CVector2f GetVectorMoved(CVector2f u, CTime t, CVector2f a)
{
    // s = ut + 0.5at^2
    float ts = t.asSeconds();
    return (u * ts) + (0.5f * a * ts * ts);
}

CVector2f GetFinalVelocity(CVector2f u, CVector2f a, CTime t)
{
    // v = u + at
    float ts = t.asSeconds();
    return u + (a * ts);
}

// =============================================================================
// CPlayer constructor/destrctor
// -----------------------------------------------------------------------------
CPlayer::CPlayer(CLevel *theParentLevel) :  mParentLevel(theParentLevel)
{
    // Create the shape
    mSmallestRadius = 10.0f;

    std::list<CVector2f> thePoints;
    thePoints.push_back(CVector2f(-mSmallestRadius, -mSmallestRadius));
    thePoints.push_back(CVector2f(mSmallestRadius,  -mSmallestRadius));
    thePoints.push_back(CVector2f(mSmallestRadius,  mSmallestRadius));
    thePoints.push_back(CVector2f(-mSmallestRadius, mSmallestRadius));

    SetShape(CConvexShape(thePoints));
    GetShape()->setFillColor(CColour(255, 0, 255));

    float mass = 1.0f; // Read from file?
    SetInverseMass(1.0f / mass);
}

CPlayer::~CPlayer()
{

}

// =============================================================================
// CPlayer::Update
// -----------------------------------------------------------------------------
void CPlayer::Update(CTime elapsedTime)
{
    HandleInput();

    // Now simulate the physics until we've simulated the entire elapsedTime
    while (elapsedTime > CTime::Zero)
    {
        // Find the acceleration applied by any physics items
        CVector2f acceleration = HandlePhysics();

        // Now try to move based on our velocity
        CTime timeSimulated = MoveUntilCollision(elapsedTime, acceleration);
        elapsedTime -= timeSimulated;
    }
}

// =============================================================================
// CPlayer::ShouldUpdateForState
// -----------------------------------------------------------------------------
bool CPlayer::ShouldUpdateForState(EGameState theState)
{
    // Only update if we're not paused
    if ((theState & kGameStatePaused) != 0)
    {
        return false;
    }

    return CUpdateable::ShouldUpdateForState(theState);
}

// =============================================================================
// CPlayer::Draw
// -----------------------------------------------------------------------------
void CPlayer::Draw(CWindow *theWindow)
{
    // Colour the player by the colour of the swing we will fire
    GetShape()->setFillColor(mSwings[mSwingToFire]->GetColour());
    theWindow->DrawShape(*GetShape());

    // Draw the cooldown timer for jumping
    CTime jumpClockTime = mJumpClock.GetElapsedTime();
    if (jumpClockTime < smJumpCooldown)
    {
        CTime timeLeft = smJumpCooldown - jumpClockTime;
        float percentageLeft = timeLeft / smJumpCooldown;
        float cooldownBarWidth = percentageLeft * (mSmallestRadius * 2.0f);
        float cooldownBarLeft = GetPosition().x - mSmallestRadius;
        float cooldownBarTop = GetPosition().y - 2.5f;
        std::list<CVector2f> points;
        points.push_back(CVector2f(0.0f, 0.0f));
        points.push_back(CVector2f(cooldownBarWidth, 0.0f));
        points.push_back(CVector2f(cooldownBarWidth, 5.0f));
        points.push_back(CVector2f(0.0f, 5.0f));
        CConvexShape cooldownBar(points);
        cooldownBar.setPosition(cooldownBarLeft, cooldownBarTop);
        cooldownBar.setFillColor(CColour::Red);
        theWindow->DrawShape(cooldownBar);
    }
}

// =============================================================================
// CPlayer::Init
// -----------------------------------------------------------------------------
void CPlayer::Init()
{
    // Register any update/renderables
    CSwingGame::RegisterUpdateable(this);
    CSwingGame::RegisterRenderable(this);

    mSwings[kSwingTypeRigid] = new CRigidSwing(this, mParentLevel);
    mSwings[kSwingTypeFlexible] = new CFlexibleSwing(this, mParentLevel);
    mSwings[kSwingTypeSpring] = new CSpringSwing(this, mParentLevel);
}

// =============================================================================
// CPlayer::Cleanup
// -----------------------------------------------------------------------------
void CPlayer::Cleanup()
{
    SAFE_DELETE(mSwings[kSwingTypeRigid]);
    SAFE_DELETE(mSwings[kSwingTypeFlexible]);
    SAFE_DELETE(mSwings[kSwingTypeSpring]);

    // Unregister update/renderables
    CSwingGame::UnregisterUpdateable(this);
    CSwingGame::UnregisterRenderable(this);
}

// =============================================================================
// CPlayer::StartLevel
// -----------------------------------------------------------------------------
void CPlayer::StartLevel(SStartPosition theStartPos)
{
    GetShape()->setPosition(theStartPos.mPosition);
    SetVelocity(theStartPos.mVelocity);

    for (int i = 0; i < kSwingTypeMax; i++)
    {
        mSwings[i]->Detach();
    }

    mSwingToFire = theStartPos.mSwingType;
    mCurrentSwing = mSwings[mSwingToFire];

    mCurrentSwing->AttemptToAttach(theStartPos.mSwingTarget);

    // Start the jump clock at the cooldown time so we can jump straight away
    mJumpClock.Restart(smJumpCooldown);
}

// =============================================================================
// CPlayer::HandleInput
// -----------------------------------------------------------------------------
void CPlayer::HandleInput()
{
#if SG_DEBUG    // In debug move the player to the mouse pos if r shift is down
    if (CKeyboard::isKeyPressed(CKeyboard::RShift))
    {
        GetShape()->setPosition(SystemUtilities::GetMousePosition());
        HandleCollisions();

        // Reset velocity
        SetVelocity(CVector2f(0.0f, 0.0f));
    }
#endif

    // Connect the swing on left mouse click
    CVector2f mousePos;
    if (SystemUtilities::WasButtonPressedThisCycle(CMouse::Left, &mousePos))
    {
        // Detach the current swing before we try to fire the next
        mCurrentSwing->Detach();

        // Update the current swing type and try to attach it
        mCurrentSwing = mSwings[mSwingToFire];
        mCurrentSwing->AttemptToAttach(mousePos);
    }

    // Disconnect on right click
    if (SystemUtilities::WasButtonPressedThisCycle(CMouse::Right, &mousePos))
    {
        // We specifically requested this detach so respond to it
        mCurrentSwing->Detach(true);
    }

    // Jump on space
    if (SystemUtilities::WasKeyPressedThisCycle(CKeyboard::Space))
    {
        // Only jump if we're attached to a swing, and detach it when we do
        if (mCurrentSwing->CanJumpFrom()
            && mJumpClock.GetElapsedTime() >= smJumpCooldown)
        {
            mCurrentSwing->Detach();
            SetVelocity(GetVelocity() + smJumpVelocity);
            mJumpClock.Restart();
        }
    }

    // Change swing type
    if (SystemUtilities::WasKeyPressedThisCycle(CKeyboard::Num1))
    {
        mSwingToFire = kSwingTypeRigid;
    }
    if (SystemUtilities::WasKeyPressedThisCycle(CKeyboard::Num2))
    {
        mSwingToFire = kSwingTypeFlexible;
    }
    if (SystemUtilities::WasKeyPressedThisCycle(CKeyboard::Num3))
    {
        mSwingToFire = kSwingTypeSpring;
    }
}

// =============================================================================
// CPlayer::HandlePhysics
// Return the resulting acceleration of the player
// -----------------------------------------------------------------------------
CVector2f CPlayer::HandlePhysics()
{
    CVector2f acceleration = mParentLevel->GetGravityAcceleration();

    acceleration = mCurrentSwing->AttenuateGravity(acceleration);

    return acceleration;
}

// =============================================================================
// CPlayer::HandleCollisions
// Return true if we resolved a collision
// -----------------------------------------------------------------------------
bool CPlayer::HandleCollisions(bool shouldResolve /* = false */)
{
    bool didCollide = false;

    // Check for collisions against all level obstacles
    std::list<CPhysicsBody *> theObstacles = mParentLevel->GetObstacles();

    CVector2f correctionVector;
    FOR_EACH_IN_LIST(CPhysicsBody *, theObstacles)
    {
        if (CollisionHandler::AreColliding(*GetShape(),
                                           *((*it)->GetShape()),
                                           &correctionVector))
        {
            didCollide = true;

            if (shouldResolve)
            {
                // Resolve the collision
                CollisionHandler::Resolve(this, (*it), correctionVector);
            }
        }
    }

    return didCollide;
}

// =============================================================================
// CPlayer::MoveUntilCollision
// Move for the given amount of time with the given acceleration or until
// we collide
// Return the time we actually simulated movement for
// -----------------------------------------------------------------------------
CTime CPlayer::MoveUntilCollision(CTime elapsedTime, CVector2f acceleration)
{
    // Find the total offset we've been requested to move
    CVector2f totalOffset;
    totalOffset = GetVectorMoved(GetVelocity(), elapsedTime, acceleration);
    float totalDistance = totalOffset.GetMagnitude();

    // If we don't need to move bail out
    if (totalDistance == 0)
    {
        return elapsedTime;
    }

    // Find how many timesteps we should use to make sure we don't move by
    // more than the shapes smallest "radius"
    int numTimesteps = ceil(totalDistance / mSmallestRadius);

    // Find how big this timesteps should be
    CTime timeStep = elapsedTime / (numTimesteps * 1.0f);

    // Now find how far we should move for each timestep
    CVector2f smallOffset = totalOffset / (numTimesteps * 1.0f);

    // And simulate these timesteps until we've done them all or we've collided
    bool haveCollided = false;
    CTime timeSimulatedSoFar = CTime::Zero;
    for (int i = 0; i < numTimesteps && !haveCollided; i++)
    {
        // Update velocity to what it will be at the end of this timestep
        SetVelocity(GetFinalVelocity(GetVelocity(), acceleration, timeStep));

        // Now move the shape and handle any collisions
        GetShape()->move(smallOffset);
        haveCollided = HandleCollisions();

        timeSimulatedSoFar += timeStep;
    }

    return timeSimulatedSoFar;
}

// =============================================================================
// CPlayer::MoveFixedDistanceUntilCollision
// -----------------------------------------------------------------------------
void CPlayer::MoveFixedDistanceUntilCollision(CVector2f offset)
{
    // Find out how far we want to move
    CVector2f totalOffset = offset;
    float totalDistance = totalOffset.GetMagnitude();

    // Break it down into a number of smaller offsets
    int numSteps = ceil(totalDistance / mSmallestRadius);
    CVector2f smallOffset = totalOffset / (numSteps * 1.0f);

    // Apply these until we collide
    bool haveCollided = false;
    for (int i = 0; i < numSteps && !haveCollided; i++)
    {
        // Move by the small offset
        GetShape()->move(smallOffset);
        haveCollided = HandleCollisions();
    }
}

// =============================================================================
// CPlayer::IsColliding
// -----------------------------------------------------------------------------
bool CPlayer::IsColliding()
{
    // Let HandleCollisions do this check
    return HandleCollisions(false);
}

// =============================================================================
// CPlayer::PauseClocks()
// -----------------------------------------------------------------------------
void CPlayer::PauseClocks(bool paused)
{
    if (paused)
    {
        mJumpClock.Pause();
    }
    else
    {
        mJumpClock.Resume();
    }
}<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
///
/// \file generic.hpp
/// -----------------
///
/// (c) Copyright Domagoj Saric 2016.
///
///  Use, modification and distribution are subject to the
///  Boost Software License, Version 1.0. (See accompanying file
///  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
///
///  See http://www.boost.org for most recent version.
///
////////////////////////////////////////////////////////////////////////////////
//------------------------------------------------------------------------------
#ifndef generic_hpp__99FE2034_248F_4C7D_8CD2_EB2BB8247377
#define generic_hpp__99FE2034_248F_4C7D_8CD2_EB2BB8247377
#pragma once
//------------------------------------------------------------------------------
#include <boost/sweater/queues/mpmc_moodycamel.hpp>

#include <boost/config_ex.hpp>
#include <boost/container/small_vector.hpp>
#include <boost/container/static_vector.hpp>
#include <boost/functionoid/functionoid.hpp>
#include <boost/range/algorithm/count_if.hpp>
#include <boost/range/iterator_range_core.hpp>
#include <boost/throw_exception.hpp>

#include <atomic>
#include <cstdint>
#include <future>
#include <iterator>
#ifdef _MSC_VER
#include <malloc.h>
#else
#include <alloca.h>
#endif // _MSC_VER
#include <memory>
#include <mutex>
#include <thread>
//------------------------------------------------------------------------------
namespace boost
{
//------------------------------------------------------------------------------
namespace sweater
{
//------------------------------------------------------------------------------

#ifndef BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
#	define BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY 0
#endif // BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY

namespace queues { template <typename Work> class mpmc_moodycamel; }

BOOST_OVERRIDABLE_SYMBOL
auto const hardware_concurrency( static_cast<std::uint8_t>( std::thread::hardware_concurrency() ) );

class impl
{
private:
#ifdef __ANROID__
    // https://petewarden.com/2015/10/11/one-weird-trick-for-faster-android-multithreading
    static auto constexpr spin_count = 30 * 1000 * 1000;
#else
    static auto constexpr spin_count =                1;
#endif // __ANROID__

    struct worker_traits : functionoid::std_traits
    {
        static constexpr auto copyable    = functionoid::support_level::na     ;
        static constexpr auto moveable    = functionoid::support_level::nofail ;
        static constexpr auto destructor  = functionoid::support_level::trivial;
        static constexpr auto is_noexcept = true;
        static constexpr auto rtti        = false;

        using empty_handler = functionoid::assert_on_empty;
    }; // struct worker_traits

    using worker_counter = std::atomic<std::uint16_t>;

    class batch_semaphore
    {
    public:
        batch_semaphore( std::uint16_t const initial_value ) : counter( initial_value ) {}

        void release() noexcept
        {
            std::unique_lock<std::mutex> lock( mutex );
            if ( counter.fetch_sub( 1, std::memory_order_acquire ) == 1 )
                event.notify_one();
        }

        BOOST_NOINLINE
        void wait() noexcept
        {
            for ( auto try_count( 0 ); try_count < spin_count; ++try_count )
            {
                bool const all_workers_done( counter.load( std::memory_order_relaxed ) == 0 );
                if ( BOOST_LIKELY( all_workers_done ) )
                    return;
            }
            std::unique_lock<std::mutex> lock( mutex );
            while ( BOOST_UNLIKELY( counter.load( std::memory_order_relaxed ) != 0 ) )
                event.wait( lock );
        }

    private:
                worker_counter          counter;
        mutable std::condition_variable event;
                std::mutex              mutex;
    }; // struct batch_semaphore

    using work_t = functionoid::callable<void(), worker_traits>;

    using my_queue = queues::mpmc_moodycamel<work_t>;

public:
	impl()
#if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
    : pool_( BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY - 1 )
#endif
	{
    #if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        BOOST_ASSUME( hardware_concurrency <= BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY );
    #else
        auto const number_of_worker_threads( hardware_concurrency - 1 );
        auto p_workers( std::make_unique<std::thread[]>( number_of_worker_threads ) );
        pool_ = make_iterator_range_n( p_workers.get(), number_of_worker_threads );
    #endif // !BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
		for ( auto & worker : pool_ )
		{
			auto const worker_loop
			(
				[this]() noexcept
				{
                    auto token( queue_.consumer_token() );

                    work_t work;

                    for ( ; ; )
                    {
                        for ( auto try_count( 0 ); try_count < spin_count; ++try_count )
                        {
                            if ( BOOST_LIKELY( queue_.dequeue( work, token ) ) )
                            {
                                work();
                                if ( spin_count > 1 ) // restart the spin-wait
                                    try_count = 0;
                            }
                        }

                        if ( BOOST_UNLIKELY( brexit_.load( std::memory_order_relaxed ) ) )
							return;

                        {
                            std::unique_lock<std::mutex> lock( mutex_ );
                            /// \note No need for a another loop here as a
                            /// spurious-wakeup would be handled by the check in
                            /// the loop above.
                            ///               (08.11.2016.) (Domagoj Saric)
                            if ( !BOOST_LIKELY( queue_.dequeue( work, token ) ) )
                            {
                                work.clear();
                                work_event_.wait( lock );
                            }
                        }
                        if ( BOOST_LIKELY( static_cast<bool>( work ) ) )
                            work();
                    }
				}
			); // worker_loop
            worker = std::thread( worker_loop );
		}
    #if !BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        p_workers.release();
    #endif // !BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
	}

	~impl() noexcept
	{
		brexit_.store( true, std::memory_order_relaxed );
        work_event_.notify_all();
		for ( auto & worker : pool_ )
			worker.join();
    #if !BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        delete[] pool_.begin();
    #endif // BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
	}

	auto number_of_workers() const
    {
        auto const result( static_cast<std::uint16_t>( pool_.size() + 1 ) );
    #if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        BOOST_ASSUME( result <= BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY );
    #endif
        return result;
    }

    /// For GCD dispatch_apply/OMP-like parallel loops.
    /// \details Guarantees that <VAR>work</VAR> will not be called more than
    /// <VAR>iterations</VAR> times (even if number_of_workers() > iterations).
	template <typename F>
	bool spread_the_sweat( std::uint16_t const iterations, F && __restrict work ) noexcept
	{
		static_assert( noexcept( work( iterations, iterations ) ), "F must be noexcept" );

        if ( BOOST_UNLIKELY( iterations == 0 ) )
            return true;

        auto const number_of_workers               ( this->number_of_workers()      );
		auto const iterations_per_worker           ( iterations / number_of_workers );
        auto const leave_one_for_the_calling_thread( iterations_per_worker == 0     ); // If iterations < workers prefer using the caller thread instead of waking up a worker thread...
		auto const threads_with_extra_iteration    ( iterations % number_of_workers - leave_one_for_the_calling_thread );
        BOOST_ASSERT( !leave_one_for_the_calling_thread || iterations < number_of_workers );

        auto const number_of_work_parts( std::min<std::uint16_t>( number_of_workers, iterations ) );
        std::uint16_t const number_of_dispatched_work_parts( number_of_work_parts - 1 );

        batch_semaphore semaphore( number_of_dispatched_work_parts );

#   if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        BOOST_ASSUME( number_of_dispatched_work_parts < BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY );
#   else
        BOOST_ASSUME( number_of_dispatched_work_parts < 512 );
#   endif
        /// \note MSVC does not support VLAs but has an alloca that returns
        /// (16 byte) aligned memory. Clang's alloca is unaligned and it does
        /// not support VLAs of non-POD types.
        ///                                   (21.01.2017.) (Domagoj Saric)
#   ifdef BOOST_MSVC
        auto const dispatched_work_parts( static_cast<work_t *>( alloca( ( number_of_dispatched_work_parts ) * sizeof( work_t ) ) ) );
#   else
        alignas( work_t ) char dispatched_work_parts_storage[ number_of_dispatched_work_parts * sizeof( work_t ) ];
        auto const dispatched_work_parts( reinterpret_cast<work_t *>( dispatched_work_parts_storage ) );
#   endif // _MSC_VER

        std::uint16_t iteration( 0 );
        if ( BOOST_LIKELY( iterations > 1 ) )
        {
            for ( std::uint8_t work_part( 0 ); work_part < number_of_dispatched_work_parts; ++work_part )
            {
                auto const start_iteration( iteration );
                auto const extra_iteration( work_part < threads_with_extra_iteration );
                auto const end_iteration  ( start_iteration + iterations_per_worker + extra_iteration );
                new ( &dispatched_work_parts[ work_part ] ) work_t
                (
                    [&work, &semaphore, start_iteration = iteration, end_iteration]() noexcept
                    {
                        work( start_iteration, end_iteration );
                        semaphore.release();
                    }
                );
                iteration = end_iteration;
            }

            auto const enqueue_succeeded( queue_.enqueue_bulk( std::make_move_iterator( dispatched_work_parts ), number_of_dispatched_work_parts ) );
            for ( std::uint8_t work_part( 0 ); work_part < number_of_dispatched_work_parts; ++work_part )
                dispatched_work_parts[ work_part ].~work_t();
            if ( !BOOST_LIKELY( enqueue_succeeded ) )
                return false;
            std::unique_lock<std::mutex> lock( mutex_ );
            work_event_.notify_all();
        }

		auto const caller_thread_start_iteration( iteration );
		BOOST_ASSERT( caller_thread_start_iteration < iterations );
		work( caller_thread_start_iteration, iterations );

        if ( BOOST_LIKELY( iterations > 1 ) )
        {
            semaphore.wait();
        }

        return true;
	}

	template <typename F>
	bool fire_and_forget( F && work )
	{
        struct self_destructed_work
        {
            self_destructed_work( F       && work ) noexcept( std::is_nothrow_move_constructible<F>::value ) { new ( storage ) F( std::move( work ) ); }
            self_destructed_work( F const &  work ) noexcept( std::is_nothrow_copy_constructible<F>::value ) { new ( storage ) F(            work   ); }
            void operator()() noexcept
            {
                auto & work( reinterpret_cast<F &>( storage ) );
                work();
                work.~F();
            }
            alignas( work ) char storage[ sizeof( work ) ];
        };
        return queue_.enqueue( self_destructed_work( std::forward<F>( work ) ) );
	}

    template <typename F>
    auto dispatch( F && work )
    {
        // http://scottmeyers.blogspot.hr/2013/03/stdfutures-from-stdasync-arent-special.html
        using result_t = typename std::result_of<F()>::type;
        std::promise<result_t> promise;
        std::future<result_t> future( promise.get_future() );
        if
        (
            fire_and_forget
            (
                [promise = std::move( promise ), work = std::forward<F>( work )]
                () mutable { promise.set_value( work() ); }
            )
        )
            return future;
        BOOST_THROW_EXCEPTION( std::bad_alloc() );
    }

private:
            std::atomic<bool>       brexit_ = ATOMIC_FLAG_INIT;
	        std::mutex              mutex_;
    mutable std::condition_variable work_event_;

#if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
	using pool_threads_t = container::static_vector<std::thread, BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY - 1>; // also sweat on the calling thread
#else
    using pool_threads_t = iterator_range<std::thread *>;
#endif
	pool_threads_t pool_;

    /// \todo Further queue refinements.
    /// https://en.wikipedia.org/wiki/Work_stealing
    /// http://www.drdobbs.com/parallel/writing-lock-free-code-a-corrected-queue/210604448
    /// https://github.com/cameron314/readerwriterqueue
    /// http://moodycamel.com/blog/2013/a-fast-lock-free-queue-for-c++
    /// http://stackoverflow.com/questions/1164023/is-there-a-production-ready-lock-free-queue-or-hash-implementation-in-c#14936831
    /// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md
    /// https://github.com/facebook/folly/blob/master/folly/MPMCQueue.h
    /// http://landenlabs.com/code/ring/ring.html
    /// https://github.com/Qarterd/Honeycomb/blob/master/src/common/Honey/Thread/Pool.cpp
    ///                                       (12.10.2016.) (Domagoj Saric)
    my_queue queue_;
}; // class impl

//------------------------------------------------------------------------------
} // namespace sweater
//------------------------------------------------------------------------------
} // namespace boost
//------------------------------------------------------------------------------
#endif // generic_hpp<commit_msg>Fixed generic fire_and_forget() to wake a worker thread. Minor optimizations.<commit_after>////////////////////////////////////////////////////////////////////////////////
///
/// \file generic.hpp
/// -----------------
///
/// (c) Copyright Domagoj Saric 2016.
///
///  Use, modification and distribution are subject to the
///  Boost Software License, Version 1.0. (See accompanying file
///  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
///
///  See http://www.boost.org for most recent version.
///
////////////////////////////////////////////////////////////////////////////////
//------------------------------------------------------------------------------
#ifndef generic_hpp__99FE2034_248F_4C7D_8CD2_EB2BB8247377
#define generic_hpp__99FE2034_248F_4C7D_8CD2_EB2BB8247377
#pragma once
//------------------------------------------------------------------------------
#include <boost/sweater/queues/mpmc_moodycamel.hpp>

#include <boost/config_ex.hpp>
#include <boost/container/small_vector.hpp>
#include <boost/container/static_vector.hpp>
#include <boost/functionoid/functionoid.hpp>
#include <boost/range/algorithm/count_if.hpp>
#include <boost/range/iterator_range_core.hpp>

#include <atomic>
#include <cstdint>
#include <exception>
#include <future>
#include <iterator>
#ifdef _MSC_VER
#include <malloc.h>
#else
#include <alloca.h>
#endif // _MSC_VER
#include <memory>
#include <mutex>
#include <thread>
//------------------------------------------------------------------------------
namespace boost
{
//------------------------------------------------------------------------------
namespace sweater
{
//------------------------------------------------------------------------------

#ifndef BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
#	define BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY 0
#endif // BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY

namespace queues { template <typename Work> class mpmc_moodycamel; }

BOOST_OVERRIDABLE_SYMBOL
auto const hardware_concurrency( static_cast<std::uint8_t>( std::thread::hardware_concurrency() ) );

class impl
{
private:
#ifdef __ANROID__
    // https://petewarden.com/2015/10/11/one-weird-trick-for-faster-android-multithreading
    static auto constexpr spin_count = 30 * 1000 * 1000;
#else
    static auto constexpr spin_count =                1;
#endif // __ANROID__

    struct worker_traits : functionoid::std_traits
    {
        static constexpr auto copyable    = functionoid::support_level::na     ;
        static constexpr auto moveable    = functionoid::support_level::nofail ;
        static constexpr auto destructor  = functionoid::support_level::trivial;
        static constexpr auto is_noexcept = true;
        static constexpr auto rtti        = false;

        using empty_handler = functionoid::assert_on_empty;
    }; // struct worker_traits

    using worker_counter = std::atomic<std::uint16_t>;

    class batch_semaphore
    {
    public:
        batch_semaphore( std::uint16_t const initial_value ) : counter( initial_value ) {}

        void release() noexcept
        {
            std::unique_lock<std::mutex> lock( mutex );
            if ( counter.fetch_sub( 1, std::memory_order_acquire ) == 1 )
                event.notify_one();
        }

        BOOST_NOINLINE
        void wait() noexcept
        {
            for ( auto try_count( 0 ); try_count < spin_count; ++try_count )
            {
                bool const all_workers_done( counter.load( std::memory_order_relaxed ) == 0 );
                if ( BOOST_LIKELY( all_workers_done ) )
                    return;
            }
            std::unique_lock<std::mutex> lock( mutex );
            while ( BOOST_UNLIKELY( counter.load( std::memory_order_relaxed ) != 0 ) )
                event.wait( lock );
        }

    private:
                worker_counter          counter;
        mutable std::condition_variable event;
                std::mutex              mutex;
    }; // struct batch_semaphore

    using work_t = functionoid::callable<void(), worker_traits>;

    using my_queue = queues::mpmc_moodycamel<work_t>;

public:
	impl()
#if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
    : pool_( BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY - 1 )
#endif
	{
    #if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        BOOST_ASSUME( hardware_concurrency <= BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY );
    #else
        auto const number_of_worker_threads( hardware_concurrency - 1 );
        auto p_workers( std::make_unique<std::thread[]>( number_of_worker_threads ) );
        pool_ = make_iterator_range_n( p_workers.get(), number_of_worker_threads );
    #endif // !BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
		for ( auto & worker : pool_ )
		{
			auto const worker_loop
			(
				[this]() noexcept
				{
                    auto token( queue_.consumer_token() );

                    work_t work;

                    for ( ; ; )
                    {
                        for ( auto try_count( 0 ); try_count < spin_count; ++try_count )
                        {
                            if ( BOOST_LIKELY( queue_.dequeue( work, token ) ) )
                            {
                                work();
                                if ( spin_count > 1 ) // restart the spin-wait
                                    try_count = 0;
                            }
                        }

                        if ( BOOST_UNLIKELY( brexit_.load( std::memory_order_relaxed ) ) )
							return;

                        {
                            std::unique_lock<std::mutex> lock( mutex_ );
                            /// \note No need for a another loop here as a
                            /// spurious-wakeup would be handled by the check in
                            /// the loop above.
                            ///               (08.11.2016.) (Domagoj Saric)
                            if ( !BOOST_LIKELY( queue_.dequeue( work, token ) ) )
                            {
                                work.clear();
                                work_event_.wait( lock );
                            }
                        }
                        if ( BOOST_LIKELY( static_cast<bool>( work ) ) )
                            work();
                    }
				}
			); // worker_loop
            worker = std::thread( worker_loop );
		}
    #if !BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        p_workers.release();
    #endif // !BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
	}

	~impl() noexcept
	{
		brexit_.store( true, std::memory_order_relaxed );
        work_event_.notify_all();
		for ( auto & worker : pool_ )
			worker.join();
    #if !BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        delete[] pool_.begin();
    #endif // BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
	}

	auto number_of_workers() const
    {
        auto const result( static_cast<std::uint16_t>( pool_.size() + 1 ) );
    #if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        BOOST_ASSUME( result <= BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY );
    #endif
        return result;
    }

    /// For GCD dispatch_apply/OMP-like parallel loops.
    /// \details Guarantees that <VAR>work</VAR> will not be called more than
    /// <VAR>iterations</VAR> times (even if number_of_workers() > iterations).
	template <typename F>
	bool spread_the_sweat( std::uint16_t const iterations, F && __restrict work ) noexcept
	{
		static_assert( noexcept( work( iterations, iterations ) ), "F must be noexcept" );

        if ( BOOST_UNLIKELY( iterations == 0 ) )
            return true;

        auto const number_of_workers               ( this->number_of_workers()      );
		auto const iterations_per_worker           ( iterations / number_of_workers );
        auto const leave_one_for_the_calling_thread( iterations_per_worker == 0     ); // If iterations < workers prefer using the caller thread instead of waking up a worker thread...
		auto const threads_with_extra_iteration    ( iterations % number_of_workers - leave_one_for_the_calling_thread );
        BOOST_ASSERT( !leave_one_for_the_calling_thread || iterations < number_of_workers );

        auto const number_of_work_parts( std::min<std::uint16_t>( number_of_workers, iterations ) );
        std::uint16_t const number_of_dispatched_work_parts( number_of_work_parts - 1 );

        batch_semaphore semaphore( number_of_dispatched_work_parts );

#   if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
        BOOST_ASSUME( number_of_dispatched_work_parts < BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY );
#   else
        BOOST_ASSUME( number_of_dispatched_work_parts < 512 );
#   endif
        /// \note MSVC does not support VLAs but has an alloca that returns
        /// (16 byte) aligned memory. Clang's alloca is unaligned and it does
        /// not support VLAs of non-POD types.
        ///                                   (21.01.2017.) (Domagoj Saric)
#   ifdef BOOST_MSVC
        auto const dispatched_work_parts( static_cast<work_t *>( alloca( ( number_of_dispatched_work_parts ) * sizeof( work_t ) ) ) );
#   else
        alignas( work_t ) char dispatched_work_parts_storage[ number_of_dispatched_work_parts * sizeof( work_t ) ];
        auto const dispatched_work_parts( reinterpret_cast<work_t *>( dispatched_work_parts_storage ) );
#   endif // _MSC_VER

        std::uint16_t iteration( 0 );
        if ( BOOST_LIKELY( iterations > 1 ) )
        {
            for ( std::uint8_t work_part( 0 ); work_part < number_of_dispatched_work_parts; ++work_part )
            {
                auto const start_iteration( iteration );
                auto const extra_iteration( work_part < threads_with_extra_iteration );
                auto const end_iteration  ( start_iteration + iterations_per_worker + extra_iteration );
                new ( &dispatched_work_parts[ work_part ] ) work_t
                (
                    [&work, &semaphore, start_iteration = iteration, end_iteration]() noexcept
                    {
                        work( start_iteration, end_iteration );
                        semaphore.release();
                    }
                );
                iteration = end_iteration;
            }

            auto const enqueue_succeeded( queue_.enqueue_bulk( std::make_move_iterator( dispatched_work_parts ), number_of_dispatched_work_parts ) );
            for ( std::uint8_t work_part( 0 ); work_part < number_of_dispatched_work_parts; ++work_part )
                dispatched_work_parts[ work_part ].~work_t();
            /// No need for a branch here as the worker thread has to handle
            /// spurious wakeups anyway.
            std::unique_lock<std::mutex> lock( mutex_ );
            work_event_.notify_all();
            return BOOST_LIKELY( enqueue_succeeded );
        }

		auto const caller_thread_start_iteration( iteration );
		BOOST_ASSERT( caller_thread_start_iteration < iterations );
		work( caller_thread_start_iteration, iterations );

        if ( BOOST_LIKELY( iterations > 1 ) )
        {
            semaphore.wait();
        }

        return true;
	}

	template <typename F>
	bool fire_and_forget( F && work )
	{
        struct self_destructed_work
        {
            self_destructed_work( F       && work ) noexcept( std::is_nothrow_move_constructible<F>::value ) { new ( storage ) F( std::move( work ) ); }
            self_destructed_work( F const &  work ) noexcept( std::is_nothrow_copy_constructible<F>::value ) { new ( storage ) F(            work   ); }
            void operator()() noexcept
            {
                auto & work( reinterpret_cast<F &>( storage ) );
                work();
                work.~F();
            }
            alignas( work ) char storage[ sizeof( work ) ];
        };
        auto const enqueue_succeeded( queue_.enqueue( self_destructed_work( std::forward<F>( work ) ) ) );
        std::unique_lock<std::mutex> lock( mutex_ );
        work_event_.notify_one();
        return BOOST_LIKELY( enqueue_succeeded );
	}

    template <typename F>
    auto dispatch( F && work )
    {
        // http://scottmeyers.blogspot.hr/2013/03/stdfutures-from-stdasync-arent-special.html
        using result_t = typename std::result_of<F()>::type;
        std::promise<result_t> promise;
        std::future<result_t> future( promise.get_future() );
        auto const dispatch_succeeded
        (
            fire_and_forget
            (
                [promise = std::move( promise ), work = std::forward<F>( work )]
                () mutable { promise.set_value( work() ); }
            )
        );
        if ( BOOST_UNLIKELY( !dispatch_succeeded ) )
            future.set_exception( std::make_exception_ptr( std::bad_alloc() ) );
        return future;
    }

private:
            std::atomic<bool>       brexit_ = ATOMIC_FLAG_INIT;
	        std::mutex              mutex_;
    mutable std::condition_variable work_event_;

#if BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY
	using pool_threads_t = container::static_vector<std::thread, BOOST_SWEATER_MAX_HARDWARE_CONCURRENCY - 1>; // also sweat on the calling thread
#else
    using pool_threads_t = iterator_range<std::thread *>;
#endif
	pool_threads_t pool_;

    /// \todo Further queue refinements.
    /// https://en.wikipedia.org/wiki/Work_stealing
    /// http://www.drdobbs.com/parallel/writing-lock-free-code-a-corrected-queue/210604448
    /// https://github.com/cameron314/readerwriterqueue
    /// http://moodycamel.com/blog/2013/a-fast-lock-free-queue-for-c++
    /// http://stackoverflow.com/questions/1164023/is-there-a-production-ready-lock-free-queue-or-hash-implementation-in-c#14936831
    /// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md
    /// https://github.com/facebook/folly/blob/master/folly/MPMCQueue.h
    /// http://landenlabs.com/code/ring/ring.html
    /// https://github.com/Qarterd/Honeycomb/blob/master/src/common/Honey/Thread/Pool.cpp
    ///                                       (12.10.2016.) (Domagoj Saric)
    my_queue queue_;
}; // class impl

//------------------------------------------------------------------------------
} // namespace sweater
//------------------------------------------------------------------------------
} // namespace boost
//------------------------------------------------------------------------------
#endif // generic_hpp<|endoftext|>


<commit_before>/*
 *  qiesmap.cpp
 *
 *  Copyright (c) 2016 Tobias Wood.
 *
 *  This Source Code Form is subject to the terms of the Mozilla Public
 *  License, v. 2.0. If a copy of the MPL was not distributed with this
 *  file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 */

#include <getopt.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "QI/Util.h"
#include "Filters/ApplyAlgorithmFilter.h"

using namespace std;
using namespace Eigen;

class ESAlgo : public Algorithm<complex<double>> {
protected:
    size_t m_size = 0;
public:
    size_t numInputs() const override { return 1; }
    size_t numConsts() const override { return 1; }
    size_t numOutputs() const override { return 6; }
    const vector<string> & names() const {
        static vector<string> _names = {"M", "T1", "T2", "th", "a", "b"};
        return _names;
    }
    size_t dataSize() const override { return m_size; }
    void setSize(const size_t s) { m_size = s; }
    virtual TArray defaultConsts() override {
        // B1
        TArray def = TArray::Ones(1);
        return def;
    }

    ArrayXd solveEig(const MatrixXd &A, const MatrixXd &B) const {
        RealQZ<MatrixXd> qz(A, B);
        VectorXd v = ArrayXd::Zero(A.cols());
        const MatrixXd &mS = qz.matrixS();
        const MatrixXd &mT = qz.matrixT();
        const MatrixXd &mZT = qz.matrixZ().transpose();
        int sInd = 0;
        double sVal = numeric_limits<double>::infinity();
        for (int i = 0; i < 6; i++) {
            const double a = mS.coeffRef(i,i);
            const double b = mT.coeffRef(i,i);
            const double l = fabs(a / b);
            if (l < sVal) {
                sVal = l;
                sInd = i;
            }
        }

        v(sInd) = 1.0;
        const double a = qz.matrixS().coeffRef(sInd,sInd);
        const double b = qz.matrixT().coeffRef(sInd,sInd);
        for (int j = sInd-1; j >= 0; j--) {
            const int st = j+1;
            const int sz = sInd-j;
            v.coeffRef(j) = -v.segment(st,sz).transpose().cwiseProduct(b*mS.block(j,st,1,sz) - a*mT.block(j,st,1,sz)).sum() / (b*mS.coeffRef(j,j) - a*mT.coeffRef(j,j));
        }
        v = (mZT * v).normalized();
        return v;
    }

    MatrixXd buildS(const ArrayXd &x, const ArrayXd &y) const {
        Matrix<double, Dynamic, 6> D(x.rows(), 6);
        D.col(0) = x*x;
        D.col(1) = x*y;
        D.col(2) = y*y;
        D.col(3) = x;
        D.col(4) = y;
        D.col(5).setConstant(1);
        return D.transpose() * D;
    }

    MatrixXd fitzC() const {
        typedef Matrix<double, 6, 6> Matrix6d;
        Matrix6d C = Matrix6d::Zero();
        // Fitzgibbon et al
        C(0,2) = -2; C(1,1) = 1; C(2,0) = -2;
        return C;
    }

    MatrixXd hyperC(const ArrayXd &x, const ArrayXd &y) const {
        typedef Matrix<double, 6, 6> Matrix6d;
        Matrix6d C = Matrix6d::Zero();
        // Fitzgibbon et al
        //C(0,2) = -2; C(1,1) = 1; C(2,0) = -2;

        // Hyper Ellipse
        const double N = x.cols();
        const double xc = x.sum() / N;
        const double yc = y.sum() / N;
        const double sx = x.square().sum() / N;
        const double sy = y.square().sum() / N;
        const double xy = (x * y).sum() / N;

        C << 6*sx, 6*xy, sx+sy, 6*xc, 2*yc, 1,
             6*xy, 4*(sx+sy), 6*xy, 4*yc, 4*xc, 0,
             sx + sy, 6*xy, 6*sy, 2*xc, 6*yc, 1,
             6*xc, 4*yc, 2*xc, 4, 0, 0,
             2*yc, 4*xc, 6*yc, 0, 4, 0,
             1, 0, 1, 0, 0, 0;

        return C;
    }

    void apply(const TInput &data, const TArray &inputs, TArray &outputs, TArray &resids, TIterations &its) const override
    {
        typedef Matrix<double, 6, 6> Matrix6d;
        typedef Matrix<double, 6, 1> Vector6d;
        const double B1 = inputs[0];
        const double scale = data.abs().maxCoeff();
        ArrayXd x = data.real() / scale;
        ArrayXd y = data.imag() / scale;

        MatrixXd S = buildS(x, y);
        Matrix6d C = hyperC(x, y);
        ArrayXd Z = solveEig(S, C);
        const double za = Z[0];
        const double zb = Z[1]/2;
        const double zc = Z[2];
        const double zd = Z[3]/2;
        const double zf = Z[4]/2;
        const double zg = Z[5];
        const double dsc=(zb*zb-za*zc);
        const double xc = (zc*zd-zb*zf)/dsc;
        const double yc = (za*zf-zb*zd)/dsc;
        const double th = atan2(yc,xc);
        double A = sqrt((2*(za*(zf*zf)+zc*(zd*zd)+zg*(zb*zb)-2*zb*zd*zf-za*zc*zg))/(dsc*(sqrt((za-zc)*(za-zc) + 4*zb*zb)-(za+zc))));
        double B = sqrt((2*(za*(zf*zf)+zc*(zd*zd)+zg*(zb*zb)-2*zb*zd*zf-za*zc*zg))/(dsc*(-sqrt((za-zc)*(za-zc) + 4*zb*zb)-(za+zc))));
        if (A > B) {
            std::swap(A, B);
        }
        //cout << "Z " << Z.transpose() << endl;
        //cout << "dsc " << dsc << " xc " << xc << " yc " << yc << " A " << A << " B " << B << endl;
        const double c = sqrt(xc*xc+yc*yc);
        const double b = (-c*A + sqrt(c*c*A*A - (c*c + B*B)*(A*A - B*B)))/(c*c + B*B);
        const double a = B / (b*B + c*sqrt(1-b*b));
        const double M = scale*c*(1-b*b)/(1-a*b);
        const double TR = 0.0065;
        const double FA = B1 * (25*M_PI/180.);
        const double T1 = -TR / log((a*(1+cos(FA)-a*b*cos(FA))-b)/(a*(1+cos(FA)-a*b)-b*cos(FA)));
        const double T2 = -TR / log(a);

        outputs[0] = M;
        outputs[1] = T1;
        outputs[2] = T2;
        outputs[3] = th;
        outputs[4] = a;
        outputs[5] = b;
    }
};

//******************************************************************************
// Arguments / Usage
//******************************************************************************
const string usage {
"Usage is: qiesmap [options] input \n\
\n\
A utility for calculating T1,T2,PD and f0 maps from SSFP data.\n\
Input must be a single complex image with at least 6 phase increments.\n\
\n\
Options:\n\
    --help, -h       : Print this message.\n\
    --verbose, -v    : Print more information.\n\
    --out, -o path   : Specify an output prefix.\n\
    --mask, -m file  : Mask input with specified file.\n\
    --B1, -b file    : B1 Map file (ratio)\n\
    --threads, -T N  : Use N threads (default=hardware limit).\n"
};

bool verbose = false;
static size_t num_threads = 4;
static string outPrefix;
const struct option long_opts[] = {
    {"help", no_argument, 0, 'h'},
    {"verbose", no_argument, 0, 'v'},
    {"out", required_argument, 0, 'o'},
    {"mask", required_argument, 0, 'm'},
    {"B1", required_argument, 0, 'b'},
    {"threads", required_argument, 0, 'T'},
    {0, 0, 0, 0}
};
const char *short_opts = "hvo:m:b:T:";

//******************************************************************************
// Main
//******************************************************************************
int main(int argc, char **argv) {
    Eigen::initParallel();
    QI::VolumeF::Pointer mask = ITK_NULLPTR;
    QI::VolumeF::Pointer B1   = ITK_NULLPTR;

    shared_ptr<ESAlgo> algo = make_shared<ESAlgo>();
    int indexptr = 0, c;
    while ((c = getopt_long(argc, argv, short_opts, long_opts, &indexptr)) != -1) {
        switch (c) {
            case 'v': verbose = true; break;
            case 'm':
                if (verbose) cout << "Opening mask file " << optarg << endl;
                mask = QI::ReadImage(optarg);
                break;
            case 'o':
                outPrefix = optarg;
                if (verbose) cout << "Output prefix will be: " << outPrefix << endl;
                break;
            case 'b':
                if (verbose) cout << "Opening B1 file: " << optarg << endl;
                B1 = QI::ReadImage(optarg);
                break;
            case 'T':
                num_threads = stoi(optarg);
                if (num_threads == 0)
                    num_threads = std::thread::hardware_concurrency();
                break;
            case 'h':
                cout << QI::GetVersion() << endl << usage << endl;
                return EXIT_SUCCESS;
            case '?': // getopt will print an error message
                return EXIT_FAILURE;
            default:
                cout << "Unhandled option " << string(1, c) << endl;
                return EXIT_FAILURE;
        }
    }
    if ((argc - optind) != 1) {
        cout << "Incorrect number of arguments." << endl << usage << endl;
        return EXIT_FAILURE;
    }

    string inputFilename = argv[optind++];
    if (verbose) cout << "Opening file: " << inputFilename << endl;
    auto data = QI::ReadVectorImage<complex<float>>(inputFilename);
    auto apply = itk::ApplyAlgorithmFilter<ESAlgo, complex<float>>::New();
    algo->setSize(data->GetNumberOfComponentsPerPixel());
    apply->SetAlgorithm(algo);
    apply->SetPoolsize(num_threads);
    apply->SetInput(0, data);
    if (mask)
        apply->SetMask(mask);
    if (B1)
        apply->SetConst(0, B1);
    if (verbose) {
        cout << "Processing" << endl;
        auto monitor = QI::GenericMonitor::New();
        apply->AddObserver(itk::ProgressEvent(), monitor);
    }
    apply->Update();
    if (verbose) {
        cout << "Elapsed time was " << apply->GetTotalTime() << "s" << endl;
        cout << "Mean time per voxel was " << apply->GetMeanTime() << "s" << endl;
        cout << "Writing results files." << endl;
    }
    outPrefix = outPrefix + "ES_";
    for (int i = 0; i < algo->numOutputs(); i++) {
        QI::WriteImage(apply->GetOutput(i), outPrefix + algo->names().at(i) + QI::OutExt());
    }

    if (verbose) cout << "Finished." << endl;
    return EXIT_SUCCESS;
}
<commit_msg>WIP: Added code to read in sequence parameters.<commit_after>/*
 *  qiesmap.cpp
 *
 *  Copyright (c) 2016 Tobias Wood.
 *
 *  This Source Code Form is subject to the terms of the Mozilla Public
 *  License, v. 2.0. If a copy of the MPL was not distributed with this
 *  file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 */

#include <getopt.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "QI/Util.h"
#include "QI/Sequences/SteadyStateSequence.cpp"
#include "Filters/ApplyAlgorithmFilter.h"

using namespace std;
using namespace Eigen;

class ESAlgo : public Algorithm<complex<double>> {
protected:
    size_t m_size = 0;
    shared_ptr<QI::SSFP_GS> m_sequence = nullptr;
public:
    size_t numInputs() const override { return 1; }
    size_t numConsts() const override { return 1; }
    size_t numOutputs() const override { return 6; }
    const vector<string> & names() const {
        static vector<string> _names = {"M", "T1", "T2", "th", "a", "b"};
        return _names;
    }
    size_t dataSize() const override { return m_size; }
    void setSize(const size_t s) { m_size = s; }
    virtual TArray defaultConsts() override {
        // B1
        TArray def = TArray::Ones(1);
        return def;
    }
    void SetSequence(const shared_ptr<QI::SSFP_GS> &s) { m_sequence = s;}

    ArrayXd solveEig(const MatrixXd &A, const MatrixXd &B) const {
        RealQZ<MatrixXd> qz(A, B);
        VectorXd v = ArrayXd::Zero(A.cols());
        const MatrixXd &mS = qz.matrixS();
        const MatrixXd &mT = qz.matrixT();
        const MatrixXd &mZT = qz.matrixZ().transpose();
        int sInd = 0;
        double sVal = numeric_limits<double>::infinity();
        for (int i = 0; i < 6; i++) {
            const double a = mS.coeffRef(i,i);
            const double b = mT.coeffRef(i,i);
            const double l = fabs(a / b);
            if (l < sVal) {
                sVal = l;
                sInd = i;
            }
        }

        v(sInd) = 1.0;
        const double a = qz.matrixS().coeffRef(sInd,sInd);
        const double b = qz.matrixT().coeffRef(sInd,sInd);
        for (int j = sInd-1; j >= 0; j--) {
            const int st = j+1;
            const int sz = sInd-j;
            v.coeffRef(j) = -v.segment(st,sz).transpose().cwiseProduct(b*mS.block(j,st,1,sz) - a*mT.block(j,st,1,sz)).sum() / (b*mS.coeffRef(j,j) - a*mT.coeffRef(j,j));
        }
        v = (mZT * v).normalized();
        return v;
    }

    MatrixXd buildS(const ArrayXd &x, const ArrayXd &y) const {
        Matrix<double, Dynamic, 6> D(x.rows(), 6);
        D.col(0) = x*x;
        D.col(1) = x*y;
        D.col(2) = y*y;
        D.col(3) = x;
        D.col(4) = y;
        D.col(5).setConstant(1);
        return D.transpose() * D;
    }

    MatrixXd fitzC() const {
        typedef Matrix<double, 6, 6> Matrix6d;
        Matrix6d C = Matrix6d::Zero();
        // Fitzgibbon et al
        C(0,2) = -2; C(1,1) = 1; C(2,0) = -2;
        return C;
    }

    MatrixXd hyperC(const ArrayXd &x, const ArrayXd &y) const {
        typedef Matrix<double, 6, 6> Matrix6d;
        Matrix6d C = Matrix6d::Zero();
        // Fitzgibbon et al
        //C(0,2) = -2; C(1,1) = 1; C(2,0) = -2;

        // Hyper Ellipse
        const double N = x.cols();
        const double xc = x.sum() / N;
        const double yc = y.sum() / N;
        const double sx = x.square().sum() / N;
        const double sy = y.square().sum() / N;
        const double xy = (x * y).sum() / N;

        C << 6*sx, 6*xy, sx+sy, 6*xc, 2*yc, 1,
             6*xy, 4*(sx+sy), 6*xy, 4*yc, 4*xc, 0,
             sx + sy, 6*xy, 6*sy, 2*xc, 6*yc, 1,
             6*xc, 4*yc, 2*xc, 4, 0, 0,
             2*yc, 4*xc, 6*yc, 0, 4, 0,
             1, 0, 1, 0, 0, 0;

        return C;
    }

    void apply(const TInput &data, const TArray &inputs, TArray &outputs, TArray &resids, TIterations &its) const override
    {
        typedef Matrix<double, 6, 6> Matrix6d;
        typedef Matrix<double, 6, 1> Vector6d;
        const double B1 = inputs[0];
        const double scale = data.abs().maxCoeff();
        ArrayXd x = data.real() / scale;
        ArrayXd y = data.imag() / scale;

        MatrixXd S = buildS(x, y);
        Matrix6d C = hyperC(x, y);
        ArrayXd Z = solveEig(S, C);
        const double za = Z[0];
        const double zb = Z[1]/2;
        const double zc = Z[2];
        const double zd = Z[3]/2;
        const double zf = Z[4]/2;
        const double zg = Z[5];
        const double dsc=(zb*zb-za*zc);
        const double xc = (zc*zd-zb*zf)/dsc;
        const double yc = (za*zf-zb*zd)/dsc;
        const double th = atan2(yc,xc);
        double A = sqrt((2*(za*(zf*zf)+zc*(zd*zd)+zg*(zb*zb)-2*zb*zd*zf-za*zc*zg))/(dsc*(sqrt((za-zc)*(za-zc) + 4*zb*zb)-(za+zc))));
        double B = sqrt((2*(za*(zf*zf)+zc*(zd*zd)+zg*(zb*zb)-2*zb*zd*zf-za*zc*zg))/(dsc*(-sqrt((za-zc)*(za-zc) + 4*zb*zb)-(za+zc))));
        if (A > B) {
            std::swap(A, B);
        }
        //cout << "Z " << Z.transpose() << endl;
        //cout << "dsc " << dsc << " xc " << xc << " yc " << yc << " A " << A << " B " << B << endl;
        const double c = sqrt(xc*xc+yc*yc);
        const double b = (-c*A + sqrt(c*c*A*A - (c*c + B*B)*(A*A - B*B)))/(c*c + B*B);
        const double a = B / (b*B + c*sqrt(1-b*b));
        const double M = scale*c*(1-b*b)/(1-a*b);
        const double TR = m_sequence->TR();
        const double ca = cos(B1 * m_sequence->flip()[0]);
        const double T1 = -TR / (log(a*(1.+ca-a*b*ca)-b) - log(a*(1.+ca-a*b)-b*ca));
        const double T2 = -TR / log(a);

        outputs[0] = M;
        outputs[1] = T1;
        outputs[2] = T2;
        outputs[3] = th;
        outputs[4] = a;
        outputs[5] = b;
    }
};

//******************************************************************************
// Arguments / Usage
//******************************************************************************
const string usage {
"Usage is: qiesmap [options] input \n\
\n\
A utility for calculating T1,T2,PD and f0 maps from SSFP data.\n\
Input must be a single complex image with at least 6 phase increments.\n\
\n\
Options:\n\
    --help, -h       : Print this message.\n\
    --verbose, -v    : Print more information.\n\
    --out, -o path   : Specify an output prefix.\n\
    --mask, -m file  : Mask input with specified file.\n\
    --B1, -b file    : B1 Map file (ratio)\n\
    --threads, -T N  : Use N threads (default=hardware limit).\n"
};

bool verbose = false;
static size_t num_threads = 4;
static string outPrefix;
const struct option long_opts[] = {
    {"help", no_argument, 0, 'h'},
    {"verbose", no_argument, 0, 'v'},
    {"out", required_argument, 0, 'o'},
    {"mask", required_argument, 0, 'm'},
    {"B1", required_argument, 0, 'b'},
    {"threads", required_argument, 0, 'T'},
    {0, 0, 0, 0}
};
const char *short_opts = "hvo:m:b:T:";

//******************************************************************************
// Main
//******************************************************************************
int main(int argc, char **argv) {
    Eigen::initParallel();
    QI::VolumeF::Pointer mask = ITK_NULLPTR;
    QI::VolumeF::Pointer B1   = ITK_NULLPTR;

    shared_ptr<ESAlgo> algo = make_shared<ESAlgo>();
    int indexptr = 0, c;
    while ((c = getopt_long(argc, argv, short_opts, long_opts, &indexptr)) != -1) {
        switch (c) {
            case 'v': verbose = true; break;
            case 'm':
                if (verbose) cout << "Opening mask file " << optarg << endl;
                mask = QI::ReadImage(optarg);
                break;
            case 'o':
                outPrefix = optarg;
                if (verbose) cout << "Output prefix will be: " << outPrefix << endl;
                break;
            case 'b':
                if (verbose) cout << "Opening B1 file: " << optarg << endl;
                B1 = QI::ReadImage(optarg);
                break;
            case 'T':
                num_threads = stoi(optarg);
                if (num_threads == 0)
                    num_threads = std::thread::hardware_concurrency();
                break;
            case 'h':
                cout << QI::GetVersion() << endl << usage << endl;
                return EXIT_SUCCESS;
            case '?': // getopt will print an error message
                return EXIT_FAILURE;
            default:
                cout << "Unhandled option " << string(1, c) << endl;
                return EXIT_FAILURE;
        }
    }
    if ((argc - optind) != 1) {
        cout << "Incorrect number of arguments." << endl << usage << endl;
        return EXIT_FAILURE;
    }

    string inputFilename = argv[optind++];
    if (verbose) cout << "Opening file: " << inputFilename << endl;
    auto data = QI::ReadVectorImage<complex<float>>(inputFilename);
    shared_ptr<QI::SSFP_GS> seq = make_shared<QI::SSFP_GS>(cin, true);
    auto apply = itk::ApplyAlgorithmFilter<ESAlgo, complex<float>>::New();
    algo->setSize(data->GetNumberOfComponentsPerPixel());
    algo->SetSequence(seq);
    apply->SetAlgorithm(algo);
    apply->SetPoolsize(num_threads);
    apply->SetInput(0, data);

    if (mask)
        apply->SetMask(mask);
    if (B1)
        apply->SetConst(0, B1);
    if (verbose) {
        cout << "Processing" << endl;
        auto monitor = QI::GenericMonitor::New();
        apply->AddObserver(itk::ProgressEvent(), monitor);
    }
    apply->Update();
    if (verbose) {
        cout << "Elapsed time was " << apply->GetTotalTime() << "s" << endl;
        cout << "Mean time per voxel was " << apply->GetMeanTime() << "s" << endl;
        cout << "Writing results files." << endl;
    }
    outPrefix = outPrefix + "ES_";
    for (int i = 0; i < algo->numOutputs(); i++) {
        QI::WriteImage(apply->GetOutput(i), outPrefix + algo->names().at(i) + QI::OutExt());
    }

    if (verbose) cout << "Finished." << endl;
    return EXIT_SUCCESS;
}
<|endoftext|>