text:
#pragma once

#include <rj_geometry/point.hpp>
#include <rj_geometry/pose.hpp>
#include <boost/circular_buffer.hpp>
#include <rj_common/utils.hpp>
#include <rj_vision_filter/filter/kalman_filter3_d.hpp>
#include <rj_vision_filter/robot/camera_robot.hpp>

namespace vision_filter {
class WorldRobot;

/**
 * Filtered robot estimation for a single camera
 */
class KalmanRobot {
public:
    /**
     * Checks previous_world_robot to see if it's valid
     *
     * @param camera_id ID of the camera this filter is applied to
     * @param creation_time Time this filter was created
     * @param init_measurement Initial robot measurement
     * @param previous_world_robot World robot from last frame (or invalid world
     * robot)
     */
    KalmanRobot(unsigned int camera_id, RJ::Time creation_time,
                CameraRobot init_measurement,
                const WorldRobot& previous_world_robot);

    /**
     * Predicts one time step forward
     *
     * @param current_time Current time of the prediction step
     */
    void predict(RJ::Time current_time);

    /**
     * Predicts one time step forward then triangulates toward the measurement
     *
     * @param current_time Current time of the prediction/update step
     * @param update_robot Robot measurement that we are using as feedback
     */
    void predict_and_update(RJ::Time current_time, CameraRobot update_robot);

    /**
     * Returns true when the filter hasn't been updated in a while and should be
     * deleted
     */
    bool is_unhealthy() const;

    /**
     * @return The camera id this belongs to
     */
    unsigned int get_camera_id() const;

    /**
     * @return This robot's id
     */
    int get_robot_id() const;

    /**
     * @return How healthy this filter is. AKA How often it's been updated
     */
    int get_health() const;

    /**
     * @return Best estimate of the linear position of the robot
     */
    rj_geometry::Point get_pos() const;

    /**
     * @return Best estimate of the heading. Not bounded
     */
    double get_theta() const;

    /**
     * @return Best estimate of the linear velocity of the robot
     */
    rj_geometry::Point get_vel() const;

    /**
     * @return Best estimate of the angular velocity
     */
    double get_omega() const;

    /**
     * @return Covariance in X and Y linear direction of the position of the
     * robot
     */
    rj_geometry::Point get_pos_cov() const;

    /**
     * @return Covariance of theta of the robot
     */
    double get_theta_cov() const;

    /**
     * @return Covariance in X and Y linear direction of the velocity of the
     * robot
     */
    rj_geometry::Point get_vel_cov() const;

    /**
     * @return Covariance of omega of the robot
     */
    double get_omega_cov() const;

    /**
     * @return List of previous camera robot measurements for kick detection
     */
    const boost::circular_buffer<CameraRobot>& get_prev_measurements() const;

private:
    RJ::Time last_update_time_;
    RJ::Time last_predict_time_;

    boost::circular_buffer<CameraRobot> previous_measurements_;

    KalmanFilter3D filter_;

    double previous_theta_;
    int unwrap_theta_ctr_;
    int health_;

    int robot_id_;

    unsigned int camera_id_;
};
}  // namespace vision_filter


text:
#include "beastquest/auth.hh"

#include <boost/beast.hpp>
#include <string>

namespace detail = boost::beast::detail;
namespace quest {

BasicAuth::BasicAuth(std::string username, std::string password)
    : username_{std::move(username)},
      password_{std::move(password)},
      auth_string_("Basic " +
                   detail::base64_encode(username_ + ":" + password_)) {}

std::string BasicAuth::GetAuthString() const noexcept { return auth_string_; }

bool BasicAuth::IsEmpty() const noexcept { return auth_string_.empty(); }

}  // namespace quest

ext: cc

max_stars_repo_name: watersalesman/BeastQuest

max_issues_repo_name: watersalesman/BeastQuest

max_forks_repo_name: watersalesman/BeastQuest


text:
/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2020, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Implements the "Walking Control Based on Step Timing Adaptation" foot
 * trajectory QP. The pdf can be found in https://arxiv.org/abs/1704.01271, and
 * in the `doc/` folder in this repository.
 */

#pragma once

#include <eigen-quadprog/QuadProg.h>
#include <Eigen/Eigen>

namespace reactive_planners
{
/**
 * @brief
 */
class PolynomialEndEffectorTrajectory
{
    /*
     * Private methods
     */
public:
    /** @brief Constructor. */
    PolynomialEndEffectorTrajectory();

    /** @brief Destructor. */
    ~PolynomialEndEffectorTrajectory();

    bool compute(const Eigen::Ref<const Eigen::Vector3d> &start_pose,
                 const Eigen::Ref<const Eigen::Vector3d> &current_pose,
                 const Eigen::Ref<const Eigen::Vector3d> &current_velocity,
                 const Eigen::Ref<const Eigen::Vector3d> &current_acceleration,
                 const Eigen::Ref<const Eigen::Vector3d> &target_pose,
                 const double &start_time,
                 const double &current_time,
                 const double &end_time);

    void get_next_state(const double &next_time,
                        Eigen::Ref<Eigen::Vector3d> next_pose,
                        Eigen::Ref<Eigen::Vector3d> next_velocity,
                        Eigen::Ref<Eigen::Vector3d> next_acceleration);

    /** @brief Display the matrices of the Problem. */
    void print_solver() const;

    /** @brief Convert the inner data to a string format. */
    std::string to_string() const;

    /*
     * Getters
     */

    /** @brief Get the height of the flying foot. */
    double get_mid_air_height()
    {
        return mid_air_height_;
    }

    /** @brief Get the last end time taken into account during the foot
     * trajectory computation. */
    double get_last_end_time_taken_into_account()
    {
        return last_end_time_seen_;
    }

    /*
     * Setters
     */

    /** @brief Set the height of the flying foot.
     *
     * @param mid_air_height
     */
    void set_mid_air_height(double mid_air_height)
    {
        mid_air_height_ = mid_air_height;
    }
    /** @brief Set the costs of x, y, z axes, and hessian regularization.
     *
     * @param cost_x
     * @param cost_y
     * @param cost_z
     * @param hess_regularization
     */
    void set_costs(double cost_x,
                   double cost_y,
                   double cost_z,
                   double hess_regularization)
    {
        cost_x_ = cost_x;
        cost_y_ = cost_y;
        cost_z_ = cost_z;
        Q_regul_ =
            Eigen::MatrixXd::Identity(nb_var_, nb_var_) * hess_regularization;
    }
    /*
     * Private methods
     */
private:
    /**
     * @brief Compute the time vector: \f$ [1, t, ..., t^{ORDER}] \f$.
     *
     * @param time
     * @param time_vec
     */
    void t_vec(const double &time, Eigen::VectorXd &time_vec)
    {
        time_vec(0) = 1.0;
        for (int i = 1; i < time_vec.size(); ++i)
        {
            time_vec(i) = std::pow(time, i);
        }
    }

    /**
     * @brief Compute the time vector first derivative.
     *
     * \f[ t_vec =  [0, 1, 2*t, ..., ORDER * t^{ORDER-1}] \f]
     *
     * @param time
     * @param time_vec
     */
    void dt_vec(const double &time, Eigen::VectorXd &time_vec)
    {
        time_vec(0) = 0.0;
        time_vec(1) = 1.0;
        for (int i = 2; i < time_vec.size(); ++i)
        {
            double id = i;
            time_vec(i) = id * std::pow(time, i - 1);
        }
    }

    /**
     * @brief Compute the time vector second derivative:
     *
     * \f[ t_vec =  [0, 0, 2, 3*2*t..., ORDER * (ORDER-1) * t^{ORDER-2}] \f]
     *
     * @param time
     * @param time_vec
     */
    void ddt_vec(const double &time, Eigen::VectorXd &time_vec)
    {
        time_vec(0) = 0.0;
        time_vec(1) = 0.0;
        time_vec(2) = 2.0;
        for (int i = 3; i < time_vec.size(); ++i)
        {
            double id = i;
            time_vec(i) = id * (id - 1.0) * std::pow(time, i - 2);
        }
    }

    /*
     * Attributes
     */
private:
    /*
     * Constant problem parameters.
     */

    /** @brief Flying foot apex to be reach mid-air. */
    double mid_air_height_;

    /** @brief Number of the polynome coefficient for the trajectory on the
     * X-axis. */
    int nb_var_x_;

    /** @brief Number of the polynome coefficient for the trajectory on the
     * Y-axis. */
    int nb_var_y_;

    /** @brief Number of the polynome coefficient for the trajectory on the
     * Z-axis. */
    int nb_var_z_;

    /*
     * Variable problem parameters.
     */

    /** @brief Time vector used to compute X(t). */
    Eigen::VectorXd time_vec_x_;

    /** @brief Time vector used to compute Y(t). */
    Eigen::VectorXd time_vec_y_;

    /** @brief Time vector used to compute Z(t). */
    Eigen::VectorXd time_vec_z_;

    /** @brief Initial position before the motion. */
    Eigen::Vector3d start_pose_;

    /** @brief Current position. */
    Eigen::Vector3d current_pose_;

    /** @brief Previous computed position from the QP. */
    Eigen::Vector3d previous_solution_pose_;

    /** @brief Current velocity. */
    Eigen::Vector3d current_velocity_;

    /** @brief Current acceleration. */
    Eigen::Vector3d current_acceleration_;

    /** @brief Target pose after the motion. */
    Eigen::Vector3d target_pose_;

    /** @brief Initial time. */
    double start_time_;

    /** @brief Current time. */
    double current_time_;

    /** @brief Final time and the end of the motion. */
    double end_time_;

    /** @brief Last end time register when we computed the QP. */
    double last_end_time_seen_;

    /*
     * QP variables
     */

    /** @brief Number of variabes in the optimization problem. */
    int nb_var_;

    /** @brief Number of equality constraints in the optimization problem. */
    int nb_eq_;

    /** @brief Number of inequality in the optimization problem. */
    int nb_ineq_;

    /** @brief Quadratic program solver.
     *
     * This is an eigen wrapper around the quad_prog fortran solver.
     */
    Eigen::QuadProgDense qp_solver_;

    /** @brief Solution of the optimization problem.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd x_opt_;

    /** @brief Lower Bound on the solution of the optimization problem.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd x_opt_lb_;

    /** @brief Upper Bound on the solution of the optimization problem.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd x_opt_ub_;

    /** @brief Quadratic term of the quadratic cost.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::MatrixXd Q_;

    /** @brief Quadratic term added to the quadratic cost in order regularize
     * the system.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::MatrixXd Q_regul_;

    /** @brief Cost weights for the X-axis. */
    double cost_x_;

    /** @brief Cost weights for the Y-axis. */
    double cost_y_;

    /** @brief Cost weights for the Z-axis. */
    double cost_z_;

