<commit_before>#include "compiler.h"
#include "generator.h"

#define BOOST_RESULT_OF_USE_DECLTYPE
#define BOOST_SPIRIT_USE_PHOENIX_V3

#include <algorithm>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/scope_exit.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/variant.hpp>
#include <iostream> // for std::cerr in parser
#include <ostream>
#include <stdexcept>
#include <string>
#include <vector>

// ----- Program structs -------------------------------------------------------
namespace bf {

namespace instruction {

struct function_call_t {
    std::string function_name;
};

struct variable_declaration_t {
    std::string variable_name;
    int         init_value;
};

struct print_variable_t {
    std::string variable_name;
};

struct print_text_t {
    std::string text;
};

struct scan_variable_t {
    std::string variable_name;
};

} // namespace bf::instruction

typedef boost::variant<
    instruction::function_call_t,
    instruction::variable_declaration_t,
    instruction::print_variable_t,
    instruction::print_text_t,
    instruction::scan_variable_t
> instruction_t;

struct function_t {
    std::string name;
    std::vector<std::string> parameters;
    std::vector<instruction_t> instructions;
};

typedef std::vector<function_t> program_t;

} // namespace bf

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::function_call_t,
        (std::string, function_name))

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::variable_declaration_t,
        (std::string, variable_name)
        (int,         init_value))

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::print_variable_t,
        (std::string, variable_name))

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::print_text_t,
        (std::string, text))

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::scan_variable_t,
        (std::string, variable_name))

BOOST_FUSION_ADAPT_STRUCT(
        bf::function_t,
        (std::string, name)
        (std::vector<std::string>, parameters)
        (std::vector<bf::instruction_t>, instructions))

// ----- Parser grammar --------------------------------------------------------
namespace bf {

namespace qi    = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;

#define KEYWORDS (qi::lit("function") | "var" | "print" | "scan")
template <typename iterator>
struct grammar : qi::grammar<iterator, program_t(), ascii::space_type> {
    grammar() : grammar::base_type(program) {
        program = *function;
        function = qi::lexeme["function"] > function_name
            > '(' > -(variable_name % ',') > ')'
            > '{' > *instruction > '}';
        function_name = qi::lexeme[+qi::alpha - KEYWORDS];
        variable_name = qi::lexeme[+qi::alpha - KEYWORDS];

        instruction = function_call
            | variable_declaration
            | print_variable
            | print_text
            | scan_variable;

        function_call        = function_name >> '(' > -(variable_name % ',') > ')' > ';';
        variable_declaration = qi::lexeme["var"] > variable_name > (('=' > qi::uint_) | qi::attr(0)) > ';';
        print_variable       = qi::lexeme["print"] >> variable_name > ';';
        print_text           = qi::lexeme["print"] >> qi::lexeme['"' > *(qi::char_ - '"') > '"'] > ';';
        scan_variable        = qi::lexeme["scan"] > variable_name > ';';

        program.name("program");
        function.name("function");
        function_name.name("function name");
        variable_name.name("variable name");
        instruction.name("instruction");
        function_call.name("function call");
        variable_declaration.name("variable declaration");
        print_variable.name("print variable");
        print_text.name("print text");
        scan_variable.name("scan variable");

        auto on_error = [](auto first, auto last, auto err, auto what) {
            std::string before(first, err);
            std::size_t bpos = before.find_last_of('\n');
            if (bpos != std::string::npos)
                before = before.substr(bpos + 1);

            std::string after(err, last);
            std::size_t apos = after.find_first_of('\n');
            if (apos != std::string::npos)
                after = after.substr(0, apos);

            std::cerr << "Error! Expecting " << what << " here:\n"
                << before << after << '\n'
                << std::string(before.size(), ' ') << '^'
                << std::endl;
        };

        qi::on_error<qi::fail>(program, boost::phoenix::bind(on_error, qi::_1, qi::_2, qi::_3, qi::_4));
    }

    qi::rule<iterator, program_t(),                           ascii::space_type> program;
    qi::rule<iterator, function_t(),                          ascii::space_type> function;
    qi::rule<iterator, std::string(),                         ascii::space_type> function_name;
    qi::rule<iterator, std::string(),                         ascii::space_type> variable_name;
    qi::rule<iterator, instruction_t(),                       ascii::space_type> instruction;

    qi::rule<iterator, instruction::function_call_t(),        ascii::space_type> function_call;
    qi::rule<iterator, instruction::variable_declaration_t(), ascii::space_type> variable_declaration;
    qi::rule<iterator, instruction::print_variable_t(),       ascii::space_type> print_variable;
    qi::rule<iterator, instruction::print_text_t(),           ascii::space_type> print_text;
    qi::rule<iterator, instruction::scan_variable_t(),        ascii::space_type> scan_variable;
};

// ----- Compilation algorithms ------------------------------------------------
program_t parse(const std::string &source) {
    // Build program struct from source input.
    grammar<decltype(source.begin())> g;
    program_t program;
    auto begin = source.begin(), end = source.end();
    const bool success = qi::phrase_parse(begin, end, g, ascii::space, program);

    if (!success || begin != end)
        throw std::logic_error("Parse unsuccessful!");

    // ----- Check program struct -----
    std::sort(program.begin(), program.end(),
            [](const function_t &f1, const function_t &f2) {return f1.name < f2.name;});
    // Ensure unique function names.
    if (program.size() > 1) {
        auto before_it = program.begin();
        for (auto it = before_it + 1; it != program.end(); ++before_it, ++it)
            if (before_it->name == it->name)
                throw std::logic_error("Function name used multiply times: " + it->name);
    }

    return program;
}

class instruction_visitor : public boost::static_visitor<void> {
public:
    instruction_visitor(const program_t &program) : m_program(program) {}

    void operator()(const instruction::function_call_t &i) {
        // Check if called function exists.
        auto function_it = std::find_if(m_program.begin(), m_program.end(),
                [&i](const function_t &f) {return f.name == i.function_name;});
        if (function_it == m_program.end())
            throw std::logic_error("Function not found: " + i.function_name);

        // Check for recursion (which is not supported).
        auto recursion_it = std::find(m_call_stack.begin(), m_call_stack.end(), i.function_name);
        if (recursion_it != m_call_stack.end()) {
            std::string call_stack_dump;
            for (const auto &function : m_call_stack)
                call_stack_dump += function + ", ";
            call_stack_dump += "(*) " + i.function_name;
            throw std::logic_error("Recursion not supported: " + call_stack_dump);
        }

        // Provide a clean scope for the called function.
        std::vector<std::map<std::string, generator::var_ptr>> scope_backup;
        std::swap(m_scope, scope_backup);
        m_scope.emplace_back();
        // TODO: Copy arguments
        // Restore old scope after the function returns.
        BOOST_SCOPE_EXIT(this_, &scope_backup) {
            std::swap(this_->m_scope, scope_backup);
        } BOOST_SCOPE_EXIT_END

        // Just for error report on recursion: Keep track of the call stack.
        m_call_stack.push_back(i.function_name);
        BOOST_SCOPE_EXIT(this_) {
            this_->m_call_stack.pop_back();
        } BOOST_SCOPE_EXIT_END

        // Finally, visit all instructions in the called function.
        for (const auto &instruction : function_it->instructions)
            boost::apply_visitor(*this, instruction);
    }

    void operator()(const instruction::variable_declaration_t &i) {
        auto it = m_scope.back().find(i.variable_name);
        if (it != m_scope.back().end())
            throw std::logic_error("Redeclaration of variable: " + i.variable_name);

        m_scope.back().emplace(i.variable_name, m_bfg.new_var(i.variable_name, i.init_value));
    }

    void operator()(const instruction::print_variable_t &i) {
        get_var(i.variable_name)->write_output();
    }

    void operator()(const instruction::print_text_t &i) {
        m_bfg.print(i.text);
    }

    void operator()(const instruction::scan_variable_t &i) {
        get_var(i.variable_name)->read_input();
    }

    const generator::var_ptr &get_var(const std::string &variable_name) const {
        for (auto scope_it = m_scope.rbegin(); scope_it != m_scope.rend(); ++scope_it) {
            auto it = scope_it->find(variable_name);
            if (it != scope_it->end())
                return it->second;
        }

        throw std::logic_error("Variable not declared in this scope: " + variable_name);
    }

    const generator &get_generator() const {
        return m_bfg;
    }

private:
    program_t                                              m_program; // TODO: just const ref?
    generator                                              m_bfg;
    std::vector<std::map<std::string, generator::var_ptr>> m_scope;
    std::vector<std::string>                               m_call_stack;
};

std::string generate(const program_t &program) {
    // As long as all function calls are inlined, this makes sense.
    instruction_visitor visitor(program);
    instruction::function_call_t call_to_main {"main"};
    instruction_t start = call_to_main;
    boost::apply_visitor(visitor, start);

    return visitor.get_generator().get_code();
}

std::string compiler::compile(const std::string &source) const {
    program_t program = parse(source);
    return generate(program);
}

} // namespace
<commit_msg>Switched to own SCOPE_EXIT implementation.<commit_after>#include "compiler.h"
#include "generator.h"

#define BOOST_RESULT_OF_USE_DECLTYPE
#define BOOST_SPIRIT_USE_PHOENIX_V3

#include <algorithm>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/scope_exit.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/variant.hpp>
#include <iostream> // for std::cerr in parser
#include <ostream>
#include <stdexcept>
#include <string>
#include <vector>

// ----- Scope_Exit (template from Andrei Alexandrescu) ------------------------
// https://www.youtube.com/watch?v=WjTrfoiB0MQ

#define CONCATENATE_IMPL(s1, s2) s1##s2
#define CONCATENATE(s1, s2) CONCATENATE_IMPL(s1, s2)

#ifdef __COUNTER__
#define ANONYMOUS_VARIABLE(str) CONCATENATE(str, __COUNTER__)
#else
#define ANONYMOUS_VARIABLE(str) CONCATENATE(str, __LINE__)
#endif

namespace detail {
    enum class scope_guard_helper {};