    /** @brief Linear term of the quadratic cost.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd q_;

    /** @brief Linear equality matrix.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::MatrixXd A_eq_;

    /** @brief Linear equality vector.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd B_eq_;

    /** @brief Linear inequality matrix.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::MatrixXd A_ineq_;

    /** @brief Linear inequality vector.
     * @see PolynomialEndEffectorTrajectory */
    Eigen::VectorXd B_ineq_;
};

}  // namespace reactive_planners

ext: hpp


text:
#pragma once
#include <vector>
#include <istream>
#include <string>
#include <algorithm>
#include <Eigen/Dense>
#include <set>

using namespace Eigen;

std::vector<std::string> input_csv(std::istream& stream);

MatrixXd readMatrix(std::istream& stream, int rows, int cols);

void stream_network_data(std::vector<MatrixXd>& weights, std::vector<VectorXd>& biases, std::set<std::string>& labels, std::istream& stream);


text:
// lexer.hpp
/*
 *  Copyright (c) 2007 Leigh Johnston.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are
 *  met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 *     * Neither the name of Leigh Johnston nor the names of any
 *       other contributors to this software may be used to endorse or
 *       promote products derived from this software without specific prior
 *       written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
 *  IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 *  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 *  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 *  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 *  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#pragma once

#include <neolib/neolib.hpp>
#include <string>
#include <vector>
#include <deque>
#include <unordered_map>
#include <fstream>
#include <sstream>
#include <optional>
#include <boost/functional/hash.hpp>
#include <neolib/core/variant.hpp>

namespace neolib
{
    struct lexer_atom_match_any {};

    inline bool operator==(const lexer_atom_match_any&, const lexer_atom_match_any&)
    {
        return true;
    }

    inline std::size_t hash_value(const lexer_atom_match_any&)
    {
        return 0u;
    }

    enum class lexer_atom_function
    {
        Eat,
        Keep,
        Not,
        End
    };

    template <typename Token, typename CharT = char>
    class lexer_token : public std::pair<Token, std::basic_string<CharT>>
    {
        typedef std::pair<Token, std::basic_string<CharT>> base_type;
    public:
        typedef Token token_type;
        typedef std::basic_string<CharT> value_type;
    public:
        lexer_token() :
            base_type{}
        {
        }
        lexer_token(token_type aToken, const value_type& aValue) :
            base_type{aToken, aValue}
        {
        }
    public:
        token_type token() const
        {
            return base_type::first;
        }
        const value_type& value() const
        {
            return base_type::second;
        }
    };

    struct no_scopes {};

    template <typename Token, typename Scope = no_scopes, typename CharT = char>
    class lexer_atom
    {
    public:
        typedef Token token_type;
        typedef Scope scope_type;
        typedef CharT char_type;
        typedef std::pair<char_type, char_type> range_type;
        typedef std::basic_string<char_type> string_type;
        typedef std::vector<lexer_atom_function> function_list;
        typedef std::pair<token_type, function_list> function_type;
        typedef std::pair<scope_type, bool> scope_change_type;
        typedef variant<char_type, range_type, string_type, lexer_atom_match_any, token_type, function_type, scope_type, scope_change_type> value_type;
        typedef string_type token_value_type;
    public:
        struct not_token : std::logic_error { not_token(const std::string& aBadToken) : std::logic_error("Invalid token: '" + aBadToken + "'") {} };
        struct not_scope : std::logic_error { not_scope(const std::string& aBadScope) : std::logic_error("Invalid scope: '" + aBadScope + "'") {} };
    public:
        lexer_atom() :
            iValue{}
        {
        }
        template <typename T>
        lexer_atom(const T& aValue, const token_value_type& aTokenValue = token_value_type{}) :
            iValue{ aValue }, iTokenValue{ aTokenValue }
        {
        }
        template <typename T>
        lexer_atom(lexer_atom_function aFunction, const T& aValue, const token_value_type& aTokenValue = token_value_type{}) :
            iValue{ function_type{ aValue, { aFunction } } }, iTokenValue{ aTokenValue }
        {
        }
    public:
        bool operator==(const lexer_atom& aOther) const
        {
            return iValue == aOther.iValue && iTokenValue == aOther.iTokenValue;
        }
        bool operator!=(const lexer_atom& aOther) const
        {
            return !(*this == aOther);
        }
    public:
        template <typename T>
        bool is() const
        {
            return std::holds_alternative<T>(iValue);
        }
        const value_type& value() const
        {
            return iValue;
        }
        bool is_token() const
        {
            return std::holds_alternative<token_type>(iValue) || std::holds_alternative<function_type>(iValue);
        }
        token_type token() const
        {
            if (std::holds_alternative<token_type>(iValue))
                return static_variant_cast<token_type>(iValue);
            else if (std::holds_alternative<function_type>(iValue))
                return static_variant_cast<const function_type&>(iValue).first;
            else if (std::holds_alternative<char_type>(iValue))
                throw not_token(std::string(1, static_variant_cast<char_type>(iValue)));
            else if (std::holds_alternative<string_type>(iValue))
                throw not_token(static_variant_cast<const string_type&>(iValue));
            else
                throw not_token("???");
        }
        void set_token(token_type aToken)
        {
            iValue = aToken;
        }
        bool is_scope() const
        {
            return std::holds_alternative<scope_type>(iValue);
        }
        scope_type scope() const
        {
            if (std::holds_alternative<scope_type>(iValue))
                return static_variant_cast<scope_type>(iValue);
            else
                throw not_scope("???");
        }
        void set_scope(scope_type aScope)
        {
            iValue = aScope;
        }
        bool has_functions() const
        {
            return std::holds_alternative<function_type>(iValue);
        }
        const function_list& functions() const
        {
            return static_variant_cast<const function_type&>(iValue).second;
        }
        function_list& functions()
        {
            return static_variant_cast<function_type&>(iValue).second;
        }
        const token_value_type& token_value() const
        {
            return iTokenValue;
        }
        token_value_type& token_value()
        {
            return iTokenValue;
        }
    private:
        value_type iValue;
        token_value_type iTokenValue;
    };

    template <typename T, typename Token, typename Scope, typename CharT>
    inline bool holds_alternative(const lexer_atom<Token, Scope, CharT>& aAtom)
    {
        return aAtom.is<T>();
    }

    template <typename Atom>
    class lexer_rule
    {
    public:
        typedef Atom atom_type;
        typedef typename atom_type::char_type char_type;
        typedef typename atom_type::token_type token_type;
        typedef typename atom_type::scope_type scope_type;
        typedef typename atom_type::range_type range_type;
    public:
        atom_type symbol;
        std::vector<atom_type> expression;
    public:
        static constexpr std::pair<scope_type, bool> enter_scope(scope_type aScope)
        {
            return std::make_pair(aScope, true);
        }
        static constexpr std::pair<scope_type, bool> leave_scope(scope_type aScope)
        {
            return std::make_pair(aScope, false);
        }
        static constexpr atom_type token_end(token_type aToken)
        {
            return atom_type{ lexer_atom_function::End, aToken };
        }
        static atom_type token_end(atom_type aAtom)
        {
            if (aAtom.has_functions())
            {
                aAtom.functions().push_back(lexer_atom_function::End);
                return aAtom;
            }
            return typename atom_type::function_type{ aAtom.token(),{ lexer_atom_function::End } };
        }
        static constexpr atom_type token_eat(token_type aToken)
        {
            return atom_type{ lexer_atom_function::Eat, aToken };
        }
        static atom_type token_eat(atom_type aAtom)
        {
            if (aAtom.has_functions())
            {
                aAtom.functions().push_back(lexer_atom_function::Eat);
                return aAtom;
            }
            return typename atom_type::function_type{ aAtom.token(),{ lexer_atom_function::Eat } };
        }
        static constexpr atom_type token_keep(token_type aToken)
        {
            return atom_type{ lexer_atom_function::Keep, aToken };
        }
        static atom_type token_keep(atom_type aAtom)
        {
            if (aAtom.has_functions())
            {
                aAtom.functions().push_back(lexer_atom_function::Keep);
                return aAtom;
            }
            return typename atom_type::function_type{ aAtom.token(),{ lexer_atom_function::Keep } };
        }
        static constexpr atom_type token_make(token_type aToken, char_type aChar)
        {
            return atom_type{aToken, typename atom_type::token_value_type{ 1, aChar } };
        }
        static constexpr atom_type token_not(token_type aToken)
        {
            return atom_type{ lexer_atom_function::Not, aToken };
        }
        static constexpr range_type token_range(char_type aFrom, char_type aTo)
        {
            return range_type{ aFrom, aTo };
        }
        static constexpr lexer_atom_match_any token_any()
        {
            return lexer_atom_match_any{};
        }
    };

    template <typename Atom>
    class lexer
    {
    public:
        typedef Atom atom_type;
        typedef typename atom_type::token_type token_type;
        typedef typename atom_type::scope_type scope_type;
        typedef typename atom_type::scope_change_type scope_change_type;
        typedef typename atom_type::char_type char_type;
        typedef lexer_token<token_type, char_type> lexer_token_type;
        typedef typename atom_type::range_type range_type;
        typedef typename atom_type::string_type string_type;
        typedef typename atom_type::function_type function_type;
        typedef lexer_rule<atom_type> rule_type;
    private:
        typedef std::shared_ptr<std::istream> stream_pointer;
    public:
        class context
        {
            friend class lexer;
        public:
            context(const lexer& aParent) :
                iParent{ aParent },
                iFinished{ false }, iError{ false }, iCharIndex{}, iLineIndex{}, iColumnIndex{}, iPreviousChar{}, iInputBufferIndex{}
            {
            }
        public:
            context& operator>>(lexer_token_type& aToken)
            {
                try
                {
                    atom_type atom;
                    iParent.get_token(*this, atom);
                    if (*this)
                        aToken = lexer_token_type{ atom.token(), atom.token_value() };
                }
                catch (std::exception& e)
                {
                    throw_with_info<std::exception>(e.what());
                }
                catch (...)
                {
                    throw_with_info<std::exception>("unknown exception");
                }
                return *this;
            }
            explicit operator bool() const
            {
                return !iError;
            }
        private:
            template <typename Exception>
            void throw_with_info(const std::string& aReason)
            {
                static std::ostringstream oss;
                oss.str("");
                oss << "Lexer error: " << aReason << std::endl;
                oss << "Line: " << iLineIndex << std::endl;
                oss << "Column: " << iColumnIndex << std::endl;
                throw Exception{ oss.str().c_str() };
            }
        private:
            const lexer& iParent;
            stream_pointer iInput;
            bool iFinished;
            bool iError;
            string_type iInputBuffer;
            std::size_t iInputBufferIndex;
            uint32_t iCharIndex;
            uint32_t iLineIndex;
            uint32_t iColumnIndex;
            char iPreviousChar;
            std::deque<atom_type> iQueue;
        };
    private:
        typedef std::vector<rule_type> rule_list;
        enum class search_type
        {
            Token,
            String
        };
        enum class match_result
        {
            None,
            Partial,
            Complete
        };
        class node
        {
        public:
            typedef neolib::variant<token_type, function_type, scope_change_type> value_type;
            typedef std::pair<node*, value_type> next_type;
            typedef std::optional<next_type> optional_next_type;
        public:
            struct bad_terminal_atom : std::logic_error { bad_terminal_atom() : std::logic_error("neolib::lexer::node::bad_terminal_atom") {} };
            struct unsupported_atom_type : std::logic_error { unsupported_atom_type() : std::logic_error("neolib::lexer::node::unsupported_atom_type") {} };
            struct node_exists : std::logic_error { node_exists() : std::logic_error("neolib::lexer::node::node_exists") {} };
            struct invalid_atom : std::invalid_argument { invalid_atom() : std::invalid_argument("neolib::lexer::node::invalid_atom") {} };
        public:
            node(lexer& aParent) :
                iParent{ aParent }, iTokenMap {}, iCharMap{}
            {
            }
        public:
            void map(const atom_type& aAtom, node& aNextNode)
            {
                if (holds_alternative<char_type>(aAtom))
                    map(static_variant_cast<char_type>(aAtom.value()), aNextNode);
                else if (holds_alternative<token_type>(aAtom))
                    map(static_variant_cast<token_type>(aAtom.value()), aNextNode);
            }
            void map(const atom_type& aAtom, const atom_type& aTerminalAtom)
            {
                if (holds_alternative<token_type>(aTerminalAtom))
                {
                    if (holds_alternative<char_type>(aAtom))
                        map(static_variant_cast<char_type>(aAtom.value()), static_variant_cast<token_type>(aTerminalAtom.value()));
                    else if (holds_alternative<token_type>(aAtom))
                        map(static_variant_cast<token_type>(aAtom.value()), static_variant_cast<token_type>(aTerminalAtom.value()));
                    else if (holds_alternative<function_type>(aAtom))
                        map(static_variant_cast<const function_type&>(aAtom.value()), static_variant_cast<token_type>(aTerminalAtom.value()));
                }
                else if (holds_alternative<function_type>(aTerminalAtom))
                {
                    if (holds_alternative<char_type>(aAtom))
                        map(static_variant_cast<char_type>(aAtom.value()), static_variant_cast<const function_type&>(aTerminalAtom.value()));
                    else if (holds_alternative<token_type>(aAtom))
                        map(static_variant_cast<token_type>(aAtom.value()), static_variant_cast<const function_type&>(aTerminalAtom.value()));
                    else if (holds_alternative<function_type>(aAtom))
                        map(static_variant_cast<const function_type&>(aAtom.value()), static_variant_cast<const function_type&>(aTerminalAtom.value()));
                }
                else
                    throw bad_terminal_atom();
            }
            void map(char_type aChar, node& aNextNode)
            {
                if (iCharMap[aChar].first == nullptr)
                    iCharMap[aChar].first = &aNextNode;
                else
                    throw node_exists();
            }
            void map(token_type aToken, node& aNextNode)
            {
                if (iTokenMap[aToken].first == nullptr)
                    iTokenMap[aToken].first = &aNextNode;
                else
                    throw node_exists();
            }
            void map(const function_type& aFunction, node& aNextNode)
            {
                if (iFunctionMap[aFunction].first == nullptr)
                    iFunctionMap[aFunction].first = &aNextNode;
                else
                    throw node_exists();
            }
            void map(char_type aChar, token_type aTerminalToken)
            {
                iCharMap[aChar].second = aTerminalToken;
            }
            void map(token_type aToken, token_type aTerminalToken)
            {
                iTokenMap[aToken].second = aTerminalToken;
            }
            void map(const function_type& aFunction, token_type aTerminalToken)
            {
                iFunctionMap[aFunction].second = aTerminalToken;
            }
            void map(char_type aChar, const function_type& aTerminalFunction)
            {
                iCharMap[aChar].second = aTerminalFunction;
            }
            void map(token_type aToken, const function_type& aTerminalFunction)
            {
                iTokenMap[aToken].second = aTerminalFunction;
            }
            void map(const function_type& aFunction, const function_type& aTerminalFunction)
            {
                iFunctionMap[aFunction].second = aTerminalFunction;
            }
            const next_type& lookup(const atom_type& aAtom) const
            {
                if (holds_alternative<char_type>(aAtom))
                    return lookup(static_variant_cast<char_type>(aAtom.value()));
                else if (holds_alternative<token_type>(aAtom))
                    return lookup(static_variant_cast<token_type>(aAtom.value()));
                else if (holds_alternative<function_type>(aAtom))
                    return lookup(static_variant_cast<const function_type&>(aAtom.value()));
                else
                    throw unsupported_atom_type();
            }
            const next_type& lookup(char_type aChar) const
            {
                return iCharMap[aChar];
            }
            const next_type& lookup(token_type aToken) const
            {
                return iTokenMap[aToken];
            }
            const next_type& lookup(const function_type& aFunction) const
            {
                return iFunctionMap[aFunction];
            }
            template <typename Iter>
            std::pair<match_result, value_type> match(Iter aFirst, Iter aLast, search_type aSearchType) const
            {
                atom_type atom = *aFirst++;
                auto atomMatch = match_atom(atom, aSearchType);
                if (atomMatch)
                {
                    if (aFirst == aLast)
                    {
                        if (atomMatch->second != neolib::none)
                            return std::make_pair(match_result::Complete, atomMatch->second);
                        else if (atomMatch->first != nullptr)
                            return std::make_pair(match_result::Partial, value_type{});
                        else
                            return std::make_pair(match_result::None, value_type{});
                    }
                    else if (atomMatch->first != nullptr && (!holds_alternative<char_type>(atom) || aSearchType == search_type::String))
                        return atomMatch->first->match(aFirst, aLast, aSearchType);
                    else
                        return std::make_pair(match_result::None, value_type{});
                }
                else
                    return std::make_pair(match_result::None, value_type{});
            }
        private:
            optional_next_type match_atom(const atom_type& aAtom, search_type aSearchType) const
            {
                if (holds_alternative<char_type>(aAtom))
                {
                    if (this == &*iParent.iNodes.begin() || aSearchType == search_type::String)
                    {
                        auto existing = iCharMap.find(static_variant_cast<char_type>(aAtom.value()));
                        if (existing != iCharMap.end())
                            return existing->second;
                    }
                    return optional_next_type{};
                }
                else if (holds_alternative<token_type>(aAtom))
                {
                    auto token = static_variant_cast<token_type>(aAtom.value());
                    auto existingToken = iTokenMap.find(token);
                    if (existingToken != iTokenMap.end())
                        return existingToken->second;
                    for (auto iterFunction = iFunctionMap.begin(); iterFunction != iFunctionMap.end(); ++iterFunction)
                    {
                        auto functionToken = iterFunction->first.first;
                        auto functions = iterFunction->first.second;
                        bool not_ = (std::find(functions.begin(), functions.end(), lexer_atom_function::Not) != functions.end());
                        if ((functionToken == token && !not_) || (functionToken != token && not_))
                            return iterFunction->second;
                    }
                    return optional_next_type{};
                }
                throw invalid_atom();
            }
        private:
            lexer& iParent;
            mutable std::unordered_map<char_type, next_type> iCharMap;
            mutable std::unordered_map<token_type, next_type> iTokenMap;
            mutable std::unordered_map<function_type, next_type, boost::hash<function_type>> iFunctionMap;
            mutable std::unordered_map<scope_type, next_type, boost::hash<function_type>> iScopeMap;
        };
        typedef std::list<node> node_list;
        typedef std::shared_ptr<std::istream> stream_pointer;
    public:
        struct style_sheet_not_utf8 : std::runtime_error { style_sheet_not_utf8(const std::string& reason = "neolib::lexer_atom::style_sheet_not_utf8") : std::runtime_error(reason) {} };
        struct bad_lex_tree : std::logic_error { bad_lex_tree(const std::string& reason = "neolib::lexer_atom::bad_lex_tree") : std::logic_error(reason) {} };
        struct end_of_file_reached : std::runtime_error { end_of_file_reached(const std::string& reason = "neolib::lexer_atom::end_of_file_reached") : std::runtime_error(reason) {} };
        struct invalid_token : std::runtime_error { invalid_token(const std::string& reason = "neolib::lexer_atom::invalid_token") : std::runtime_error(reason) {} };
    public:
        template <typename Iter>
        lexer(Iter aFirstRule, Iter aLastRule) :
            iDefaultScope{}
        {
            for (auto r = aFirstRule; r != aLastRule; ++r)
                build(*r);
        }
        template <typename Iter>
        lexer(scope_type aDefaultScope, Iter aFirstRule, Iter aLastRule) :
            iDefaultScope{ aDefaultScope }
        {
            for (auto r = aFirstRule; r != aLastRule; ++r)
                build(*r);
        }
    public:
        context open(const std::string& aPath) const
        {
            context newContext{ *this };
            newContext.iInput = std::make_shared<std::ifstream>(aPath, std::ios_base::in | std::ios_base::binary);
            return newContext;
        }
        context use(std::istream& aStream) const
        {
            context newContext{ *this };
            newContext.iInput = stream_pointer(stream_pointer{}, &aStream);
            return newContext;
        }
        context use(const std::string& aText) const
        {
            context newContext{ *this };
            newContext.iInput = std::make_shared<std::istringstream>(aText);
            return newContext;
        }
    private:
        const lexer& get_token(context& aContext, atom_type& aAtom) const
        {
            if (aContext.iFinished && aContext.iQueue.empty())
            {
                aContext.iError = true;
                return *this;
            }
            if (aContext.iQueue.empty())
                if (!next(aContext))
                    return *this;
            bool backup = false;
            for (auto iter = aContext.iQueue.end(); iter != aContext.iQueue.begin(); iter = (backup ? aContext.iQueue.end() : iter - 1))
            {
                backup = false;
                auto match = iNodes.front().match(iter - 1, aContext.iQueue.end(), search_type::Token);
                if (match.first == match_result::Partial)
                {
                    if (!next(aContext))
                        throw end_of_file_reached();
                    backup = true;
                    continue;
                }
                if (match.first == match_result::Complete)
                {
                    atom_type atom = atom_type{};
                    bool endToken = false;
                    if (std::holds_alternative<token_type>(match.second))
                        atom = atom_type{ static_variant_cast<token_type>(match.second) };
                    else if (std::holds_alternative<function_type>(match.second))
                    {
                        auto& functions = static_variant_cast<const function_type&>(match.second);
                        atom = atom_type{ functions.first };
                        auto iter2 = iter - 1;
                        for (auto f = functions.second.begin(); f != functions.second.end(); ++f)
                        {
                            switch (*f)
                            {
                            case lexer_atom_function::Eat:
                                if (iter2 != aContext.iQueue.end())
                                {
                                    std::ptrdiff_t diff = iter - iter2;
                                    iter2 = aContext.iQueue.erase(iter2);
                                    iter = iter2 + diff;
                                }
                                break;
                            case lexer_atom_function::Keep:
                                if (iter2 != aContext.iQueue.end())
                                    ++iter2;
                                break;
                            case lexer_atom_function::End:
                                endToken = true;
                                break;
                            }
                        }
                    }
                    for (auto iter2 = iter - 1; iter2 != aContext.iQueue.end(); ++iter2)
                        atom.token_value().append(iter2->token_value());
                    if (endToken)
                    {
                        aContext.iQueue.clear();
                        aContext.iQueue.push_back(atom);
                        break;
                    }
                    else
                    {
                        if (iter != aContext.iQueue.end() || aContext.iQueue.back() != atom)
                        {
                            aContext.iQueue.erase(iter - 1, aContext.iQueue.end());
                            aContext.iQueue.insert(aContext.iQueue.end(), atom);
                            backup = true;
                        }
                        else
                        {
                            if (iter - 1 == aContext.iQueue.begin())
                            {
                                if (next(aContext))
                                    backup = true;
                            }
                        }
                    }
                }
                else if (match.first == match_result::None)
                {
                    if (iter - 1 == aContext.iQueue.begin())
                    {
                        next(aContext);
                        break;
                    }
                }
            }
            auto match = iNodes.front().match(aContext.iQueue[0].token_value().begin(), aContext.iQueue[0].token_value().end(), search_type::String);
            if (match.first == match_result::Complete)
            {
                bool changed = false;
                if (std::holds_alternative<token_type>(match.second))
                {
                    auto token = static_variant_cast<token_type>(match.second);
                    if (aContext.iQueue[0].token() != token)
                    {
                        aContext.iQueue[0].set_token(token);
                        changed = true;
                    }
                }
                else if (std::holds_alternative<function_type>(match.second))
                {
                    auto token = static_variant_cast<const function_type&>(match.second).first;
                    if (aContext.iQueue[0].token() != token)
                    {
                        aContext.iQueue[0].set_token(token);
                        changed = true;
                    }
                }
                if (changed)
                    return get_token(aContext, aAtom); // recurse
            }
            aAtom = aContext.iQueue[0];
            aContext.iQueue.pop_front();
            return *this;
        }
        void build(const rule_type& aRule)
        {
            if (iNodes.empty())
                iNodes.push_back(node{ *this });
            build(aRule, 0u, iNodes.front());
        }
        node& build(const rule_type& aRule, std::size_t aExpressionIndex, node& aNode)
        {
            return build(aRule, aRule.expression[aExpressionIndex], aExpressionIndex, aNode);
        }
        node& build(const rule_type& aRule, const atom_type& aAtom, std::size_t aExpressionIndex, node& aNode, bool aHalt = false)
        {
            if (holds_alternative<range_type>(aAtom))
            {
                auto& r = static_variant_cast<const range_type&>(aAtom.value());
                for (char_type ch = r.first; ch < r.second; ++ch)
                    build(aRule, ch, aExpressionIndex, aNode);
                return build(aRule, r.second, aExpressionIndex, aNode);
            }
            else if (holds_alternative<string_type>(aAtom))
            {
                auto s = static_variant_cast<const std::string&>(aAtom.value());
                auto& n = build(aRule, s[0], aExpressionIndex, aNode, s.size() > 1 ? true : false);
                s = s.substr(1);
                if (!s.empty())
                    return build(aRule, s, aExpressionIndex, n);
                else
                    return n;
            }
            else if (aExpressionIndex == aRule.expression.size() - 1 && !aHalt)
            {
                aNode.map(aAtom, aRule.symbol);
                return aNode;
            }
            else
            {
                auto& next = aNode.lookup(aAtom);
                if (next.first != nullptr)
                {
                    if (!aHalt)
                        return build(aRule, aExpressionIndex + 1, *next.first);
                    else
                        return *next.first;
                }
                else
                {
                    iNodes.push_back(node{ *this });
                    auto& newNode = iNodes.back();
                    aNode.map(aAtom, newNode);
                    if (!aHalt)
                        return build(aRule, aExpressionIndex + 1, newNode);
                    else
                        return newNode;
                }
            }
        }
        bool next(context& aContext) const
        {
            const std::size_t BUF_SIZE = 32;
            if (aContext.iInputBuffer.empty())
            {
                aContext.iInputBuffer.resize(BUF_SIZE);
                aContext.iInput->read(&aContext.iInputBuffer[0], BUF_SIZE);
                std::streamsize amount = aContext.iInput->gcount();
                aContext.iInputBuffer.resize(static_cast<std::size_t>(amount));
                if (amount == 0)
                {
                    if (aContext.iCharIndex == 0)
                        aContext.iError = true;
                    aContext.iFinished = true;
                    return false;
                }
            }
            if (aContext.iCharIndex == 0)
            {
                const string_type BOM_UTF8 = "\xEF\xBB\xBF";
                const string_type BOM_UTF16LE = "\xFF\xFE";
                const string_type BOM_UTF16BE = "\xFE\xFF";
                if (aContext.iInputBuffer.find(BOM_UTF8) == 0)
                    aContext.iInputBuffer = aContext.iInputBuffer.substr(BOM_UTF8.size());
                else if (aContext.iInputBuffer.find(BOM_UTF16LE) == 0)
                    throw style_sheet_not_utf8();
                else if (aContext.iInputBuffer.find(BOM_UTF16BE) == 0)
                    throw style_sheet_not_utf8();
            }
            if (aContext.iPreviousChar == '\n' || aContext.iCharIndex == 0)
            {
                ++aContext.iLineIndex;
                aContext.iColumnIndex = 0;
            }
            ++aContext.iColumnIndex;
            ++aContext.iCharIndex;
            char_type ch = aContext.iInputBuffer[aContext.iInputBufferIndex];
            aContext.iPreviousChar = ch;
            aContext.iQueue.emplace_back(ch, string_type(1, ch));
            if (++aContext.iInputBufferIndex == aContext.iInputBuffer.size())
            {
                aContext.iInputBuffer.clear();
                aContext.iInputBufferIndex = 0;
            }
            return true;
        }
    private:
        scope_type iDefaultScope;
        node_list iNodes;
    };
}

max_stars_repo_name: madebr/neolib

max_issues_repo_name: madebr/neolib

max_forks_repo_name: madebr/neolib


text:
/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * Author: Wim Meeussen
 */

#include <boost/bind.hpp>

#include "robot_mechanism_controllers/cartesian_pose_controller.h"
#include <algorithm>
#include "kdl/chainfksolverpos_recursive.hpp"
#include "pluginlib/class_list_macros.h"
#include "tf_conversions/tf_kdl.h"

using namespace KDL;
using namespace tf;
using namespace std;

PLUGINLIB_EXPORT_CLASS( controller::CartesianPoseController, pr2_controller_interface::Controller)

namespace controller {

CartesianPoseController::CartesianPoseController()
: robot_state_(NULL)
{}

CartesianPoseController::~CartesianPoseController()
{
  command_filter_.reset();
}

bool CartesianPoseController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
{
  node_ = n;

  // get name of root and tip from the parameter server
  std::string tip_name;
  if (!node_.getParam("root_name", root_name_)){
    ROS_ERROR("CartesianPoseController: No root name found on parameter server (namespace: %s)",
              node_.getNamespace().c_str());
    return false;
  }
  if (!node_.getParam("tip_name", tip_name)){
    ROS_ERROR("CartesianPoseController: No tip name found on parameter server (namespace: %s)",
              node_.getNamespace().c_str());
    return false;
  }

  // test if we got robot pointer
  assert(robot_state);
  robot_state_ = robot_state;

  // create robot chain from root to tip
  if (!chain_.init(robot_state_, root_name_, tip_name))
    return false;
  if (!chain_.allCalibrated())
  {
    ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  chain_.toKDL(kdl_chain_);

  // create solver
  jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
  jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
  jnt_pos_.resize(kdl_chain_.getNrOfJoints());
  jnt_eff_.resize(kdl_chain_.getNrOfJoints());
  jacobian_.resize(kdl_chain_.getNrOfJoints());

  // create pid controller for the translation and for the rotation
  control_toolbox::Pid pid_controller;
  if (!pid_controller.init(ros::NodeHandle(node_,"fb_trans"))) return false;
  for (unsigned int i = 0; i < 3; i++)
    pid_controller_.push_back(pid_controller);
  if (!pid_controller.init(ros::NodeHandle(node_,"fb_rot"))) return false;
  for (unsigned int i = 0; i < 3; i++)
    pid_controller_.push_back(pid_controller);

  // subscribe to pose commands
  sub_command_.subscribe(node_, "command", 10);
  command_filter_.reset(new tf::MessageFilter<geometry_msgs::PoseStamped>(
                          sub_command_, tf_, root_name_, 10, node_));
  command_filter_->registerCallback(boost::bind(&CartesianPoseController::command, this, _1));

  // realtime publisher for control state
  state_error_publisher_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist>(node_, "state/error", 1));
  state_pose_publisher_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped>(node_, "state/pose", 1));