    template <typename fun>
    class scope_guard {
        fun m_fn;
    public:
        scope_guard(fun &&fn) : m_fn(std::forward<fun>(fn)) {}
        ~scope_guard() {m_fn();}
    };

    template <typename fun>
    scope_guard<fun> operator+(scope_guard_helper, fun &&fn) {
        return scope_guard<fun>(std::forward<fun>(fn));
    }
}

#define SCOPE_EXIT \
    auto ANONYMOUS_VARIABLE(SCOPE_EXIT_STATE) \
        = ::detail::scope_guard_helper() + [&]()

// ----- Program structs -------------------------------------------------------
namespace bf {

namespace instruction {

struct function_call_t {
    std::string function_name;
};

struct variable_declaration_t {
    std::string variable_name;
    int         init_value;
};

struct print_variable_t {
    std::string variable_name;
};

struct print_text_t {
    std::string text;
};

struct scan_variable_t {
    std::string variable_name;
};

} // namespace bf::instruction

typedef boost::variant<
    instruction::function_call_t,
    instruction::variable_declaration_t,
    instruction::print_variable_t,
    instruction::print_text_t,
    instruction::scan_variable_t
> instruction_t;

struct function_t {
    std::string name;
    std::vector<std::string> parameters;
    std::vector<instruction_t> instructions;
};

typedef std::vector<function_t> program_t;

} // namespace bf

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::function_call_t,
        (std::string, function_name))

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::variable_declaration_t,
        (std::string, variable_name)
        (int,         init_value))

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::print_variable_t,
        (std::string, variable_name))

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::print_text_t,
        (std::string, text))

BOOST_FUSION_ADAPT_STRUCT(
        bf::instruction::scan_variable_t,
        (std::string, variable_name))

BOOST_FUSION_ADAPT_STRUCT(
        bf::function_t,
        (std::string, name)
        (std::vector<std::string>, parameters)
        (std::vector<bf::instruction_t>, instructions))

// ----- Parser grammar --------------------------------------------------------
namespace bf {

namespace qi    = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;

#define KEYWORDS (qi::lit("function") | "var" | "print" | "scan")
template <typename iterator>
struct grammar : qi::grammar<iterator, program_t(), ascii::space_type> {
    grammar() : grammar::base_type(program) {
        program = *function;
        function = qi::lexeme["function"] > function_name
            > '(' > -(variable_name % ',') > ')'
            > '{' > *instruction > '}';
        function_name = qi::lexeme[+qi::alpha - KEYWORDS];
        variable_name = qi::lexeme[+qi::alpha - KEYWORDS];

        instruction = function_call
            | variable_declaration
            | print_variable
            | print_text
            | scan_variable;

        function_call        = function_name >> '(' > -(variable_name % ',') > ')' > ';';
        variable_declaration = qi::lexeme["var"] > variable_name > (('=' > qi::uint_) | qi::attr(0)) > ';';
        print_variable       = qi::lexeme["print"] >> variable_name > ';';
        print_text           = qi::lexeme["print"] >> qi::lexeme['"' > *(qi::char_ - '"') > '"'] > ';';
        scan_variable        = qi::lexeme["scan"] > variable_name > ';';

        program.name("program");
        function.name("function");
        function_name.name("function name");
        variable_name.name("variable name");
        instruction.name("instruction");
        function_call.name("function call");
        variable_declaration.name("variable declaration");
        print_variable.name("print variable");
        print_text.name("print text");
        scan_variable.name("scan variable");

        auto on_error = [](auto first, auto last, auto err, auto what) {
            std::string before(first, err);
            std::size_t bpos = before.find_last_of('\n');
            if (bpos != std::string::npos)
                before = before.substr(bpos + 1);

            std::string after(err, last);
            std::size_t apos = after.find_first_of('\n');
            if (apos != std::string::npos)
                after = after.substr(0, apos);

            std::cerr << "Error! Expecting " << what << " here:\n"
                << before << after << '\n'
                << std::string(before.size(), ' ') << '^'
                << std::endl;
        };

        qi::on_error<qi::fail>(program, boost::phoenix::bind(on_error, qi::_1, qi::_2, qi::_3, qi::_4));
    }

    qi::rule<iterator, program_t(),                           ascii::space_type> program;
    qi::rule<iterator, function_t(),                          ascii::space_type> function;
    qi::rule<iterator, std::string(),                         ascii::space_type> function_name;
    qi::rule<iterator, std::string(),                         ascii::space_type> variable_name;
    qi::rule<iterator, instruction_t(),                       ascii::space_type> instruction;

    qi::rule<iterator, instruction::function_call_t(),        ascii::space_type> function_call;
    qi::rule<iterator, instruction::variable_declaration_t(), ascii::space_type> variable_declaration;
    qi::rule<iterator, instruction::print_variable_t(),       ascii::space_type> print_variable;
    qi::rule<iterator, instruction::print_text_t(),           ascii::space_type> print_text;
    qi::rule<iterator, instruction::scan_variable_t(),        ascii::space_type> scan_variable;
};

// ----- Compilation algorithms ------------------------------------------------
program_t parse(const std::string &source) {
    // Build program struct from source input.
    grammar<decltype(source.begin())> g;
    program_t program;
    auto begin = source.begin(), end = source.end();
    const bool success = qi::phrase_parse(begin, end, g, ascii::space, program);

    if (!success || begin != end)
        throw std::logic_error("Parse unsuccessful!");

    // ----- Check program struct -----
    std::sort(program.begin(), program.end(),
            [](const function_t &f1, const function_t &f2) {return f1.name < f2.name;});
    // Ensure unique function names.
    if (program.size() > 1) {
        auto before_it = program.begin();
        for (auto it = before_it + 1; it != program.end(); ++before_it, ++it)
            if (before_it->name == it->name)
                throw std::logic_error("Function name used multiply times: " + it->name);
    }

    return program;
}

class instruction_visitor : public boost::static_visitor<void> {
public:
    instruction_visitor(const program_t &program) : m_program(program) {}

    void operator()(const instruction::function_call_t &i) {
        // Check if called function exists.
        auto function_it = std::find_if(m_program.begin(), m_program.end(),
                [&i](const function_t &f) {return f.name == i.function_name;});
        if (function_it == m_program.end())
            throw std::logic_error("Function not found: " + i.function_name);

        // Check for recursion (which is not supported).
        auto recursion_it = std::find(m_call_stack.begin(), m_call_stack.end(), i.function_name);
        if (recursion_it != m_call_stack.end()) {
            std::string call_stack_dump;
            for (const auto &function : m_call_stack)
                call_stack_dump += function + ", ";
            call_stack_dump += "(*) " + i.function_name;
            throw std::logic_error("Recursion not supported: " + call_stack_dump);
        }

        // Provide a clean scope for the called function.
        std::vector<std::map<std::string, generator::var_ptr>> scope_backup;
        std::swap(m_scope, scope_backup);
        m_scope.emplace_back();
        // TODO: Copy arguments
        // Restore old scope after the function returns.
        SCOPE_EXIT {std::swap(m_scope, scope_backup);};

        // Just for error report on recursion: Keep track of the call stack.
        m_call_stack.push_back(i.function_name);
        SCOPE_EXIT {m_call_stack.pop_back();};

        // Finally, visit all instructions in the called function.
        for (const auto &instruction : function_it->instructions)
            boost::apply_visitor(*this, instruction);
    }

    void operator()(const instruction::variable_declaration_t &i) {
        auto it = m_scope.back().find(i.variable_name);
        if (it != m_scope.back().end())
            throw std::logic_error("Redeclaration of variable: " + i.variable_name);

        m_scope.back().emplace(i.variable_name, m_bfg.new_var(i.variable_name, i.init_value));
    }

    void operator()(const instruction::print_variable_t &i) {
        get_var(i.variable_name)->write_output();
    }

    void operator()(const instruction::print_text_t &i) {
        m_bfg.print(i.text);
    }

    void operator()(const instruction::scan_variable_t &i) {
        get_var(i.variable_name)->read_input();
    }

    const generator::var_ptr &get_var(const std::string &variable_name) const {
        for (auto scope_it = m_scope.rbegin(); scope_it != m_scope.rend(); ++scope_it) {
            auto it = scope_it->find(variable_name);
            if (it != scope_it->end())
                return it->second;
        }

        throw std::logic_error("Variable not declared in this scope: " + variable_name);
    }

    const generator &get_generator() const {
        return m_bfg;
    }

private:
    program_t                                              m_program; // TODO: just const ref?
    generator                                              m_bfg;
    std::vector<std::map<std::string, generator::var_ptr>> m_scope;
    std::vector<std::string>                               m_call_stack;
};

std::string generate(const program_t &program) {
    // As long as all function calls are inlined, this makes sense.
    instruction_visitor visitor(program);
    instruction::function_call_t call_to_main {"main"};
    instruction_t start = call_to_main;
    boost::apply_visitor(visitor, start);

    return visitor.get_generator().get_code();
}

std::string compiler::compile(const std::string &source) const {
    program_t program = parse(source);
    return generate(program);
}

} // namespace
<|endoftext|>


<commit_before>#include "player.h"

#include "game_system.h"
#include "bullet.h"
#include "utils.h"
#include "texture_manager.h"

#include <iostream>

namespace vot
{
    Player::Player(const sf::Texture &texture) :
        Character(texture),
        _cooldown(0.0f),
        _homing_cooldown(0.0f),
        _look_at_target(false),
        _auto_target(true),
        _target(nullptr),
        _powerup_hitbox(20.0f)
    {

    }