  return true;
}

void CartesianPoseController::starting()
{
  // reset pid controllers
  for (unsigned int i=0; i<6; i++)
    pid_controller_[i].reset();

  // initialize desired pose/twist
  twist_ff_ = Twist::Zero();
  pose_desi_ = getPose();
  last_time_ = robot_state_->getTime();

  loop_count_ = 0;
}

void CartesianPoseController::update()
{
  // get time
  ros::Time time = robot_state_->getTime();
  ros::Duration dt = time - last_time_;
  last_time_ = time;

  // get current pose
  pose_meas_ = getPose();

  // pose feedback into twist
  twist_error_ = diff(pose_meas_, pose_desi_);
  KDL::Wrench wrench_desi;
  for (unsigned int i=0; i<6; i++)
    wrench_desi(i) = pid_controller_[i].computeCommand(twist_error_(i), dt);

  // get the chain jacobian
  jac_solver_->JntToJac(jnt_pos_, jacobian_);

  // Converts the wrench into joint efforts with a jacbobian-transpose
  for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++){
    jnt_eff_(i) = 0;
    for (unsigned int j=0; j<6; j++)
      jnt_eff_(i) += (jacobian_(j,i) * wrench_desi(j));
  }

  // set effort to joints
  chain_.addEfforts(jnt_eff_);

  if (++loop_count_ % 100 == 0){
    if (state_error_publisher_){
      if (state_error_publisher_->trylock()){
        state_error_publisher_->msg_.linear.x = twist_error_.vel(0);
        state_error_publisher_->msg_.linear.y = twist_error_.vel(1);
        state_error_publisher_->msg_.linear.z = twist_error_.vel(2);
        state_error_publisher_->msg_.angular.x = twist_error_.rot(0);
	state_error_publisher_->msg_.angular.y = twist_error_.rot(1);
        state_error_publisher_->msg_.angular.z = twist_error_.rot(2);
        state_error_publisher_->unlockAndPublish();
      }
    }
    if (state_pose_publisher_){
      if (state_pose_publisher_->trylock()){
	Pose tmp;
        tf::poseKDLToTF(pose_meas_, tmp);
	poseStampedTFToMsg(Stamped<Pose>(tmp, ros::Time::now(), root_name_), state_pose_publisher_->msg_);
        state_pose_publisher_->unlockAndPublish();
      }
    }
  }
}

Frame CartesianPoseController::getPose()
{
  // get the joint positions and velocities
  chain_.getPositions(jnt_pos_);

  // get cartesian pose
  Frame result;
  jnt_to_pose_solver_->JntToCart(jnt_pos_, result);

  return result;
}

void CartesianPoseController::command(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
{
  // convert message to transform
  Stamped<Pose> pose_stamped;
  poseStampedMsgToTF(*pose_msg, pose_stamped);

  // convert to reference frame of root link of the controller chain
  tf_.transformPose(root_name_, pose_stamped, pose_stamped);
  tf::poseTFToKDL(pose_stamped, pose_desi_);
}

} // namespace

ext: cpp


text:
#include <smtbx/refinement/constraints/geometrical_hydrogens.h>
#include <scitbx/sparse/io.h>

#include <boost/lambda/lambda.hpp>
#include <scitbx/array_family/tiny_reductions.h>

namespace smtbx { namespace refinement { namespace constraints {

  //*** Base class ***

  template <int n_hydrogens>
  index_range
  geometrical_hydrogen_sites<n_hydrogens>
  ::component_indices_for(scatterer_type const *scatterer) const
  {
    using boost::lambda::_1;
    boost::optional<std::size_t>
    i_sc = af::first_index(hydrogen, _1 == scatterer);
    return i_sc ? index_range(index() + 3*(*i_sc), 3)
                : index_range();
  }

  template <int n_hydrogens>
  void geometrical_hydrogen_sites<n_hydrogens>
  ::write_component_annotations_for(scatterer_type const *scatterer,
                                    std::ostream &output) const
  {
    using boost::lambda::_1;
    boost::optional<std::size_t>
    i_sc = af::first_index(hydrogen, _1 == scatterer);
    if (i_sc) {
      scatterer_type const *h = hydrogen[*i_sc];
      output << h->label << ".x,";
      output << h->label << ".y,";
      output << h->label << ".z,";
    }
  }

  //*** CH3, NH2, OH ***

  template <int n_hydrogens, bool staggered>
  void
  terminal_tetrahedral_xhn_sites<n_hydrogens, staggered>
  ::linearise(uctbx::unit_cell const &unit_cell,
              sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter
    *pivot           = dynamic_cast<site_parameter *>(this->argument(0)),
    *pivot_neighbour = dynamic_cast<site_parameter *>(this->argument(1));
    scalar_parameter *azimuth, *length;
    site_parameter *stagger;
    if (staggered) stagger = dynamic_cast<site_parameter *>(this->argument(2));
    else           azimuth = dynamic_cast<scalar_parameter *>(this->argument(2));
    length  = dynamic_cast<scalar_parameter *>(this->argument(3));

    // Local frame
    cart_t x_p = unit_cell.orthogonalize(pivot->value),
           x_pn = unit_cell.orthogonalize(pivot_neighbour->value);
    if (staggered) {
      cart_t x_s = unit_cell.orthogonalize(stagger->value);
      e_zero_azimuth = x_s - x_pn;
    }
    af::tiny<cart_t, 3>
    e = scitbx::math::orthonormal_basis(x_p - x_pn, e_zero_azimuth);

    double l = length->value;
    double cos_phi, sin_phi;
    if (!staggered) {
      double phi = azimuth->value;
      cos_phi = std::cos(phi);
      sin_phi = std::sin(phi);
    }
    else {
      cos_phi = -1.;
      sin_phi = 0.;
    }

    // Loop over the Hydrogen atoms
    for (int k=0; k < n_hydrogens; ++k) {

      // Cosine and Sine of the azimutal angle of the k-th Hydrogen
      /* Mathematica:
       Table[TrigExpand[Cos[\[Phi] + n Pi/3]], {n, {2, 4}}]
       Table[TrigExpand[Sin[\[Phi] + n Pi/3]], {n, {2, 4}}]
       */
      double c, s;
      switch (k) {
        case 0:
          // 1st Hydrogen: azimuthal angle = phi
          c = cos_phi;
          s = sin_phi;
          break;
        case 1:
          // 2nd Hydrogen: azimuthal angle = phi + 2pi/3
          c = -0.5        *cos_phi - half_sqrt_3*sin_phi;
          s =  half_sqrt_3*cos_phi -         0.5*sin_phi;
          break;
        case 2:
          // 3rd Hydrogen: azimuthal angle = phi + 4pi/3
          c = -0.5        *cos_phi + half_sqrt_3*sin_phi;
          s = -half_sqrt_3*cos_phi -         0.5*sin_phi;
        default:
          break;
      }

      // Site of k-th Hydrogen
      cart_t u = sin_tetrahedral_angle*(c*e[1] + s*e[2]) + e[0]/3.;
      this->x_h[k] = x_p + l*u;

      // Derivatives
      if (!jacobian_transpose) continue;
      sparse_matrix_type &jt = *jacobian_transpose;
      std::size_t const j_h = this->index() + 3*k;

      // Riding
      for (int i=0; i<3; ++i) {
        jt.col(j_h + i) = jt.col(pivot->index() + i);
      }

      /** We take advantage of the fact that azimuth and length are
          independent variables. So jt.col(azimuth->index()) is either
          zero or is a column of the identity matrix.
       */

      // Rotation
      if (!staggered && azimuth->is_variable()) {
        cart_t grad_c = l*sin_tetrahedral_angle*(-s*e[1] + c*e[2]);
        frac_t grad_f = unit_cell.fractionalize(grad_c);
        for (int i=0; i<3; ++i) jt(azimuth->index(), j_h + i) = grad_f[i];
      }

      // Bond stretching
      if (length->is_variable()) {
        frac_t grad_f = unit_cell.fractionalize(u);
        for (int i=0; i<3; ++i) jt(length->index(), j_h + i) = grad_f[i];
      }
    }
  }

  template class terminal_tetrahedral_xhn_sites<1, /*staggered=*/false>;
  template class terminal_tetrahedral_xhn_sites<2, /*staggered=*/false>;
  template class terminal_tetrahedral_xhn_sites<3, /*staggered=*/false>;

  template class terminal_tetrahedral_xhn_sites<1, /*staggered=*/true>;
  template class terminal_tetrahedral_xhn_sites<2, /*staggered=*/true>;
  template class terminal_tetrahedral_xhn_sites<3, /*staggered=*/true>;

  // angle parameter

  void angle_parameter::linearise(uctbx::unit_cell const &unit_cell,
                         sparse_matrix_type *jacobian_transpose)
  {
    site_parameter *sites[] = {
      dynamic_cast<site_parameter *>(argument(0)),
      dynamic_cast<site_parameter *>(argument(1)),
      dynamic_cast<site_parameter *>(argument(2))
    };
    cart_t crds[] = {
      unit_cell.orthogonalize(sites[0]->value),
      unit_cell.orthogonalize(sites[1]->value),
      unit_cell.orthogonalize(sites[2]->value)
    };
    value = (crds[0]-crds[1]).angle(crds[2]-crds[1]);
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    // http://salilab.org/modeller/8v0/manual/node248.html
    cart_t ij = (crds[0] - crds[1]);
    cart_t kj = (crds[2] - crds[1]);
    const double ij_l = ij.length(), kj_l = kj.length();
    ij = ij.normalize();
    kj = kj.normalize();
    const double ca = ij*kj;
    //if( std::abs(ca) >= 1.0-1e-16 )
    const double oos = 1./std::sqrt(1-ca*ca);
    cart_t grad[3];
    grad[0] = (ij*ca - kj)*oos/ij_l;  // d(angle)/d(left)
    grad[2] = (kj*ca - ij)*oos/kj_l;  // d(angle)/d(right)
    grad[1] = -(grad[0] + grad[2]);   // d(angle)/d(center)
    for (int i=0; i < 3; i++)  {
      frac_t grad_f = unit_cell.fractionalize(grad[i]);
      for (int j=0; j < 3; j++) {
        jt(sites[i]->index()+j, index()) = grad_f[j];
      }
    }
  }

  // X-CH2-Y

  void secondary_xh2_sites::linearise(uctbx::unit_cell const &unit_cell,
                                      sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter
    *pivot             = dynamic_cast<site_parameter *>(argument(0)),
    *pivot_neighbour_0 = dynamic_cast<site_parameter *>(argument(1)),
    *pivot_neighbour_1 = dynamic_cast<site_parameter *>(argument(2));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(3));
    scalar_parameter *h_c_h = dynamic_cast<scalar_parameter *>(argument(4));

    bool is_angle_param = dynamic_cast<angle_parameter *>(argument(4)) != 0;
    // Local frame
    /* (C, e0, e1) is the bisecting plane of the angle X-C-Y
        with e0 bisecting X-C-Y
     */
    cart_t x_p  = unit_cell.orthogonalize(pivot->value);
    cart_t x_pn_0 = unit_cell.orthogonalize(pivot_neighbour_0->value),
           x_pn_1 = unit_cell.orthogonalize(pivot_neighbour_1->value);
    cart_t u_pn_0 = (x_p - x_pn_0).normalize(),
           u_pn_1 = (x_p - x_pn_1).normalize();
    cart_t e0 = (u_pn_1 + u_pn_0).normalize();
    cart_t e2 = (u_pn_1 - u_pn_0).normalize();
    cart_t e1 = e2.cross(e0);
    double l = length->value,
      theta = h_c_h->value,
      cos_phi = 0;
    const double theta_to_phi_const = 0.0698;
    // calculate new theta/2 = 0.9678 + 0.0698 cos phi as in shelxl
    if (is_angle_param) {
      cos_phi = u_pn_0*u_pn_1;
      theta = 2*(0.9678 + theta_to_phi_const*cos_phi);
    }
    // Hydrogen sites
    double c = std::cos(theta/2), s = std::sin(theta/2);
    af::tiny<cart_t, 2> u_h(c*e0 + s*e1, c*e0 - s*e1);
    for (int k=0; k<2; ++k) x_h[k] = x_p + l*u_h[k];

    // Derivatives
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    af::tiny<std::size_t, 2> j_h(index(), index() + 3);

    // Riding
    for (int k=0; k<2; ++k) {
      for (int i=0; i<3; ++i)
        jt.col(j_h[k] + i) = jt.col(pivot->index() + i);
    }

    // Bond stretching
    if (length->is_variable()) {
      for (int k=0; k<2; ++k) {
        frac_t grad_f = unit_cell.fractionalize(u_h[k]);
        for (int i=0; i<3; ++i) jt(length->index(), j_h[k] + i) = grad_f[i];
      }
    }