    void Player::init()
    {
        add_hardpoint_placement(new HardpointPlacement(0, 8, 225, 315));
        add_hardpoint_placement(new HardpointPlacement(-16, 4, 180, 300));
        add_hardpoint_placement(new HardpointPlacement(16, 4, 240, 0));
        add_hardpoint_placement(new HardpointPlacement(20, 10, 270, 45));
        add_hardpoint_placement(new HardpointPlacement(-20, 10, 75, 270));

        create_new_hardpoint(Powerup::BULLET);

        add_thruster_placement(28, 0, 0, 0.35f);
        add_thruster_placement(28, 0, 180, 0.35f);
        add_thruster_placement(-28, 0, 180, 0.35f);
        add_thruster_placement(-28, 0, 0, 0.35f);

        add_thruster_placement(-8, 14, 180, 0.5f);
        add_thruster_placement(8, 14, 180, 0.5f);
        add_thruster_placement(8, -8, 0, 0.35f);
        add_thruster_placement(-8, -8, 0, 0.35f);

        add_thruster_placement(-28, 3, 270, 0.35f);
        add_thruster_placement(28, 3, 90, 0.35f);
    }

    void Player::update(float dt)
    {
        auto gs = GameSystem::main();

        // Keyboard input {{{
        sf::Vector2f acc;
        float rot_acc = 0.0f;

        if (sf::Keyboard::isKeyPressed(sf::Keyboard::A))
        {
            acc.x -= strafe_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::D))
        {
            acc.x += strafe_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::S))
        {
            acc.y += backwards_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::W))
        {
            acc.y -= forwards_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Q))
        {
            rot_acc -= rot_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::E))
        {
            rot_acc += rot_speed();
        }

        acceleration(acc);
        rot_acceleration(rot_acc);

        if (gs->is_key_pressed(sf::Keyboard::T))
        {
            if (sf::Keyboard::isKeyPressed(sf::Keyboard::LShift))
            {
                _auto_target = !_auto_target;
            }
            else
            {
                target(gs->next_target(_target));
            }
        }
        if (gs->is_key_pressed(sf::Keyboard::Z))
        {
            _look_at_target = !_look_at_target;
        }
        // }}}

        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Space))
        {
            auto points = hardpoints();
            for (auto i = 0u; i < points->size(); i++)
            {
                points->at(i)->fire();
            }
        }

        /* {{{
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Space))
        {
            if (_cooldown <= 0.0f)
            {
                _cooldown = 0.3f;
                auto bullet_level = _powerups[Powerup::BULLET];

                if (bullet_level == 0)
                {
                    auto bullet = spawn_pattern_bullet("player_bullet_small", 2u);
                    auto trans = forward_center_trans();
                    bullet->init_transform(trans);
                }
                else
                {
                    spawn_pattern_bullet_pair("player_bullet_small", 2u, 4.0f, 0.0f);
                    if (bullet_level > 1)
                    {
                        spawn_pattern_bullet_pair("player_bullet_small", 2u, 30.0f, -20.0f);
                        _cooldown = 0.25f;
                    }
                    if (bullet_level > 2)
                    {
                        spawn_pattern_bullet_pair("player_bullet_small", 2u, 34.0f, -20.0f);
                        _cooldown = 0.20f;
                    }
                    if (bullet_level > 3)
                    {
                        _cooldown = 0.15f;
                    }
                }
            }

            auto homing_level = _powerups[Powerup::HOMING];
            if (_homing_cooldown <= 0.0f && homing_level > 0)
            {
                _homing_cooldown = 0.75f;
                if (homing_level == 1)
                {
                    auto homing_bullet = spawn_homing_bullet();
                    auto angle = rotation() - 90.0f;
                    homing_bullet->setup(location(), angle);
                    homing_bullet->target(_target);
                }
                else
                {
                    spawn_homing_bullet_pair(80.0f);

                    if (homing_level == 3 || homing_level > 4)
                    {
                        auto homing_bullet = spawn_homing_bullet();
                        auto angle = rotation() - 90.0f;
                        homing_bullet->setup(location(), angle);
                        homing_bullet->target(_target);
                    }
                    if (homing_level > 3)
                    {
                        spawn_homing_bullet_pair(100.0f);
                        _homing_cooldown = 0.7f;
                    }
                    if (homing_level > 5)
                    {
                        _homing_cooldown = 0.65f;
                    }
                }
            }
        }
        }}}*/

        if (_look_at_target && _target != nullptr)
        {
            auto rot_spd = rot_speed();
            auto angles = Utils::calculate_angles(getPosition(), _target->getPosition(), rotation(), -90.0f);
            rot_spd *= dt;
            if (angles.delta_angle() < rot_spd && angles.delta_angle() > -rot_spd)
            {
                rotation(angles.to_angle());
            }
            else
            {
                rotateBy(angles.delta_angle() > 0 ? -rot_spd : rot_spd);
            }
        }

        _cooldown -= dt;
        _homing_cooldown -= dt;

        Character::update(dt);

        _powerup_hitbox.location(getPosition());
    }

    void Player::target(Enemy *value)
    {
        _target = value;

        auto points = hardpoints();
        for (auto i = 0u; i < points->size(); i++)
        {
            points->at(i)->target(value);
        }
    }
    Enemy *Player::target() const
    {
        return _target;
    }

    bool Player::looking_at_target() const
    {
        return _look_at_target;
    }
    bool Player::auto_target() const
    {
        return _auto_target;
    }

    Circle &Player::powerup_hitbox()
    {
        return _powerup_hitbox;
    }

    void Player::add_powerup(const Powerup &powerup)
    {
        create_new_hardpoint(powerup.type());
    }
    void Player::create_new_hardpoint(Powerup::Type type)
    {
        HardpointPlacement *empty_placement = nullptr;
        for (auto i = 0u; i < _hardpoint_placements.size(); i++)
        {
            auto placement = _hardpoint_placements[i].get();
            if (placement->hardpoint() == nullptr)
            {
                empty_placement = placement;
                break;
            }
        }

        if (empty_placement == nullptr)
        {
            return;
        }

        if (type == Powerup::BULLET)
        {
            auto pattern_bullet = GameSystem::main()->bullet_manager().find_src_pattern_bullet("player_bullet_small");
            auto pattern_turret = new PatternBulletHardpoint(*pattern_bullet, Group::PLAYER);
            auto bullet_turret_texture = TextureManager::texture("bullet_turret");
            pattern_turret->texture(bullet_turret_texture);
            pattern_turret->target(_target);

            empty_placement->hardpoint(pattern_turret);
            add_hardpoint(pattern_turret);
        }
        if (type == Powerup::BEAM)
        {
            auto beam_blueprint = GameSystem::main()->beam_manager().find_src_beam("beam1");
            auto beam_turret = new BeamHardpoint(*beam_blueprint, Group::PLAYER);

            auto beam_turret_texture = TextureManager::texture("beam_turret");
            beam_turret->texture(beam_turret_texture);
            beam_turret->target(_target);

            empty_placement->hardpoint(beam_turret);
            add_hardpoint(beam_turret);
        }
        if (type == Powerup::HOMING)
        {
            auto homing_blueprint = GameSystem::main()->bullet_manager().find_src_homing_bullet("homing_blue");
            auto homing_turret = new HomingBulletHardpoint(*homing_blueprint, Group::PLAYER);

            auto bullet_turret_texture = TextureManager::texture("bullet_turret");
            homing_turret->texture(bullet_turret_texture);
            homing_turret->target(_target);

            empty_placement->hardpoint(homing_turret);
            add_hardpoint(homing_turret);
        }
    }

    void Player::add_hardpoint_placement(HardpointPlacement *placement)
    {
        _hardpoint_placements.push_back(std::unique_ptr<HardpointPlacement>(placement));
    }
    void Player::add_thruster_placement(float x, float y, float rotation, float size)
    {
        auto thruster = new Thruster();
        thruster->setPosition(x, y);
        thruster->setRotation(rotation);
        thruster->thrust_size(size);
        add_thruster(thruster);
    }
}
<commit_msg>- Working on making look at target work with more physics-y system.<commit_after>#include "player.h"

#include "game_system.h"
#include "bullet.h"
#include "utils.h"
#include "texture_manager.h"

#include <iostream>

namespace vot
{
    Player::Player(const sf::Texture &texture) :
        Character(texture),
        _cooldown(0.0f),
        _homing_cooldown(0.0f),
        _look_at_target(false),
        _auto_target(true),
        _target(nullptr),
        _powerup_hitbox(20.0f)
    {

    }

    void Player::init()
    {
        add_hardpoint_placement(new HardpointPlacement(0, 8, 225, 315));
        add_hardpoint_placement(new HardpointPlacement(-16, 4, 180, 300));
        add_hardpoint_placement(new HardpointPlacement(16, 4, 240, 0));
        add_hardpoint_placement(new HardpointPlacement(20, 10, 270, 45));
        add_hardpoint_placement(new HardpointPlacement(-20, 10, 75, 270));

        create_new_hardpoint(Powerup::BULLET);

        add_thruster_placement(28, 0, 0, 0.35f);
        add_thruster_placement(28, 0, 180, 0.35f);
        add_thruster_placement(-28, 0, 180, 0.35f);
        add_thruster_placement(-28, 0, 0, 0.35f);

        add_thruster_placement(-8, 14, 180, 0.5f);
        add_thruster_placement(8, 14, 180, 0.5f);
        add_thruster_placement(8, -8, 0, 0.35f);
        add_thruster_placement(-8, -8, 0, 0.35f);

        add_thruster_placement(-28, 3, 270, 0.35f);
        add_thruster_placement(28, 3, 90, 0.35f);
    }

    void Player::update(float dt)
    {
        auto gs = GameSystem::main();

        // Keyboard input {{{
        sf::Vector2f acc;
        float rot_acc = 0.0f;

        if (sf::Keyboard::isKeyPressed(sf::Keyboard::A))
        {
            acc.x -= strafe_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::D))
        {
            acc.x += strafe_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::S))
        {
            acc.y += backwards_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::W))
        {
            acc.y -= forwards_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Q))
        {
            rot_acc -= rot_speed();
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::E))
        {
            rot_acc += rot_speed();
        }

        if (gs->is_key_pressed(sf::Keyboard::T))
        {
            if (sf::Keyboard::isKeyPressed(sf::Keyboard::LShift))
            {
                _auto_target = !_auto_target;
            }
            else
            {
                target(gs->next_target(_target));
            }
        }
        if (gs->is_key_pressed(sf::Keyboard::Z))
        {
            _look_at_target = !_look_at_target;
        }
        // }}}

        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Space))
        {
            auto points = hardpoints();
            for (auto i = 0u; i < points->size(); i++)
            {
                points->at(i)->fire();
            }
        }

        if (_look_at_target && _target != nullptr)
        {
            auto rot_spd = rot_speed();
            auto angles = Utils::calculate_angles(getPosition(), _target->getPosition(), rotation(), -90.0f);
            rot_spd *= dt;
            if (angles.delta_angle() < rot_spd && angles.delta_angle() > -rot_spd)
            {
                rotation(angles.to_angle());
                //rot_acc -= angles.delta_angle();
            }
            else
            {
                rot_acc += (angles.delta_angle() > 0) ? -rot_speed() : rot_speed();
                //rotateBy(angles.delta_angle() > 0 ? -rot_spd : rot_spd);
            }
        }

        acceleration(acc);
        rot_acceleration(rot_acc);

        _cooldown -= dt;
        _homing_cooldown -= dt;

        Character::update(dt);

        _powerup_hitbox.location(getPosition());
    }

    void Player::target(Enemy *value)
    {
        _target = value;

        auto points = hardpoints();
        for (auto i = 0u; i < points->size(); i++)
        {
            points->at(i)->target(value);
        }
    }
    Enemy *Player::target() const
    {
        return _target;
    }

    bool Player::looking_at_target() const
    {
        return _look_at_target;
    }
    bool Player::auto_target() const
    {
        return _auto_target;
    }

    Circle &Player::powerup_hitbox()
    {
        return _powerup_hitbox;
    }

    void Player::add_powerup(const Powerup &powerup)
    {
        create_new_hardpoint(powerup.type());
    }
    void Player::create_new_hardpoint(Powerup::Type type)
    {
        HardpointPlacement *empty_placement = nullptr;
        for (auto i = 0u; i < _hardpoint_placements.size(); i++)
        {
            auto placement = _hardpoint_placements[i].get();
            if (placement->hardpoint() == nullptr)
            {
                empty_placement = placement;
                break;
            }
        }

        if (empty_placement == nullptr)
        {
            return;
        }

        if (type == Powerup::BULLET)
        {
            auto pattern_bullet = GameSystem::main()->bullet_manager().find_src_pattern_bullet("player_bullet_small");
            auto pattern_turret = new PatternBulletHardpoint(*pattern_bullet, Group::PLAYER);
            auto bullet_turret_texture = TextureManager::texture("bullet_turret");
            pattern_turret->texture(bullet_turret_texture);
            pattern_turret->target(_target);

            empty_placement->hardpoint(pattern_turret);
            add_hardpoint(pattern_turret);
        }
        if (type == Powerup::BEAM)
        {
            auto beam_blueprint = GameSystem::main()->beam_manager().find_src_beam("beam1");
            auto beam_turret = new BeamHardpoint(*beam_blueprint, Group::PLAYER);

            auto beam_turret_texture = TextureManager::texture("beam_turret");
            beam_turret->texture(beam_turret_texture);
            beam_turret->target(_target);

            empty_placement->hardpoint(beam_turret);
            add_hardpoint(beam_turret);
        }
        if (type == Powerup::HOMING)
        {
            auto homing_blueprint = GameSystem::main()->bullet_manager().find_src_homing_bullet("homing_blue");
            auto homing_turret = new HomingBulletHardpoint(*homing_blueprint, Group::PLAYER);

            auto bullet_turret_texture = TextureManager::texture("bullet_turret");
            homing_turret->texture(bullet_turret_texture);
            homing_turret->target(_target);

            empty_placement->hardpoint(homing_turret);
            add_hardpoint(homing_turret);
        }
    }

    void Player::add_hardpoint_placement(HardpointPlacement *placement)
    {
        _hardpoint_placements.push_back(std::unique_ptr<HardpointPlacement>(placement));
    }
    void Player::add_thruster_placement(float x, float y, float rotation, float size)
    {
        auto thruster = new Thruster();
        thruster->setPosition(x, y);
        thruster->setRotation(rotation);
        thruster->thrust_size(size);
        add_thruster(thruster);
    }
}
<|endoftext|>


<commit_before>// g++ -Wall -Wextra sver.cpp -lopencv_core -shared -fPIC -O2 -ffast-math -o sver.so

/*
keypoints of form (x, y, a, c, d, theta)
each represents "invVR" matrix =
/     \     /                         \
|a 0 x|     |cos(theta) -sin(theta)  0|
|c d y|  *  |sin(theta)  cos(theta)  0|
|0 0 1|     |         0           0  1|
\     /     \                         /

"kpts" parameters are keypoint lists
-----
fm == feature_matches :: [(Int, Int)]
indices into kpts{1,2} indicating a match
*/
#include <cmath>
#include <cstdio>
#include <opencv2/core/core.hpp>
#include <vector>

// References:
// http://stackoverflow.com/questions/487108/how-to-supress-specific-warnings-in-g
// suppress unused warnings of specific variables
#define MARKUSED(X)  ((void)(&(X)))

using cv::Matx;
using std::vector;

// adapted from hesaff's helpers.h
#ifndef M_TAU
#define M_TAU 6.28318
#endif
template <class T> T ensure_0toTau(T x)
{
    if(x < 0) { return ensure_0toTau(x+M_TAU); }
    else if(x >= M_TAU) { return ensure_0toTau(x-M_TAU); }
    else { return x; }
}

template<typename T> Matx<T, 3, 3> get_invV_mat(T x, T y, T a, T c, T d, T theta) {
    T ct = (T)cos(theta), st = (T)sin(theta);
    // https://github.com/aweinstock314/haskell-stuff/blob/master/ExpressionSimplifier.hs
    return Matx<T, 3, 3>(
             a*ct,      a*(-st),   x,
        c*ct+d*st, c*(-st)+d*ct,   y,
              0.0,        0.0, 1.0);
}

void debug_print_kpts(double* kpts, size_t kpts_len) {
    for(size_t i=0; i<kpts_len*6; i+=6) {
        printf("kpts[%lu]: [%f, %f, %f, %f, %f, %f]\n", i/6,
            kpts[i+0], kpts[i+1], kpts[i+2],
            kpts[i+3], kpts[i+4], kpts[i+5]);
    }
}

void debug_print_mat3x3(const char* name, const Matx<double, 3, 3>& mat) {
    printf("%s: [[%f, %f, %f], [%f, %f, %f], [%f, %f, %f]]\n", name,
        mat(0, 0), mat(0, 1), mat(0, 2),
        mat(1, 0), mat(1, 1), mat(1, 2),
        mat(2, 0), mat(2, 1), mat(2, 2));
}

template<typename T> inline T xy_distance(const Matx<T, 3, 3>& kpt1, const Matx<T, 3, 3>& kpt2) {
    // ktool.get_invVR_mats_xys
    T x1 = kpt1(0, 2), y1 = kpt1(1, 2);
    T x2 = kpt2(0, 2), y2 = kpt2(1, 2);
    // ltool.L2_sqrd
    T dx = x2-x1, dy = y2-y1;
    return dx*dx + dy*dy;
}

template<typename T> inline T det_distance(const Matx<T, 3, 3>& kpt1, const Matx<T, 3, 3>& kpt2) {
    // ktool.get_invVR_mats_sqrd_scale
    T a1 = kpt1(0, 0), b1 = kpt1(0, 1), c1 = kpt1(1, 0), d1 = kpt1(1, 1);
    T a2 = kpt2(0, 0), b2 = kpt2(0, 1), c2 = kpt2(1, 0), d2 = kpt2(1, 1);
    T det1 = a1*d1 - b1*c1, det2 = a2*d2 - b2*c2;
    // ltool.det_distance
    T dist = det1/det2;
    if(dist < 1) { dist = 1/dist; }
    return dist;
}

template<typename T> inline T ori_distance(const Matx<T, 3, 3>& kpt1, const Matx<T, 3, 3>& kpt2) {
    // ktool.get_invVR_mats_oris
    T a1 = kpt1(0, 0), b1 = kpt1(0, 1);
    T a2 = kpt2(0, 0), b2 = kpt2(0, 1);
    T ori1 = ensure_0toTau(-atan2(b1, a1));
    T ori2 = ensure_0toTau(-atan2(b2, a2));
    // ltool.ori_distance
    T delta = fabs(ori1 - ori2);
    delta = ensure_0toTau(delta);
    return std::min(delta, M_TAU-delta);
}