    // H-C-H flapping
    if (h_c_h->is_variable() && !is_angle_param) {
      af::tiny<cart_t, 2> grad_c(l/2*(-s*e0 + c*e1), l/2*(-s*e0 - c*e1));
      for (int k=0; k<2; ++k) {
        frac_t grad_f = unit_cell.fractionalize(grad_c[k]);
        for (int i=0; i<3; ++i) jt(h_c_h->index(), j_h[k] + i) = grad_f[i];
      }
    }
    else if (is_angle_param) {
      /*
      d(cos(theta/2))/d(phi) = d(cos(theta/2)/d(theta)*d(theta)/d(phi) =
        sin(theta/2)*theta_to_phi_const*sin(phi)
      d(sin(theta/2))/d(phi) = d(sin(theta/2)/d(theta)*d(theta)/d(phi) =
        cos(theta/2)*theta_to_phi_const*cos(phi)
      */
      double sin_phi = std::sqrt(1-cos_phi*cos_phi),
        k = l*theta_to_phi_const/2;
      af::tiny<cart_t, 2> grad_c(
        k*(s*sin_phi*e0 + c*cos_phi*e1),
        k*(s*sin_phi*e0 - c*cos_phi*e1));
      for (int i=0; i < 2; i++) {
        frac_t grad_f = unit_cell.fractionalize(grad_c[i]);
        for (int j=0; j < 3; j++)  {
          jt(j_h[i]+j, h_c_h->index()) = grad_f[j];
        }
      }
    }
  }

  /***    H
          |
       X0-C-X1
          |
          X2
   */
  void tertiary_xh_site::linearise(uctbx::unit_cell const &unit_cell,
                                   sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    af::tiny<site_parameter *, 3> pivot_neighbour;
    for (int k=0; k<3; ++k) {
      pivot_neighbour[k] = dynamic_cast<site_parameter *>(argument(k+1));
    }
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(4));

    // Local frame
    cart_t x_p = unit_cell.orthogonalize(pivot->value);
    af::tiny<cart_t, 3> u_cn; // Directions C->Xi
    for (int k=0; k<3; ++k) {
      cart_t x = unit_cell.orthogonalize(pivot_neighbour[k]->value);
      u_cn[k] = (x_p - x).normalize();
    }
    cart_t u = u_cn[0] - u_cn[1], v = u_cn[1] - u_cn[2];
    cart_t e0 = u.cross(v).normalize();
    if (e0*(u_cn[0] + u_cn[1] + u_cn[2]) < 0) e0 = -e0;
    double l = length->value;

    // Hydrogen site
    x_h[0] = x_p + l*e0;

    // Derivatives
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int i=0; i<3; ++i) {
      jt.col(j_h + i) = jt.col(pivot->index() + i);
    }

    // Bond stretching
    if (length->is_variable()) {
      frac_t grad_f = unit_cell.fractionalize(e0);
      for (int i=0; i<3; ++i) jt(length->index(), j_h + i) = grad_f[i];
    }
  }

  /// aromatic or amide Y-XH-Z
  void secondary_planar_xh_site::linearise(uctbx::unit_cell const &unit_cell,
                                           sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    site_parameter
    *pivot_neighbour_0 = dynamic_cast<site_parameter *>(argument(1)),
    *pivot_neighbour_1 = dynamic_cast<site_parameter *>(argument(2));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(3));

    // Local frame
    cart_t x_p = unit_cell.orthogonalize(pivot->value);
    cart_t
    u_yx = (x_p - unit_cell.orthogonalize(pivot_neighbour_0->value)).normalize(),
    u_zx = (x_p - unit_cell.orthogonalize(pivot_neighbour_1->value)).normalize();
    cart_t e0 = (u_yx + u_zx).normalize();
    double l = length->value;

    // Hydrogen site
    x_h[0] = x_p + l*e0;

    // Jacobian
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int i=0; i<3; ++i) jt.col(j_h + i) = jt.col(pivot->index() + i);

    // Bond stretching
    if (length->is_variable()) {
      frac_t grad_f = unit_cell.fractionalize(e0);
      for (int i=0; i<3; ++i) jt(length->index(), j_h + i) = grad_f[i];
    }
  }

  // Terminal Z-Y=XH2
  void terminal_planar_xh2_sites::
  linearise(uctbx::unit_cell const &unit_cell,
            sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    site_parameter
    *pivot_neighbour = dynamic_cast<site_parameter *>(argument(1));
    site_parameter
    *pivot_neighbour_substituent = dynamic_cast<site_parameter *>(argument(2));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(3));

    // Local frame
    cart_t p = unit_cell.orthogonalize(pivot->value);
    cart_t y = unit_cell.orthogonalize(pivot_neighbour->value);
    cart_t z = unit_cell.orthogonalize(pivot_neighbour_substituent->value);
    cart_t e0 = (p - y).normalize();
    cart_t u_yz = z - y;
    cart_t e1 = (e0 - 1/(e0*u_yz) * u_yz).normalize();
    double l = length->value;

    // Hydrogen sites
    af::tiny<cart_t, 2> u_h(0.5*e0 + half_sqrt_3*e1,
                            0.5*e0 - half_sqrt_3*e1);
    for (int k=0; k<2; ++k) x_h[k] = p + l*u_h[k];

    // Jacobian
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int k=0; k<2; ++k) for (int i=0; i<3; ++i) {
      jt.col(j_h + 3*k + i) = jt.col(pivot->index() + i);
    }

    // Bond stretching
    if (length->is_variable()) {
      for (int k=0; k<2; ++k) {
        frac_t grad_f = unit_cell.fractionalize(u_h[k]);
        for (int i=0; i<3; ++i) jt(length->index(), j_h + 3*k + i) = grad_f[i];
      }
    }
  }

  // Acetylenic X-CH
  void terminal_linear_ch_site
  ::linearise(uctbx::unit_cell const &unit_cell,
              sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    site_parameter *pivot_neighbour = dynamic_cast<site_parameter *>(argument(1));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(2));

    // Local frame
    cart_t p = unit_cell.orthogonalize(pivot->value);
    cart_t x = unit_cell.orthogonalize(pivot_neighbour->value);
    cart_t e0 = (p - x).normalize();
    double l = length->value;

    // Hydrogen site
    x_h[0] = p + l*e0;

    // Jacobian
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int i=0; i<3; ++i) jt.col(j_h + i) = jt.col(pivot->index() + i);

    // Bond stretching
    if (length->is_variable()) {
      frac_t grad_f = unit_cell.fractionalize(e0);
      for (int i=0; i<3; ++i) jt(length->index(), j_h + i) = grad_f[i];
    }
  }

  // boron cage B(n)-H
  void polyhedral_bh_site
  ::linearise(uctbx::unit_cell const &unit_cell,
              sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(1));
    const cart_t c = unit_cell.orthogonalize(pivot->value);
    cart_t p(0,0,0);
    for (int i=2; i<n_arguments(); i++)  {
      p = p + (unit_cell.orthogonalize(
        dynamic_cast<site_parameter *>(argument(i))->value) - c).normalize();
    }
    p = -p.normalize();
    double l = length->value;
    // Hydrogen site
    x_h[0] = c + p*l;

    // Jacobian
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int i=0; i<3; i++)
      jt.col(j_h + i) = jt.col(pivot->index() + i);

    // Bond stretching
    if (length->is_variable()) {
      frac_t grad_f = unit_cell.fractionalize(p);
      for (int i=0; i<3; i++)
        jt(length->index(), j_h + i) = grad_f[i];
    }
  }

  // Explicitly instantiate template classes used in other libraries
  template class geometrical_hydrogen_sites<1>;
  template class geometrical_hydrogen_sites<2>;
  template class geometrical_hydrogen_sites<3>;
}}}


text:
#pragma once
#include <boost/function.hpp>

namespace RavEngine{

template<typename ... A>
using Function = boost::function<A...>;

}

max_stars_repo_name: Ravbug/RavEngine

max_issues_repo_name: Ravbug/RavEngine

max_forks_repo_name: Ravbug/RavEngine


text:
#include "SoundManager.h"

#include <boost/scoped_ptr.hpp>
#include "../../include/gameaudio/Error.h"
#include "SoundAbstract.h"
#include "Listener.h"
#include "../../include/gameaudio/IFileFactory.h"
#include <algorithm>
#include "FileFactory.h"

#include "LoopSound.h"
#include "StaticSound.h"
#include "StreamSound.h"

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#ifdef _WIN32
#include <Windows.h>
#endif

using namespace gameaudio;

namespace {
	SoundManager *gSoundManager;
	class SMDeleter {
	public:
		~SMDeleter() {
			gameaudio::releaseSoundManager();
		};
	} gSMDeleter;
}

ISoundManager& gameaudio::getSoundManager() {
	if (!gSoundManager) {
		gSoundManager = new SoundManager();
	}
	return *(gSoundManager);
}

void gameaudio::releaseSoundManager() {
	delete gSoundManager;
	gSoundManager = 0;
}

SoundManager::SoundManager()
	: _device(NULL), _context(NULL), _listener(NULL), _defaultFileFactory(NULL),
	_mutex(NULL), _thread(NULL), _threadEnd(false), _modified(false), _lastModified(NULL)
{
	_device = alcOpenDevice(NULL);
	if (!_device) {
		_device = alcOpenDevice("Generic Software");
		if (!_device) {
			throw Error("Cannot open OpenAL device");
		}
	}
	_context = alcCreateContext(_device, NULL);
	if (!_device) throw Error("Cannot create OpenAL context");
	alcMakeContextCurrent(_context);

	_defaultFileFactory = new FileFactory();
	addFileFactory(_defaultFileFactory);
	_listener = new Listener();

	_mutex = new boost::mutex();
	_thread = new boost::thread(boost::bind(&SoundManager::_update, this));

#ifdef _WIN32
	SetThreadPriority(_thread->native_handle(), THREAD_PRIORITY_TIME_CRITICAL);
#endif
}

SoundManager::~SoundManager() {
	{
		boost::mutex::scoped_lock lock(*_mutex);
		_threadEnd = true;
	}
	if (_thread->joinable()) _thread->join();
	delete _thread;
	delete _mutex;

	foreach (ISound *sound, _sounds) {
		delete sound;
	}
	_sounds.clear();

	_fileFactories.clear();
	delete _defaultFileFactory;

	alcMakeContextCurrent(NULL);
	alcDestroyContext(_context);
	alcCloseDevice(_device);
}

void SoundManager::_update() {
	std::set<SoundAbstract*>::iterator itr;
	bool is_empty = true;

	while (true) {
		{
			boost::mutex::scoped_lock lock(*_mutex);
			if (_sounds.empty()) {
				is_empty = true;
			} else {
				if (_modified) {
					itr = _sounds.upper_bound(_lastModified);
					if (itr == _sounds.end()) itr = _sounds.begin();
					_modified = false;
				}
				if (is_empty) {
					itr = _sounds.begin();
					is_empty = false;
				} else {
					++itr;
					if (itr == _sounds.end()) itr = _sounds.begin();
				}
				(*itr)->update();
			}
			if (_threadEnd) break;
		}

		boost::thread::sleep(boost::get_system_time()+boost::posix_time::milliseconds(1));
	}
}