extern "C" {
    void get_affine_inliers(double* kpts1, size_t kpts1_len,
                    double* kpts2, size_t kpts2_len,
                    size_t* fm, size_t fm_len,
                    double xy_thresh_sqrd, double scale_thresh_sqrd, double ori_thresh,
                    // memory is expected to by allocated by the caller (i.e. via numpy.empty)
                    bool* out_inlier_flags, double* out_errors_list, double* out_matrices_list)
    {

        MARKUSED(kpts1_len);
        MARKUSED(kpts2_len);
//#define RUNTIME_BOUNDS_CHECKING
#ifdef RUNTIME_BOUNDS_CHECKING
        for(size_t fm_ind = 0; fm_ind < fm_len; fm_ind += 2) {
            if((fm[fm_ind] >= kpts1_len) || (fm[fm_ind + 1] >= kpts2_len))
            {
                puts("bad fm indexes");
                return;
            }
        }
#endif
// remove some redundancy in a possibly-ugly way
#define SETUP_RELEVANT_VARIABLES \
double* kpt1 = &kpts1[6*fm[fm_ind+0]]; \
double* kpt2 = &kpts2[6*fm[fm_ind+1]]; \
Matx<double, 3, 3> invVR1_m = get_invV_mat( \
    kpt1[0], kpt1[1], kpt1[2], \
    kpt1[3], kpt1[4], kpt1[5]); \
Matx<double, 3, 3> invVR2_m = get_invV_mat( \
    kpt2[0], kpt2[1], kpt2[2], \
    kpt2[3], kpt2[4], kpt2[5]);
        //vector<Matx<double, 3, 3> > Aff_mats;
// MATRIX_REF(i) should be the same as Aff_mats[i], but
//  directly operating on the numpy-allocated memory
//   (less allocation == faster code)
#define MATRIX_REF(i) (*((i)+((Matx<double, 3, 3>*)out_matrices_list)))
        //vector<vector<double> > xy_errs, scale_errs, ori_errs;
        for(size_t fm_ind = 0; fm_ind < fm_len; fm_ind += 2) {
            SETUP_RELEVANT_VARIABLES
            Matx<double, 3, 3> V1_m = invVR1_m.inv();
            Matx<double, 3, 3> Aff_mat = invVR2_m * V1_m;
            //Aff_mats.push_back(Aff_mat);
            MATRIX_REF(fm_ind/2) = Aff_mat;
        }
        const size_t num_matches = fm_len/2;
        for(size_t i = 0; i < num_matches; i++) {
            //xy_errs.push_back(vector<double>());
            //scale_errs.push_back(vector<double>());
            //ori_errs.push_back(vector<double>());
            for(size_t fm_ind = 0; fm_ind < fm_len; fm_ind += 2) {
                SETUP_RELEVANT_VARIABLES
                Matx<double, 3, 3> Aff_mat = MATRIX_REF(i);
                // _test_hypothesis_inliers
                Matx<double, 3, 3> invVR1_mt = Aff_mat * invVR1_m;
                double xy_err = xy_distance(invVR1_mt, invVR2_m);
                double scale_err = det_distance(invVR1_mt, invVR2_m);
                double ori_err = ori_distance(invVR1_mt, invVR2_m);
                //xy_errs[i].push_back(xy_err);
                //scale_errs[i].push_back(scale_err);
                //ori_errs[i].push_back(ori_err);
// poke the error values directly into the output array with pointer voodoo to
//  avoid intermediate allocations (the explicit structure is shown by the
//   commented xy_errs, scale_errs, and ori_errs variables).
#define PACKED_INSERT(OFFSET, VAR) \
*(out_errors_list+(num_matches*3*i)+(num_matches*(OFFSET))+(fm_ind/2)) = (VAR)
                PACKED_INSERT(0, xy_err);
                PACKED_INSERT(1, ori_err);
                PACKED_INSERT(2, scale_err);
#undef PACKED_INSERT
                bool is_inlier = (xy_err < xy_thresh_sqrd) &&
                                 (scale_err < scale_thresh_sqrd) &&
                                 (ori_err < ori_thresh);
                *(out_inlier_flags+(num_matches*i)+(fm_ind/2)) = is_inlier;
                //printf("errs[%u][%u]: %f, %f, %f\n", fm_ind, i, xy_err, scale_err, ori_err);
            }
        }
#undef MATRIX_REF
#undef SETUP_RELEVANT_VARIABLES
/*
#define SHOW_ERRVEC(vec) \
for(size_t i = 0; i < vec.size(); i++) { \
    putchar('['); \
    for(size_t j = 0; j < vec[i].size(); j++) { \
        printf("%f, ", vec[i][j]); \
    } \
    puts("]"); \
}
        puts("-----");
        SHOW_ERRVEC(xy_errs)
        puts("-----");
        SHOW_ERRVEC(scale_errs)
        puts("-----");
        SHOW_ERRVEC(ori_errs)
        puts("-----");
#undef SHOW_ERRVEC
*/
        // Code for copying Aff_mats into the output is redundant now
        //  that the output is operated on directly (via MATRIX_REF)
        /*
        //printf("%lu\n", Aff_mats.size());
        for(size_t i = 0; i < Aff_mats.size(); i++) {
            const size_t mat_size = 3*3*sizeof(double);
            //char msg[] = {'M', 'a', 't', 0x30+i%10, 0};
            //debug_print_mat3x3(msg, Aff_mats[i]);
            double* dest = (out_matrices_list+(3*3*i));
            //char* destc = (char*)(out_matrices_list+(3*3*i));
            //printf("%x\n", dest);
            //printf("before: "); for(size_t j=0; j < 9; j++) {printf("%f ", *(dest+j)); }
            //printf("\nbefore: "); for(size_t j=0; j < mat_size; j+=8) {printf("0x%08x ", *(destc+j)); }
            memcpy(dest, &Aff_mats[i], mat_size);
            //printf("\nafter: "); for(size_t j=0; j < 9; j++) {printf("%f ", *(dest+j)); }
            //printf("\nafter: "); for(size_t j=0; j < mat_size; j+=8) {printf("0x%08x ", *(destc+j)); }
            //puts("\n");
        }
        */
    }

    void hello_world() {
        puts("Hello from C++!");
    }
}
<commit_msg>Factored out CHECK_FM_BOUNDS and get_Aff_mat from get_affine_inliers in sver.cpp (in preperation to use them in get_best_affine_inliers).<commit_after>// g++ -Wall -Wextra sver.cpp -lopencv_core -shared -fPIC -O2 -ffast-math -o sver.so

/*
keypoints of form (x, y, a, c, d, theta)
each represents "invVR" matrix =
/     \     /                         \
|a 0 x|     |cos(theta) -sin(theta)  0|
|c d y|  *  |sin(theta)  cos(theta)  0|
|0 0 1|     |         0           0  1|
\     /     \                         /

"kpts" parameters are keypoint lists
-----
fm == feature_matches :: [(Int, Int)]
indices into kpts{1,2} indicating a match
*/
#include <cmath>
#include <cstdio>
#include <opencv2/core/core.hpp>
#include <vector>

// References:
// http://stackoverflow.com/questions/487108/how-to-supress-specific-warnings-in-g
// suppress unused warnings of specific variables
#define MARKUSED(X)  ((void)(&(X)))

//#define RUNTIME_BOUNDS_CHECKING
#ifdef RUNTIME_BOUNDS_CHECKING
#define CHECK_FM_BOUNDS(fm, fm_len, kpts1_len, kpts2_len) \
    for(size_t fm_ind = 0; fm_ind < fm_len; fm_ind += 2) { \
        if((fm[fm_ind] >= kpts1_len) || (fm[fm_ind + 1] >= kpts2_len)) { \
            puts("CHECK_FM_BOUNDS: bad fm indexes"); \
            return; \
        } \
    }
#else
#define CHECK_FM_BOUNDS(fm, fm_len, kpts1_len, kpts2_len)
#endif

using cv::Matx;
using std::vector;

// adapted from hesaff's helpers.h
#ifndef M_TAU
#define M_TAU 6.28318
#endif
template <class T> T ensure_0toTau(T x)
{
    if(x < 0) { return ensure_0toTau(x+M_TAU); }
    else if(x >= M_TAU) { return ensure_0toTau(x-M_TAU); }
    else { return x; }
}

template<typename T> Matx<T, 3, 3> get_invV_mat(T x, T y, T a, T c, T d, T theta) {
    T ct = (T)cos(theta), st = (T)sin(theta);
    // https://github.com/aweinstock314/haskell-stuff/blob/master/ExpressionSimplifier.hs
    return Matx<T, 3, 3>(
             a*ct,      a*(-st),   x,
        c*ct+d*st, c*(-st)+d*ct,   y,
              0.0,        0.0, 1.0);
}

void debug_print_kpts(double* kpts, size_t kpts_len) {
    for(size_t i=0; i<kpts_len*6; i+=6) {
        printf("kpts[%lu]: [%f, %f, %f, %f, %f, %f]\n", i/6,
            kpts[i+0], kpts[i+1], kpts[i+2],
            kpts[i+3], kpts[i+4], kpts[i+5]);
    }
}

void debug_print_mat3x3(const char* name, const Matx<double, 3, 3>& mat) {
    printf("%s: [[%f, %f, %f], [%f, %f, %f], [%f, %f, %f]]\n", name,
        mat(0, 0), mat(0, 1), mat(0, 2),
        mat(1, 0), mat(1, 1), mat(1, 2),
        mat(2, 0), mat(2, 1), mat(2, 2));
}

template<typename T> inline T xy_distance(const Matx<T, 3, 3>& kpt1, const Matx<T, 3, 3>& kpt2) {
    // ktool.get_invVR_mats_xys
    T x1 = kpt1(0, 2), y1 = kpt1(1, 2);
    T x2 = kpt2(0, 2), y2 = kpt2(1, 2);
    // ltool.L2_sqrd
    T dx = x2-x1, dy = y2-y1;
    return dx*dx + dy*dy;
}

template<typename T> inline T det_distance(const Matx<T, 3, 3>& kpt1, const Matx<T, 3, 3>& kpt2) {
    // ktool.get_invVR_mats_sqrd_scale
    T a1 = kpt1(0, 0), b1 = kpt1(0, 1), c1 = kpt1(1, 0), d1 = kpt1(1, 1);
    T a2 = kpt2(0, 0), b2 = kpt2(0, 1), c2 = kpt2(1, 0), d2 = kpt2(1, 1);
    T det1 = a1*d1 - b1*c1, det2 = a2*d2 - b2*c2;
    // ltool.det_distance
    T dist = det1/det2;
    if(dist < 1) { dist = 1/dist; }
    return dist;
}

template<typename T> inline T ori_distance(const Matx<T, 3, 3>& kpt1, const Matx<T, 3, 3>& kpt2) {
    // ktool.get_invVR_mats_oris
    T a1 = kpt1(0, 0), b1 = kpt1(0, 1);
    T a2 = kpt2(0, 0), b2 = kpt2(0, 1);
    T ori1 = ensure_0toTau(-atan2(b1, a1));
    T ori2 = ensure_0toTau(-atan2(b2, a2));
    // ltool.ori_distance
    T delta = fabs(ori1 - ori2);
    delta = ensure_0toTau(delta);
    return std::min(delta, M_TAU-delta);
}