template<typename T> ISound::encoding_type detectEncodingType(T path) {
	ISound::encoding_type encoding;
	if (path.rfind(".ogg") == path.size() - 4) {
		encoding = ISound::ENCODING_OGG_VORBIS;
	} else {
		encoding = ISound::ENCODING_WAV;
	}
	return encoding;
}
template<> ISound::encoding_type detectEncodingType(std::wstring path) {
	ISound::encoding_type encoding;
	if (path.rfind(L".ogg") == path.size() - 4) {
		encoding = ISound::ENCODING_OGG_VORBIS;
	} else {
		encoding = ISound::ENCODING_WAV;
	}
	return encoding;
}

SoundAbstract* SoundManager::_createSound(IFileReader* reader, ISound::encoding_type encoding, bool stream, bool loop, unsigned looppos) {
	SoundAbstract *sound;
	boost::shared_ptr<IFileReader> sreader(reader);

	if (stream) {
		sound = new StreamSound(sreader, encoding, loop, looppos);
	} else {
		if (loop) {
			sound = new LoopSound(sreader, encoding, looppos);
		} else {
			sound = new StaticSound(sreader, encoding);
		}
	}
	return sound;
}

ISound* SoundManager::createSound(const char* filepath, bool loop, bool stream, unsigned looppos, ISound::encoding_type encoding) {
	IFileReader* reader = NULL;
	foreach (IFileFactory* factory, _fileFactories) {
		if (reader = factory->createFileReader(filepath)) break;
	}
	if (reader == NULL) {
		throw Error("Cannot find available file reader");
	}
	if (encoding == ISound::ENCODING_AUTO) {
		encoding = detectEncodingType(std::string(filepath));
	}

	SoundAbstract* sound = _createSound(reader, encoding, stream, loop, looppos);
	{
		boost::mutex::scoped_lock lock(*_mutex);
		_sounds.insert(sound);
		_modified = true;
		_lastModified = sound;
	}
	return sound;
}

ISound* SoundManager::createSound(const wchar_t* filepath, bool loop, bool stream, unsigned looppos, ISound::encoding_type encoding) {
	IFileReader* reader = NULL;
	foreach (IFileFactory* factory, _fileFactories) {
		if (reader = factory->createFileReader(filepath)) break;
	}
	if (reader == NULL) {
		throw Error("Cannot find available file reader");
	}
	if (encoding == ISound::ENCODING_AUTO) {
		encoding = detectEncodingType(std::wstring(filepath));
	}

	SoundAbstract* sound = _createSound(reader, encoding, stream, loop, looppos);
	{
		boost::mutex::scoped_lock lock(*_mutex);
		_sounds.insert(sound);
		_modified = true;
		_lastModified = sound;
	}
	return sound;
}

void SoundManager::removeSound(ISound* s) {
	boost::mutex::scoped_lock lock(*_mutex);
	SoundAbstract *sound = dynamic_cast<SoundAbstract*>(s);
	_sounds.erase(sound);
	_modified = true;
	_lastModified = sound;
}

IListener* SoundManager::getListener() const {
	return _listener;
}

void SoundManager::addFileFactory(IFileFactory* factory) {
	if (std::find(_fileFactories.begin(), _fileFactories.end(), factory) != _fileFactories.end()) {
		throw Error("The FileFactory has already added");
	}
	_fileFactories.push_front(factory);
}

void SoundManager::removeFileFactory(IFileFactory* factory) {
	std::list<IFileFactory*>::iterator itr = std::find(_fileFactories.begin(), _fileFactories.end(), factory);
	if (itr == _fileFactories.end()) {
		throw Error("The FileFactory is not added");
	}
	_fileFactories.erase(itr);
}

void SoundManager::setDopplerFactor(float v) {
	alDopplerFactor(v);
}

float SoundManager::getDopplerFactor() {
	return alGetFloat(AL_DOPPLER_FACTOR);
}

void SoundManager::setSpeedOfSound(float v) {
	alSpeedOfSound(v);
}

float SoundManager::getSpeedOfSound() {
	return alGetFloat(AL_SPEED_OF_SOUND);
}

max_stars_repo_name: YosukeM/GameAudio

max_stars_repo_licenses: Unlicense MIT

max_issues_repo_name: YosukeM/GameAudio

max_issues_repo_licenses: Unlicense MIT

max_forks_repo_name: YosukeM/GameAudio

max_forks_repo_licenses: Unlicense MIT


text:
//=======================================================================
// Copyright Baptiste Wicht 2011-2016.
// Distributed under the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#ifndef AST_TERNARY_H
#define AST_TERNARY_H

#include <boost/fusion/include/adapt_struct.hpp>

namespace eddic {

namespace ast {

/*!
 * \class ASTTernary
 * \brief The AST node for a ternary operator.
 */
struct Ternary : x3::file_position_tagged {
    Value condition;
    Value true_value;
    Value false_value;

};

} //end of ast

} //end of eddic

//Adapt the struct for the AST
BOOST_FUSION_ADAPT_STRUCT(
    eddic::ast::Ternary,
    (eddic::ast::Value, condition)
    (eddic::ast::Value, true_value)
    (eddic::ast::Value, false_value)
)

#endif

max_stars_repo_name: wichtounet/eddic

max_issues_repo_name: wichtounet/eddic

max_forks_repo_name: wichtounet/eddic


text:
/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file DiscreteBayesNet.cpp
 * @date Feb 15, 2011
 * @author Duy-Nguyen Ta
 * @author Frank Dellaert
 */

#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/FactorGraph-inst.h>

#include <boost/make_shared.hpp>
#include <boost/range/adaptor/reversed.hpp>

namespace gtsam {

  // Instantiate base class
  template class FactorGraph<DiscreteConditional>;

  /* ************************************************************************* */
  bool DiscreteBayesNet::equals(const This& bn, double tol) const
  {
    return Base::equals(bn, tol);
  }

  /* ************************************************************************* */
//  void DiscreteBayesNet::add_front(const Signature& s) {
//    push_front(boost::make_shared<DiscreteConditional>(s));
//  }

  /* ************************************************************************* */
  void DiscreteBayesNet::add(const Signature& s) {
    push_back(boost::make_shared<DiscreteConditional>(s));
  }

  /* ************************************************************************* */
  double DiscreteBayesNet::evaluate(const DiscreteConditional::Values & values) const {
    // evaluate all conditionals and multiply
    double result = 1.0;
    for(DiscreteConditional::shared_ptr conditional: *this)
      result *= (*conditional)(values);
    return result;
  }

  /* ************************************************************************* */
  DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const {
    // solve each node in turn in topological sort order (parents first)
    DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
    for (auto conditional: boost::adaptors::reverse(*this))
      conditional->solveInPlace(*result);
    return result;
  }

  /* ************************************************************************* */
  DiscreteFactor::sharedValues DiscreteBayesNet::sample() const {
    // sample each node in turn in topological sort order (parents first)
    DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
    for (auto conditional: boost::adaptors::reverse(*this))
      conditional->sampleInPlace(*result);
    return result;
  }

/* ************************************************************************* */
} // namespace


text: #include <boost/test/utils/runtime/argument.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*

 */

#ifndef aingle_ResolverSchema_hh__
#define aingle_ResolverSchema_hh__

#include <boost/noncopyable.hpp>
#include <cstdint>
#include <memory>

#include "Config.hh"
#include "Reader.hh"

/// \file ResolverSchema.hh
///

namespace aingle {

class ValidSchema;
class Layout;
class Resolver;

class AINGLE_DECL ResolverSchema {
public:
    ResolverSchema(const ValidSchema &writer, const ValidSchema &reader, const Layout &readerLayout);

private:
    friend class ResolvingReader;
    void parse(Reader &reader, uint8_t *address);
    std::shared_ptr<Resolver> resolver_;
};

} // namespace aingle

#endif

ext: hh

max_stars_repo_name: AIngleLab/aae

max_issues_repo_name: AIngleLab/aae

max_forks_repo_name: AIngleLab/aae


text:
// Copyright (c) 2015 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <pow.h>

#include <chain.h>
#include <chainparams.h>
#include <config.h>
#include <random.h>
#include <util.h>

#include <test/test_bitcoin.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(pow_tests, BasicTestingSetup)

/* Test calculation of next difficulty target with no constraints applying */
BOOST_AUTO_TEST_CASE(get_next_work) {
    DummyConfig config(CBaseChainParams::MAIN);

    int64_t nLastRetargetTime = 1261130161; // Block #30240
    CBlockIndex pindexLast;
    pindexLast.nHeight = 32255;
    pindexLast.nTime = 1262152739; // Block #32255
    pindexLast.nBits = 0x1d00ffff;
    BOOST_CHECK_EQUAL(
        CalculateNextWorkRequired(&pindexLast, nLastRetargetTime,
                                  config.GetChainParams().GetConsensus()),
        0x1d00d86a);
}

/* Test the constraint on the upper bound for next work */
BOOST_AUTO_TEST_CASE(get_next_work_pow_limit) {
    DummyConfig config(CBaseChainParams::MAIN);

    int64_t nLastRetargetTime = 1231006505; // Block #0
    CBlockIndex pindexLast;
    pindexLast.nHeight = 2015;
    pindexLast.nTime = 1233061996; // Block #2015
    pindexLast.nBits = 0x1d00ffff;
    BOOST_CHECK_EQUAL(
        CalculateNextWorkRequired(&pindexLast, nLastRetargetTime,
                                  config.GetChainParams().GetConsensus()),
        0x1d00ffff);
}