template<typename T> inline Matx<T, 3, 3> get_Aff_mat(const Matx<T, 3, 3>& invVR1_m,
                                                      const Matx<T, 3, 3>& invVR2_m) {

    const Matx<double, 3, 3> V1_m = invVR1_m.inv();
    const Matx<double, 3, 3> Aff_mat = invVR2_m * V1_m;
    return Aff_mat;
}

extern "C" {
    void get_affine_inliers(double* kpts1, size_t kpts1_len,
                    double* kpts2, size_t kpts2_len,
                    size_t* fm, size_t fm_len,
                    double xy_thresh_sqrd, double scale_thresh_sqrd, double ori_thresh,
                    // memory is expected to by allocated by the caller (i.e. via numpy.empty)
                    bool* out_inlier_flags, double* out_errors_list, double* out_matrices_list)
    {

        MARKUSED(kpts1_len);
        MARKUSED(kpts2_len);
        CHECK_FM_BOUNDS(fm, fm_len, kpts1_len, kpts2_len);
// remove some redundancy in a possibly-ugly way
#define SETUP_RELEVANT_VARIABLES \
double* kpt1 = &kpts1[6*fm[fm_ind+0]]; \
double* kpt2 = &kpts2[6*fm[fm_ind+1]]; \
Matx<double, 3, 3> invVR1_m = get_invV_mat( \
    kpt1[0], kpt1[1], kpt1[2], \
    kpt1[3], kpt1[4], kpt1[5]); \
Matx<double, 3, 3> invVR2_m = get_invV_mat( \
    kpt2[0], kpt2[1], kpt2[2], \
    kpt2[3], kpt2[4], kpt2[5]);
        //vector<Matx<double, 3, 3> > Aff_mats;
// MATRIX_REF(i) should be the same as Aff_mats[i], but
//  directly operating on the numpy-allocated memory
//   (less allocation == faster code)
#define MATRIX_REF(i) (*((i)+((Matx<double, 3, 3>*)out_matrices_list)))
        //vector<vector<double> > xy_errs, scale_errs, ori_errs;
        for(size_t fm_ind = 0; fm_ind < fm_len; fm_ind += 2) {
            SETUP_RELEVANT_VARIABLES
            //Aff_mats.push_back(get_Aff_mat(invVR1_m, invVR2_m));
            MATRIX_REF(fm_ind/2) = get_Aff_mat(invVR1_m, invVR2_m);
        }
        const size_t num_matches = fm_len/2;
        for(size_t i = 0; i < num_matches; i++) {
            //xy_errs.push_back(vector<double>());
            //scale_errs.push_back(vector<double>());
            //ori_errs.push_back(vector<double>());
            for(size_t fm_ind = 0; fm_ind < fm_len; fm_ind += 2) {
                SETUP_RELEVANT_VARIABLES
                Matx<double, 3, 3> Aff_mat = MATRIX_REF(i);
                // _test_hypothesis_inliers
                Matx<double, 3, 3> invVR1_mt = Aff_mat * invVR1_m;
                double xy_err = xy_distance(invVR1_mt, invVR2_m);
                double scale_err = det_distance(invVR1_mt, invVR2_m);
                double ori_err = ori_distance(invVR1_mt, invVR2_m);
                //xy_errs[i].push_back(xy_err);
                //scale_errs[i].push_back(scale_err);
                //ori_errs[i].push_back(ori_err);
// poke the error values directly into the output array with pointer voodoo to
//  avoid intermediate allocations (the explicit structure is shown by the
//   commented xy_errs, scale_errs, and ori_errs variables).
#define PACKED_INSERT(OFFSET, VAR) \
*(out_errors_list+(num_matches*3*i)+(num_matches*(OFFSET))+(fm_ind/2)) = (VAR)
                PACKED_INSERT(0, xy_err);
                PACKED_INSERT(1, ori_err);
                PACKED_INSERT(2, scale_err);
#undef PACKED_INSERT
                bool is_inlier = (xy_err < xy_thresh_sqrd) &&
                                 (scale_err < scale_thresh_sqrd) &&
                                 (ori_err < ori_thresh);
                *(out_inlier_flags+(num_matches*i)+(fm_ind/2)) = is_inlier;
                //printf("errs[%u][%u]: %f, %f, %f\n", fm_ind, i, xy_err, scale_err, ori_err);
            }
        }
#undef MATRIX_REF
#undef SETUP_RELEVANT_VARIABLES
/*
#define SHOW_ERRVEC(vec) \
for(size_t i = 0; i < vec.size(); i++) { \
    putchar('['); \
    for(size_t j = 0; j < vec[i].size(); j++) { \
        printf("%f, ", vec[i][j]); \
    } \
    puts("]"); \
}
        puts("-----");
        SHOW_ERRVEC(xy_errs)
        puts("-----");
        SHOW_ERRVEC(scale_errs)
        puts("-----");
        SHOW_ERRVEC(ori_errs)
        puts("-----");
#undef SHOW_ERRVEC
*/
        // Code for copying Aff_mats into the output is redundant now
        //  that the output is operated on directly (via MATRIX_REF)
        /*
        //printf("%lu\n", Aff_mats.size());
        for(size_t i = 0; i < Aff_mats.size(); i++) {
            const size_t mat_size = 3*3*sizeof(double);
            //char msg[] = {'M', 'a', 't', 0x30+i%10, 0};
            //debug_print_mat3x3(msg, Aff_mats[i]);
            double* dest = (out_matrices_list+(3*3*i));
            //char* destc = (char*)(out_matrices_list+(3*3*i));
            //printf("%x\n", dest);
            //printf("before: "); for(size_t j=0; j < 9; j++) {printf("%f ", *(dest+j)); }
            //printf("\nbefore: "); for(size_t j=0; j < mat_size; j+=8) {printf("0x%08x ", *(destc+j)); }
            memcpy(dest, &Aff_mats[i], mat_size);
            //printf("\nafter: "); for(size_t j=0; j < 9; j++) {printf("%f ", *(dest+j)); }
            //printf("\nafter: "); for(size_t j=0; j < mat_size; j+=8) {printf("0x%08x ", *(destc+j)); }
            //puts("\n");
        }
        */
    }

    void hello_world() {
        puts("Hello from C++!");
    }
}
<|endoftext|>


<commit_before>#include <mirrage/renderer/pass/animation_pass.hpp>

#include <mirrage/renderer/animation_comp.hpp>

#include <mirrage/ecs/components/transform_comp.hpp>
#include <mirrage/ecs/ecs.hpp>

#include <glm/gtx/string_cast.hpp>

using mirrage::ecs::components::Transform_comp;

namespace mirrage::renderer {

	namespace {
		constexpr auto shader_buffer_size = static_cast<vk::DeviceSize>(64 * 3 * 4 * int(sizeof(float)));
		constexpr auto initial_animation_capacity = static_cast<vk::DeviceSize>(16 * shader_buffer_size);

		auto animation_substance(util::Str_id substance_id, Skinning_type st)
		{
			switch(st) {
				case Skinning_type::linear_blend_skinning: return substance_id;
				case Skinning_type::dual_quaternion_skinning: return "dq_"_strid + substance_id;
			}
			return substance_id;
		}
	} // namespace

	Animation_pass::Animation_pass(Deferred_renderer& r, ecs::Entity_manager& entities)
	  : _renderer(r)
	  , _ecs(entities)
	  , _animation_uniforms(r.device(), initial_animation_capacity, vk::BufferUsageFlagBits::eUniformBuffer)
	{
		_ecs.register_component_type<Pose_comp>();
		_ecs.register_component_type<Animation_comp>();
		_ecs.register_component_type<Simple_animation_controller_comp>();

		MIRRAGE_INVARIANT(!r.gbuffer().animation_data_layout,
		                  "More than one animation implementation active!");
		r.gbuffer().animation_data_layout =
		        r.device().create_descriptor_set_layout(vk::DescriptorSetLayoutBinding{
		                0, vk::DescriptorType::eUniformBufferDynamic, 1, vk::ShaderStageFlagBits::eVertex});

		auto buffers                 = _animation_uniforms.buffer_count();
		_animation_desc_sets         = util::build_vector(buffers, [&](auto) {
            return r.create_descriptor_set(*r.gbuffer().animation_data_layout, 1);
        });
		auto anim_desc_buffer_writes = util::build_vector(buffers, [&](auto i) {
			return vk::DescriptorBufferInfo{
			        _animation_uniforms.buffer(i), static_cast<vk::DeviceSize>(0), shader_buffer_size};
		});
		auto anim_desc_writes        = util::build_vector(buffers, [&](auto i) {
            return vk::WriteDescriptorSet{*_animation_desc_sets.at(i),
                                          0,
                                          0,
                                          1,
                                          vk::DescriptorType::eUniformBufferDynamic,
                                          nullptr,
                                          &anim_desc_buffer_writes[i]};
        });

		r.device().vk_device()->updateDescriptorSets(
		        std::uint32_t(anim_desc_writes.size()), anim_desc_writes.data(), 0, nullptr);
	}

	void Animation_pass::update(util::Time time)
	{
		for(auto [controller, anim] : _ecs.list<Simple_animation_controller_comp, Animation_comp>()) {
			if(controller._next_animation.is_some()) {
				// fade requested
				auto& next = controller._next_animation.get_or_throw();

				if(controller._current_animation.is_nothing()
				   || controller._current_animation.get_or_throw().animation_id == next.animation_id) {
					// no fade required
					controller._current_animation = controller._next_animation;
					controller._next_animation    = util::nothing;
				} else if(controller._prev_animation.is_nothing()) {
					// no fade in progress => fade to new
					controller._prev_animation    = controller._current_animation;
					controller._current_animation = controller._next_animation;
					controller._next_animation    = util::nothing;
					controller._fade_time_left    = controller._fade_time;
				}
			}

			if(controller._fade_time_left > 0.f) {
				// fade in progress
				controller._fade_time_left = controller._fade_time_left - time.value();
				if(controller._fade_time_left <= 0.f) {
					controller._fade_time_left = 0.f;
					controller._prev_animation = util::nothing;
				}
			}

			for(auto& s : anim._animation_states) {
				if(controller._prev_animation.is_some()
				   && s.animation_id == controller._prev_animation.get_or_throw().animation_id)
					controller._prev_animation.get_or_throw().time = s.time;

				if(controller._current_animation.is_some()
				   && s.animation_id == controller._current_animation.get_or_throw().animation_id)
					controller._current_animation.get_or_throw().time = s.time;
			}

			if(controller._current_animation.is_some() && controller._prev_animation.is_some()) {
				anim._animation_states.resize(2);
				anim._animation_states[0]              = controller._prev_animation.get_or_throw();
				anim._animation_states[0].blend_weight = controller._fade_time_left / controller._fade_time;
				anim._animation_states[1]              = controller._current_animation.get_or_throw();
				anim._animation_states[1].blend_weight =
				        1.f - controller._fade_time_left / controller._fade_time;

			} else if(controller._current_animation.is_some()) {
				anim._animation_states.resize(1);
				anim._animation_states[0]              = controller._current_animation.get_or_throw();
				anim._animation_states[0].blend_weight = 1.f;
			}
		}

		for(auto& anim : _ecs.list<Animation_comp>()) {
			anim.step_time(time);
		}
	}

	void Animation_pass::draw(Frame_data& frame)
	{
		_compute_poses(frame);

		// TODO: add Animation_listeners to intercept/replace computed poses before drawing

		_upload_poses(frame);
	}
	void Animation_pass::_compute_poses(Frame_data& frame)
	{
		// mark all cached animations as unused
		_unused_animation_keys.clear();
		_unused_animation_keys.reserve(_animation_key_cache.size());
		for(auto&& [key, value] : _animation_key_cache) {
			(void) value;
			_unused_animation_keys.emplace(key);
		}

		// update visible animations
		for(auto& geo : frame.geometry_queue) {
			_ecs.get(geo.entity).process([&](ecs::Entity_facet& entity) {
				auto anim_mb = entity.get<Animation_comp>();
				if(anim_mb.is_nothing())
					return; // not animated

				auto& anim = anim_mb.get_or_throw();
				if(anim._animation_states.empty() || !anim._dirty)
					return; // no animation playing

				entity.get<Pose_comp>().process(
				        [&](auto& skeleton) { _update_animation(geo.entity, anim, skeleton); });
			});
		}

		// erase all unused animation keys from the cache
		for(auto&& key : _unused_animation_keys) {
			_animation_key_cache.erase(key);
		}
	}
	void Animation_pass::_upload_poses(Frame_data& frame)
	{
		// upload skeleton pose
		_animation_uniform_offsets.clear();
		_animation_uniform_queue.clear();
		auto required_size = std::int32_t(0);
		auto alignment     = std::int32_t(
                _renderer.device().physical_device_properties().limits.minUniformBufferOffsetAlignment);

		auto aligned_byte_size = [&](auto bone_count) {
			auto size = bone_count * std::int32_t(sizeof(Final_bone_transform));
			return size < alignment ? alignment : size + (alignment - size % alignment);
		};

		for(auto& geo : frame.geometry_queue) {
			if(!geo.model->rigged())
				continue;

			auto entity_mb = _ecs.get(geo.entity);
			if(entity_mb.is_nothing())
				continue;

			auto entity = entity_mb.get_or_throw();
			auto offset = gsl::narrow<std::uint32_t>(required_size);

			auto upload_required = entity.get<Shared_pose_comp>().process(true, [&](auto& sp) {
				auto pose_offset = util::find_maybe(_animation_uniform_offsets, sp.pose_owner);
				offset           = pose_offset.get_or(offset);
				_animation_uniform_offsets.emplace(geo.entity, pose_offset.get_or(offset));

				entity = _ecs.get(sp.pose_owner).get_or_throw("Invalid entity in render queue");
				entity.get<Pose_comp>().process([&](auto& pose) {
					geo.substance_id = animation_substance(geo.substance_id, pose.skeleton().skinning_type());
				});

				if(pose_offset.is_some())
					return false;

				return true;
			});

			if(upload_required) {
				entity.get<Pose_comp>().process([&](auto& pose) {
					geo.substance_id = animation_substance(geo.substance_id, pose.skeleton().skinning_type());

					auto [ex, success] = _animation_uniform_offsets.try_emplace(entity.handle(), offset);
					offset             = ex->second;

					if(success) {
						_animation_uniform_queue.emplace_back(geo.model, pose, offset);
						required_size += aligned_byte_size(geo.model->bone_count());
					}
				});
			}

			geo.animation_uniform_offset = offset;
		}

		if(_animation_uniforms.resize(required_size)) {
			// recreate DescriptorSet if the buffer has been recreated
			auto anim_desc_buffer_write = vk::DescriptorBufferInfo{
			        _animation_uniforms.write_buffer(), 0, vk::DeviceSize(64u * 3u * 4u * sizeof(float))};
			auto anim_desc_writes = vk::WriteDescriptorSet{
			        *_animation_desc_sets.at(std::size_t(_animation_uniforms.write_buffer_index())),
			        0,
			        0,
			        1,
			        vk::DescriptorType::eUniformBufferDynamic,
			        nullptr,
			        &anim_desc_buffer_write};

			_renderer.device().vk_device()->updateDescriptorSets(1, &anim_desc_writes, 0, nullptr);
		}

		for(auto& upload : _animation_uniform_queue) {
			_animation_uniforms.update_objects<Final_bone_transform>(upload.uniform_offset, [&](auto out) {
				auto geo_matrices = out.subspan(0, upload.model->bone_count());
				upload.pose->skeleton().to_final_transforms(upload.pose->bone_transforms(), geo_matrices);
			});
		}
		_animation_uniforms.flush(frame.main_command_buffer,
		                          vk::PipelineStageFlagBits::eVertexShader,
		                          vk::AccessFlagBits::eUniformRead | vk::AccessFlagBits::eShaderRead);

		_renderer.gbuffer().animation_data =
		        *_animation_desc_sets.at(std::size_t(_animation_uniforms.read_buffer_index()));
	}

	void Animation_pass::_update_animation(ecs::Entity_handle owner,
	                                       Animation_comp&    anim_comp,
	                                       Pose_comp&         result)
	{
		const auto& skeleton_data = *result._skeleton;
		const auto  bone_count    = skeleton_data.bone_count();

		result._bone_transforms.clear();
		result._bone_transforms.resize(std::size_t(bone_count), {{0, 0, 0, 0}, {0, 0, 0}, {0, 0, 0}});

		anim_comp._dirty = false;

		for(const auto& state : anim_comp._animation_states) {
			const auto& animation = *state.animation;

			// look up cached keys
			auto key = detail::Animation_key_cache_key{owner, state.animation_id};
			_unused_animation_keys.erase(key);
			auto cached_keys = _animation_key_cache[key];
			cached_keys.resize(std::size_t(bone_count), Animation_key{});

			// update pose
			for(auto i : util::range(std::size_t(bone_count))) {
				auto& key = cached_keys[i];

				result._bone_transforms[i] +=
				        state.blend_weight
				        * animation.bone_transform(Bone_id(i), state.time, key).get_or([&] {
					          return skeleton_data.node_transform(Bone_id(i));
				          });
			}
		}
	}

	auto Animation_pass_factory::create_pass(Deferred_renderer&                renderer,
	                                         util::maybe<ecs::Entity_manager&> entities,
	                                         Engine&,
	                                         bool&) -> std::unique_ptr<Render_pass>
	{
		return std::make_unique<Animation_pass>(
		        renderer, entities.get_or_throw("Animation_pass requires an entitymanager."));
	}

	auto Animation_pass_factory::rank_device(vk::PhysicalDevice,
	                                         util::maybe<std::uint32_t>,
	                                         int current_score) -> int
	{
		return current_score;
	}

	void Animation_pass_factory::configure_device(vk::PhysicalDevice,
	                                              util::maybe<std::uint32_t>,
	                                              graphic::Device_create_info&)
	{
	}
} // namespace mirrage::renderer
<commit_msg>fixed out-of-bounds-error in animation pass<commit_after>#include <mirrage/renderer/pass/animation_pass.hpp>

#include <mirrage/renderer/animation_comp.hpp>

#include <mirrage/ecs/components/transform_comp.hpp>
#include <mirrage/ecs/ecs.hpp>

#include <glm/gtx/string_cast.hpp>

using mirrage::ecs::components::Transform_comp;

namespace mirrage::renderer {

	namespace {
		constexpr auto shader_buffer_size = static_cast<vk::DeviceSize>(64 * 3 * 4 * int(sizeof(float)));
		constexpr auto initial_animation_capacity = static_cast<vk::DeviceSize>(16 * shader_buffer_size);

		auto animation_substance(util::Str_id substance_id, Skinning_type st)
		{
			switch(st) {
				case Skinning_type::linear_blend_skinning: return substance_id;
				case Skinning_type::dual_quaternion_skinning: return "dq_"_strid + substance_id;
			}
			return substance_id;
		}
	} // namespace

	Animation_pass::Animation_pass(Deferred_renderer& r, ecs::Entity_manager& entities)
	  : _renderer(r)
	  , _ecs(entities)
	  , _animation_uniforms(r.device(), initial_animation_capacity, vk::BufferUsageFlagBits::eUniformBuffer)
	{
		_ecs.register_component_type<Pose_comp>();
		_ecs.register_component_type<Animation_comp>();
		_ecs.register_component_type<Simple_animation_controller_comp>();

		MIRRAGE_INVARIANT(!r.gbuffer().animation_data_layout,
		                  "More than one animation implementation active!");
		r.gbuffer().animation_data_layout =
		        r.device().create_descriptor_set_layout(vk::DescriptorSetLayoutBinding{
		                0, vk::DescriptorType::eUniformBufferDynamic, 1, vk::ShaderStageFlagBits::eVertex});

		auto buffers                 = _animation_uniforms.buffer_count();
		_animation_desc_sets         = util::build_vector(buffers, [&](auto) {
            return r.create_descriptor_set(*r.gbuffer().animation_data_layout, 1);
        });
		auto anim_desc_buffer_writes = util::build_vector(buffers, [&](auto i) {
			return vk::DescriptorBufferInfo{
			        _animation_uniforms.buffer(i), static_cast<vk::DeviceSize>(0), shader_buffer_size};
		});
		auto anim_desc_writes        = util::build_vector(buffers, [&](auto i) {
            return vk::WriteDescriptorSet{*_animation_desc_sets.at(i),
                                          0,
                                          0,
                                          1,
                                          vk::DescriptorType::eUniformBufferDynamic,
                                          nullptr,
                                          &anim_desc_buffer_writes[i]};
        });

		r.device().vk_device()->updateDescriptorSets(
		        std::uint32_t(anim_desc_writes.size()), anim_desc_writes.data(), 0, nullptr);
	}