/* Test the constraint on the lower bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_lower_limit_actual) {
    DummyConfig config(CBaseChainParams::MAIN);

    int64_t nLastRetargetTime = 1279008237; // Block #66528
    CBlockIndex pindexLast;
    pindexLast.nHeight = 68543;
    pindexLast.nTime = 1279297671; // Block #68543
    pindexLast.nBits = 0x1c05a3f4;
    BOOST_CHECK_EQUAL(
        CalculateNextWorkRequired(&pindexLast, nLastRetargetTime,
                                  config.GetChainParams().GetConsensus()),
        0x1c0168fd);
}

/* Test the constraint on the upper bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_upper_limit_actual) {
    DummyConfig config(CBaseChainParams::MAIN);

    int64_t nLastRetargetTime = 1263163443; // NOTE: Not an actual block time
    CBlockIndex pindexLast;
    pindexLast.nHeight = 46367;
    pindexLast.nTime = 1269211443; // Block #46367
    pindexLast.nBits = 0x1c387f6f;
    BOOST_CHECK_EQUAL(
        CalculateNextWorkRequired(&pindexLast, nLastRetargetTime,
                                  config.GetChainParams().GetConsensus()),
        0x1d00e1fd);
}

BOOST_AUTO_TEST_CASE(GetBlockProofEquivalentTime_test) {
    DummyConfig config(CBaseChainParams::MAIN);

    std::vector<CBlockIndex> blocks(10000);
    for (int i = 0; i < 10000; i++) {
        blocks[i].pprev = i ? &blocks[i - 1] : nullptr;
        blocks[i].nHeight = i;
        blocks[i].nTime =
            1269211443 +
            i * config.GetChainParams().GetConsensus().nPowTargetSpacing;
        blocks[i].nBits = 0x207fffff; /* target 0x7fffff000... */
        blocks[i].nChainWork =
            i ? blocks[i - 1].nChainWork + GetBlockProof(blocks[i])
              : arith_uint256(0);
    }

    for (int j = 0; j < 1000; j++) {
        CBlockIndex *p1 = &blocks[InsecureRandRange(10000)];
        CBlockIndex *p2 = &blocks[InsecureRandRange(10000)];
        CBlockIndex *p3 = &blocks[InsecureRandRange(10000)];

        int64_t tdiff = GetBlockProofEquivalentTime(
            *p1, *p2, *p3, config.GetChainParams().GetConsensus());
        BOOST_CHECK_EQUAL(tdiff, p1->GetBlockTime() - p2->GetBlockTime());
    }
}

static CBlockIndex GetBlockIndex(CBlockIndex *pindexPrev, int64_t nTimeInterval,
                                 uint32_t nBits) {
    CBlockIndex block;
    block.pprev = pindexPrev;
    block.nHeight = pindexPrev->nHeight + 1;
    block.nTime = pindexPrev->nTime + nTimeInterval;
    block.nBits = nBits;

    block.nChainWork = pindexPrev->nChainWork + GetBlockProof(block);
    return block;
}

BOOST_AUTO_TEST_CASE(retargeting_test) {
    DummyConfig config(CBaseChainParams::MAIN);

    std::vector<CBlockIndex> blocks(115);

    const Consensus::Params &params = config.GetChainParams().GetConsensus();
    const arith_uint256 powLimit = UintToArith256(params.powLimit);
    arith_uint256 currentPow = powLimit >> 1;
    uint32_t initialBits = currentPow.GetCompact();

    // Genesis block.
    blocks[0] = CBlockIndex();
    blocks[0].nHeight = 0;
    blocks[0].nTime = 1269211443;
    blocks[0].nBits = initialBits;

    blocks[0].nChainWork = GetBlockProof(blocks[0]);

    // Pile up some blocks.
    for (size_t i = 1; i < 100; i++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], params.nPowTargetSpacing,
                                  initialBits);
    }

    CBlockHeader blkHeaderDummy;

    // We start getting 2h blocks time. For the first 5 blocks, it doesn't
    // matter as the MTP is not affected. For the next 5 block, MTP difference
    // increases but stays below 12h.
    for (size_t i = 100; i < 110; i++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 2 * 3600, initialBits);
        BOOST_CHECK_EQUAL(
            GetNextWorkRequired(&blocks[i], &blkHeaderDummy, params),
            initialBits);
    }

    // Now we expect the difficulty to decrease.
    blocks[110] = GetBlockIndex(&blocks[109], 2 * 3600, initialBits);
    currentPow.SetCompact(currentPow.GetCompact());
    currentPow += (currentPow >> 2);
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&blocks[110], &blkHeaderDummy, params),
        currentPow.GetCompact());

    // As we continue with 2h blocks, difficulty continue to decrease.
    blocks[111] =
        GetBlockIndex(&blocks[110], 2 * 3600, currentPow.GetCompact());
    currentPow.SetCompact(currentPow.GetCompact());
    currentPow += (currentPow >> 2);
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&blocks[111], &blkHeaderDummy, params),
        currentPow.GetCompact());

    // We decrease again.
    blocks[112] =
        GetBlockIndex(&blocks[111], 2 * 3600, currentPow.GetCompact());
    currentPow.SetCompact(currentPow.GetCompact());
    currentPow += (currentPow >> 2);
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&blocks[112], &blkHeaderDummy, params),
        currentPow.GetCompact());

    // We check that we do not go below the minimal difficulty.
    blocks[113] =
        GetBlockIndex(&blocks[112], 2 * 3600, currentPow.GetCompact());
    currentPow.SetCompact(currentPow.GetCompact());
    currentPow += (currentPow >> 2);
    BOOST_CHECK(powLimit.GetCompact() != currentPow.GetCompact());
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&blocks[113], &blkHeaderDummy, params),
        powLimit.GetCompact());

    // Once we reached the minimal difficulty, we stick with it.
    blocks[114] = GetBlockIndex(&blocks[113], 2 * 3600, powLimit.GetCompact());
    BOOST_CHECK(powLimit.GetCompact() != currentPow.GetCompact());
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&blocks[114], &blkHeaderDummy, params),
        powLimit.GetCompact());
}

BOOST_AUTO_TEST_CASE(cash_difficulty_test) {
    DummyConfig config(CBaseChainParams::MAIN);

    std::vector<CBlockIndex> blocks(3000);

    const Consensus::Params &params = config.GetChainParams().GetConsensus();
    const arith_uint256 powLimit = UintToArith256(params.powLimit);
    uint32_t powLimitBits = powLimit.GetCompact();
    arith_uint256 currentPow = powLimit >> 4;
    uint32_t initialBits = currentPow.GetCompact();

    // Genesis block.
    blocks[0] = CBlockIndex();
    blocks[0].nHeight = 0;
    blocks[0].nTime = 1269211443;
    blocks[0].nBits = initialBits;

    blocks[0].nChainWork = GetBlockProof(blocks[0]);

    // Block counter.
    size_t i;

    // Pile up some blocks every 10 mins to establish some history.
    for (i = 1; i < 2050; i++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 600, initialBits);
    }

    CBlockHeader blkHeaderDummy;
    uint32_t nBits =
        GetNextCashWorkRequired(&blocks[2049], &blkHeaderDummy, params);

    // Difficulty stays the same as long as we produce a block every 10 mins.
    for (size_t j = 0; j < 10; i++, j++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 600, nBits);
        BOOST_CHECK_EQUAL(
            GetNextCashWorkRequired(&blocks[i], &blkHeaderDummy, params),
            nBits);
    }

    // Make sure we skip over blocks that are out of wack. To do so, we produce
    // a block that is far in the future, and then produce a block with the
    // expected timestamp.
    blocks[i] = GetBlockIndex(&blocks[i - 1], 6000, nBits);
    BOOST_CHECK_EQUAL(
        GetNextCashWorkRequired(&blocks[i++], &blkHeaderDummy, params), nBits);
    blocks[i] = GetBlockIndex(&blocks[i - 1], 2 * 600 - 6000, nBits);
    BOOST_CHECK_EQUAL(
        GetNextCashWorkRequired(&blocks[i++], &blkHeaderDummy, params), nBits);

    // The system should continue unaffected by the block with a bogous
    // timestamps.
    for (size_t j = 0; j < 20; i++, j++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 600, nBits);
        BOOST_CHECK_EQUAL(
            GetNextCashWorkRequired(&blocks[i], &blkHeaderDummy, params),
            nBits);
    }

    // We start emitting blocks slightly faster. The first block has no impact.
    blocks[i] = GetBlockIndex(&blocks[i - 1], 550, nBits);
    BOOST_CHECK_EQUAL(
        GetNextCashWorkRequired(&blocks[i++], &blkHeaderDummy, params), nBits);

    // Now we should see difficulty increase slowly.
    for (size_t j = 0; j < 10; i++, j++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 550, nBits);
        const uint32_t nextBits =
            GetNextCashWorkRequired(&blocks[i], &blkHeaderDummy, params);

        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Make sure that difficulty increases very slowly.
        BOOST_CHECK(nextTarget < currentTarget);
        BOOST_CHECK((currentTarget - nextTarget) < (currentTarget >> 10));

        nBits = nextBits;
    }

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1c0fe7b1);

    // If we dramatically shorten block production, difficulty increases faster.
    for (size_t j = 0; j < 20; i++, j++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 10, nBits);
        const uint32_t nextBits =
            GetNextCashWorkRequired(&blocks[i], &blkHeaderDummy, params);

        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Make sure that difficulty increases faster.
        BOOST_CHECK(nextTarget < currentTarget);
        BOOST_CHECK((currentTarget - nextTarget) < (currentTarget >> 4));

        nBits = nextBits;
    }

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1c0db19f);

    // We start to emit blocks significantly slower. The first block has no
    // impact.
    blocks[i] = GetBlockIndex(&blocks[i - 1], 6000, nBits);
    nBits = GetNextCashWorkRequired(&blocks[i++], &blkHeaderDummy, params);

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1c0d9222);

    // If we dramatically slow down block production, difficulty decreases.
    for (size_t j = 0; j < 93; i++, j++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 6000, nBits);
        const uint32_t nextBits =
            GetNextCashWorkRequired(&blocks[i], &blkHeaderDummy, params);

        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Check the difficulty decreases.
        BOOST_CHECK(nextTarget <= powLimit);
        BOOST_CHECK(nextTarget > currentTarget);
        BOOST_CHECK((nextTarget - currentTarget) < (currentTarget >> 3));

        nBits = nextBits;
    }

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1c2f13b9);

    // Due to the window of time being bounded, next block's difficulty actually
    // gets harder.
    blocks[i] = GetBlockIndex(&blocks[i - 1], 6000, nBits);
    nBits = GetNextCashWorkRequired(&blocks[i++], &blkHeaderDummy, params);
    BOOST_CHECK_EQUAL(nBits, 0x1c2ee9bf);

    // And goes down again. It takes a while due to the window being bounded and
    // the skewed block causes 2 blocks to get out of the window.
    for (size_t j = 0; j < 192; i++, j++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 6000, nBits);
        const uint32_t nextBits =
            GetNextCashWorkRequired(&blocks[i], &blkHeaderDummy, params);

        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Check the difficulty decreases.
        BOOST_CHECK(nextTarget <= powLimit);
        BOOST_CHECK(nextTarget > currentTarget);
        BOOST_CHECK((nextTarget - currentTarget) < (currentTarget >> 3));

        nBits = nextBits;
    }

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1d00ffff);

    // Once the difficulty reached the minimum allowed level, it doesn't get any
    // easier.
    for (size_t j = 0; j < 5; i++, j++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 6000, nBits);
        const uint32_t nextBits =
            GetNextCashWorkRequired(&blocks[i], &blkHeaderDummy, params);

        // Check the difficulty stays constant.
        BOOST_CHECK_EQUAL(nextBits, powLimitBits);
        nBits = nextBits;
    }
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: MicrohexHQ/bitcoin-abc

max_issues_repo_name: trongham/bitcoin-abc

max_forks_repo_name: trongham/bitcoin-abc