	void Animation_pass::update(util::Time time)
	{
		for(auto [controller, anim] : _ecs.list<Simple_animation_controller_comp, Animation_comp>()) {
			if(controller._next_animation.is_some()) {
				// fade requested
				auto& next = controller._next_animation.get_or_throw();

				if(controller._current_animation.is_nothing()
				   || controller._current_animation.get_or_throw().animation_id == next.animation_id) {
					// no fade required
					controller._current_animation = controller._next_animation;
					controller._next_animation    = util::nothing;
				} else if(controller._prev_animation.is_nothing()) {
					// no fade in progress => fade to new
					controller._prev_animation    = controller._current_animation;
					controller._current_animation = controller._next_animation;
					controller._next_animation    = util::nothing;
					controller._fade_time_left    = controller._fade_time;
				}
			}

			if(controller._fade_time_left > 0.f) {
				// fade in progress
				controller._fade_time_left = controller._fade_time_left - time.value();
				if(controller._fade_time_left <= 0.f) {
					controller._fade_time_left = 0.f;
					controller._prev_animation = util::nothing;
				}
			}

			for(auto& s : anim._animation_states) {
				if(controller._prev_animation.is_some()
				   && s.animation_id == controller._prev_animation.get_or_throw().animation_id)
					controller._prev_animation.get_or_throw().time = s.time;

				if(controller._current_animation.is_some()
				   && s.animation_id == controller._current_animation.get_or_throw().animation_id)
					controller._current_animation.get_or_throw().time = s.time;
			}

			if(controller._current_animation.is_some() && controller._prev_animation.is_some()) {
				anim._animation_states.resize(2);
				anim._animation_states[0]              = controller._prev_animation.get_or_throw();
				anim._animation_states[0].blend_weight = controller._fade_time_left / controller._fade_time;
				anim._animation_states[1]              = controller._current_animation.get_or_throw();
				anim._animation_states[1].blend_weight =
				        1.f - controller._fade_time_left / controller._fade_time;

			} else if(controller._current_animation.is_some()) {
				anim._animation_states.resize(1);
				anim._animation_states[0]              = controller._current_animation.get_or_throw();
				anim._animation_states[0].blend_weight = 1.f;
			}
		}

		for(auto& anim : _ecs.list<Animation_comp>()) {
			anim.step_time(time);
		}
	}

	void Animation_pass::draw(Frame_data& frame)
	{
		_compute_poses(frame);

		// TODO: add Animation_listeners to intercept/replace computed poses before drawing

		_upload_poses(frame);
	}
	void Animation_pass::_compute_poses(Frame_data& frame)
	{
		// mark all cached animations as unused
		_unused_animation_keys.clear();
		_unused_animation_keys.reserve(_animation_key_cache.size());
		for(auto&& [key, value] : _animation_key_cache) {
			(void) value;
			_unused_animation_keys.emplace(key);
		}

		// update visible animations
		for(auto& geo : frame.geometry_queue) {
			_ecs.get(geo.entity).process([&](ecs::Entity_facet& entity) {
				auto anim_mb = entity.get<Animation_comp>();
				if(anim_mb.is_nothing())
					return; // not animated

				auto& anim = anim_mb.get_or_throw();
				if(anim._animation_states.empty() || !anim._dirty)
					return; // no animation playing

				entity.get<Pose_comp>().process(
				        [&](auto& skeleton) { _update_animation(geo.entity, anim, skeleton); });
			});
		}

		// erase all unused animation keys from the cache
		for(auto&& key : _unused_animation_keys) {
			_animation_key_cache.erase(key);
		}
	}
	void Animation_pass::_upload_poses(Frame_data& frame)
	{
		// upload skeleton pose
		_animation_uniform_offsets.clear();
		_animation_uniform_queue.clear();
		auto required_size = std::int32_t(0);
		auto alignment     = std::int32_t(
                _renderer.device().physical_device_properties().limits.minUniformBufferOffsetAlignment);

		auto aligned_byte_size = [&](auto bone_count) {
			auto size = bone_count * std::int32_t(sizeof(Final_bone_transform));
			return size < alignment ? alignment : size + (alignment - size % alignment);
		};

		for(auto& geo : frame.geometry_queue) {
			if(!geo.model->rigged())
				continue;

			auto entity_mb = _ecs.get(geo.entity);
			if(entity_mb.is_nothing())
				continue;

			auto entity = entity_mb.get_or_throw();
			auto offset = gsl::narrow<std::uint32_t>(required_size);

			auto upload_required = entity.get<Shared_pose_comp>().process(true, [&](auto& sp) {
				auto pose_offset = util::find_maybe(_animation_uniform_offsets, sp.pose_owner);
				offset           = pose_offset.get_or(offset);
				_animation_uniform_offsets.emplace(geo.entity, pose_offset.get_or(offset));

				entity = _ecs.get(sp.pose_owner).get_or_throw("Invalid entity in render queue");
				entity.get<Pose_comp>().process([&](auto& pose) {
					geo.substance_id = animation_substance(geo.substance_id, pose.skeleton().skinning_type());
				});

				if(pose_offset.is_some())
					return false;

				return true;
			});

			if(upload_required) {
				entity.get<Pose_comp>().process([&](auto& pose) {
					geo.substance_id = animation_substance(geo.substance_id, pose.skeleton().skinning_type());

					auto [ex, success] = _animation_uniform_offsets.try_emplace(entity.handle(), offset);
					offset             = ex->second;

					if(success) {
						_animation_uniform_queue.emplace_back(geo.model, pose, offset);
						required_size += aligned_byte_size(geo.model->bone_count());
					}
				});
			}

			geo.animation_uniform_offset = offset;
		}

		// add padding to allow overstepping
		required_size += gsl::narrow<std::uint32_t>(64u * 3u * 4u * sizeof(float));

		if(_animation_uniforms.resize(required_size)) {
			// recreate DescriptorSet if the buffer has been recreated
			auto anim_desc_buffer_write = vk::DescriptorBufferInfo{
			        _animation_uniforms.write_buffer(), 0, vk::DeviceSize(64u * 3u * 4u * sizeof(float))};
			auto anim_desc_writes = vk::WriteDescriptorSet{
			        *_animation_desc_sets.at(std::size_t(_animation_uniforms.write_buffer_index())),
			        0,
			        0,
			        1,
			        vk::DescriptorType::eUniformBufferDynamic,
			        nullptr,
			        &anim_desc_buffer_write};

			_renderer.device().vk_device()->updateDescriptorSets(1, &anim_desc_writes, 0, nullptr);
		}

		for(auto& upload : _animation_uniform_queue) {
			_animation_uniforms.update_objects<Final_bone_transform>(upload.uniform_offset, [&](auto out) {
				auto geo_matrices = out.subspan(0, upload.model->bone_count());
				upload.pose->skeleton().to_final_transforms(upload.pose->bone_transforms(), geo_matrices);
			});
		}
		_animation_uniforms.flush(frame.main_command_buffer,
		                          vk::PipelineStageFlagBits::eVertexShader,
		                          vk::AccessFlagBits::eUniformRead | vk::AccessFlagBits::eShaderRead);

		_renderer.gbuffer().animation_data =
		        *_animation_desc_sets.at(std::size_t(_animation_uniforms.read_buffer_index()));
	}

	void Animation_pass::_update_animation(ecs::Entity_handle owner,
	                                       Animation_comp&    anim_comp,
	                                       Pose_comp&         result)
	{
		const auto& skeleton_data = *result._skeleton;
		const auto  bone_count    = skeleton_data.bone_count();

		result._bone_transforms.clear();
		result._bone_transforms.resize(std::size_t(bone_count), {{0, 0, 0, 0}, {0, 0, 0}, {0, 0, 0}});

		anim_comp._dirty = false;

		for(const auto& state : anim_comp._animation_states) {
			const auto& animation = *state.animation;

			// look up cached keys
			auto key = detail::Animation_key_cache_key{owner, state.animation_id};
			_unused_animation_keys.erase(key);
			auto cached_keys = _animation_key_cache[key];
			cached_keys.resize(std::size_t(bone_count), Animation_key{});

			// update pose
			for(auto i : util::range(std::size_t(bone_count))) {
				auto& key = cached_keys[i];

				result._bone_transforms[i] +=
				        state.blend_weight
				        * animation.bone_transform(Bone_id(i), state.time, key).get_or([&] {
					          return skeleton_data.node_transform(Bone_id(i));
				          });
			}
		}
	}

	auto Animation_pass_factory::create_pass(Deferred_renderer&                renderer,
	                                         util::maybe<ecs::Entity_manager&> entities,
	                                         Engine&,
	                                         bool&) -> std::unique_ptr<Render_pass>
	{
		return std::make_unique<Animation_pass>(
		        renderer, entities.get_or_throw("Animation_pass requires an entitymanager."));
	}

	auto Animation_pass_factory::rank_device(vk::PhysicalDevice,
	                                         util::maybe<std::uint32_t>,
	                                         int current_score) -> int
	{
		return current_score;
	}

	void Animation_pass_factory::configure_device(vk::PhysicalDevice,
	                                              util::maybe<std::uint32_t>,
	                                              graphic::Device_create_info&)
	{
	}
} // namespace mirrage::renderer
<|endoftext|>