text:
/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      100902    K. Kumar          File header and footer added.
 *      100916    D. Dirkx          Added minor comments during checking.
 *      100928    K. Kumar          Small comment modifications.
 *      100929    K. Kumar          Small comment modifications.
 *      110202    K. Kumar          Added overload for map with State* for
 *                                  computeNearestLeftNeighborUsingBinarySearch( ).
 *      110803    J. Leloux         Added convertStringToTemplate.
 *      110805    J. Leloux         Added outputCurrentRunningTime( ).
 *      110807    K. Kumar          Minor comment modifications.
 *      110810    J. Leloux         Minor comment modifications.
 *      110913    K. Kumar          Implemented automatic root-path functions based on
 *                                  suggestions by M. Persson.
 *      111117    K. Kumar          Added listAllFilesInDirectory( ) function.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *
 *    Notes
 *      Need to add bounds checking with respect to targetValue for
 *      computeNearestLeftNeighborUsingBinarySearch( ).
 */

#include <map>
#include <iterator>
#include <Eigen/Core>

namespace tudat
{
namespace basic_mathematics
{

//! Nearest left neighbor binary search.
int computeNearestLeftNeighborUsingBinarySearch(
        const Eigen::VectorXd& vectorOfSortedData,
        const double targetValueInVectorOfSortedData )
{
    // Declare local variables.
    // Declare bounds of vector of sorted data and current position.
    int leftLimitOfVectorOfSortedData = 0;
    int rightLimitOfVectorOfSortedData = vectorOfSortedData.rows( ) - 1;
    int currentPositionInVectorOfSortedData;

    // Check if data is sorted in ascending order.
    // ( true if ascending, else false ).
    bool isVectorOfSortedDataAscending
            = ( vectorOfSortedData[ rightLimitOfVectorOfSortedData ]
                >= vectorOfSortedData[ leftLimitOfVectorOfSortedData ] );

    // Loop through vector of sorted data until left and right limits
    // are neighbours.
    while ( rightLimitOfVectorOfSortedData
            - leftLimitOfVectorOfSortedData > 1 )
    {
        // Compute midpoint ( bitshift is same as division by 2.0 ).
        currentPositionInVectorOfSortedData
                = ( rightLimitOfVectorOfSortedData
                    + leftLimitOfVectorOfSortedData ) >> 1;

        // Check which limit to replace ( if ascending and target datum
        // is in right half, replace left limit ).
        if ( targetValueInVectorOfSortedData
             >= vectorOfSortedData[ currentPositionInVectorOfSortedData ]
             && isVectorOfSortedDataAscending )
        {
            // Set left limit to current position in vector of sorted data.
            leftLimitOfVectorOfSortedData = currentPositionInVectorOfSortedData;
        }

        else
        {
            // Set right limit to current position in vector of sorted data.
            rightLimitOfVectorOfSortedData = currentPositionInVectorOfSortedData;
        }
    }

    // Set current position to left limit.
    currentPositionInVectorOfSortedData = leftLimitOfVectorOfSortedData;

    // Return current position in vector.
    return currentPositionInVectorOfSortedData;
}

//! Nearest left neighbor binary search.
int computeNearestLeftNeighborUsingBinarySearch(
        const std::map < double, Eigen::VectorXd >& sortedIndepedentAndDependentVariables,
        const double targetValueInMapOfData )
{
    // Declare local variables.
    // Declare bounds of key of map of data and current position.
    int leftLimitOfKeyOfMapOfData = 0;
    int rightLimitOfKeyOfMapOfData = sortedIndepedentAndDependentVariables
                                     .size( ) - 1;
    int currentPositionInKeyOfMapOfData;

    // Declare map iterator
    std::map < double, Eigen::VectorXd >::const_iterator mapIterator;

    // Loop through vector of sorted data until left and right limits
    // are neighbours
    while ( rightLimitOfKeyOfMapOfData - leftLimitOfKeyOfMapOfData > 1 )
    {
        // Compute midpoint ( bitshift is same as division by 2.0 ).
        currentPositionInKeyOfMapOfData
                = ( rightLimitOfKeyOfMapOfData
                   + leftLimitOfKeyOfMapOfData ) >> 1;

        // Set map iterator to begin begin of map of sorted independent and
        // dependent variables.
        mapIterator = sortedIndepedentAndDependentVariables.begin( );

        // Advance iterator to location of current position in key of map of
        // data.
        advance( mapIterator, currentPositionInKeyOfMapOfData );

        // Check that target value lies to the right of lower bound.
        if ( targetValueInMapOfData
             >= mapIterator->first )
        {
            // Set left limit to current position in map of data.
            leftLimitOfKeyOfMapOfData = currentPositionInKeyOfMapOfData;
        }

        else
        {
            // Set right limit to current position in map of data.
            rightLimitOfKeyOfMapOfData = currentPositionInKeyOfMapOfData;
        }
    }

    // Set current position to left limit.
    currentPositionInKeyOfMapOfData = leftLimitOfKeyOfMapOfData;

    // Return current position in map of data.
    return currentPositionInKeyOfMapOfData;
}

} // namespace basic_mathematics
} // namespace tudat

max_stars_repo_name: JPelamatti/ThesisTUDAT

max_issues_repo_name: JPelamatti/ThesisTUDAT

max_forks_repo_name: JPelamatti/ThesisTUDAT


text:
// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[overlaps
//` Checks if two geometries overlap

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
    // Checks if the two geometries overlaps or not.
    bg::model::polygon<bg::model::d2::point_xy<double> > poly1;
    bg::read_wkt("POLYGON((0 0,0 4,4 4,4 0,0 0))", poly1);
    bg::model::polygon<bg::model::d2::point_xy<double> > poly2;
    bg::read_wkt("POLYGON((2 2,2 6,6 7,6 1,2 2))", poly2);
    bool check_overlap = bg::overlaps(poly1, poly2);
    if (check_overlap) {
         std::cout << "Overlaps: Yes" << std::endl;
    } else {
        std::cout << "Overlaps: No" << std::endl;
    }

    bg::model::polygon<bg::model::d2::point_xy<double> > poly3;
    bg::read_wkt("POLYGON((-1 -1,-3 -4,-7 -7,-4 -3,-1 -1))", poly3);
    check_overlap = bg::overlaps(poly1, poly3);
    if (check_overlap) {
         std::cout << "Overlaps: Yes" << std::endl;
    } else {
        std::cout << "Overlaps: No" << std::endl;
    }

    return 0;
}

//]

//[overlaps_output
/*`
Output:
[pre
Overlaps: Yes

[$img/algorithms/overlaps.png]

Overlaps: No
]
*/
//]

ext: cpp

max_stars_repo_name: jkerkela/geometry

max_issues_repo_name: jkerkela/geometry

max_forks_repo_name: jkerkela/geometry


text:
#include "AL_API/alwebrtcpluginapi.hpp"
#include "alsdkplugin.hpp"
#include <boost/config.hpp> // for BOOST_SYMBOL_EXPORT
#include <iostream>

class AlWebRtcFactory : public AlWebRtcPluginApi {
public:
  AlWebRtcFactory() {}

  std::string name() const { return "AlWebRtcFactory"; }

  AlSdkAPI *createSdkApi() { return new AlSdkPlugin(); }

  ~AlWebRtcFactory() {}
};

extern "C" BOOST_SYMBOL_EXPORT AlWebRtcFactory plugin;
AlWebRtcFactory plugin;

ext: cc

max_stars_repo_name: xorsnn/altexo

max_issues_repo_name: xorsnn/altexo

max_forks_repo_name: xorsnn/altexo


text:
/// lsqcpp/lsqcpp.hpp
///
/// Author:     Fabian Meyer
/// Created On: 22 Jul 2019
/// License:    MIT

#ifndef LSQCPP_LSQCPP_HPP_
#define LSQCPP_LSQCPP_HPP_

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <iostream>
#include <iomanip>
#include <functional>

namespace lsqcpp
{
    using Index = Eigen::MatrixXd::Index;

    /// Parametrization container for jacobian estimation using finite differences.
    /// This class holds parameters for the finite differences operation, such as the
    /// number of threads to be used and the numerical espilon.
    template<typename _Scalar>
    struct FiniteDifferencesParameter
    {
    public:
        using Scalar = _Scalar;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Finite differences only supports non-integer scalars.");

        FiniteDifferencesParameter() = default;

        FiniteDifferencesParameter(const Scalar eps)
            : _eps(eps)
        { }

        FiniteDifferencesParameter(const Scalar eps, const int threads)
            : _eps(eps), _threads(threads)
        { }

        /// Sets the numerical espilon that is used as step between two sucessive function evaluations.
        /// @param eps numerical epsilon
        void setEpsilon(const Scalar eps)
        {
            _eps = eps;
        }

        /// Returns the numerical epsilon, which is used for jacobian approximation.
        /// @return numerical epsilon
        Scalar epsilon() const
        {
            return _eps;
        }

        /// Sets the number of threads which should be used to compute finite differences
        /// (OMP only).
        /// Set to 0 or negative for auto-detection of a suitable number.
        /// Each dimension of the input vector can be handled independently.
        /// @param threads number of threads
        void setThreads(const int threads)
        {
            _threads = threads;
        }

        /// Returns the number of threads which should be used to compute finite differences
        /// (OMP only).
        /// @return threads number of threads
        int threads() const
        {
            return _threads;
        }

    private:
        Scalar _eps = std::sqrt(Eigen::NumTraits<Scalar>::epsilon());
        int _threads = int{1};
    };

    /// Functor to compute forward differences.
    /// Computes the gradient of the objective f(x) as follows:
    ///
    /// grad(x) = (f(x + eps) - f(x)) / eps
    ///
    /// The computation requires len(x) evaluations of the objective.
    struct ForwardDifferences
    {
        template<typename Scalar, typename Objective, typename I, typename O, typename J>
        void operator()(const Eigen::MatrixBase<I> &xval,
                        const Eigen::MatrixBase<O> &fval,
                        const Objective &objective,
                        const FiniteDifferencesParameter<Scalar>& param,
                        Eigen::MatrixBase<J> &jacobian) const
        {
            static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Finite differences only supports non-integer scalars.");

            using InputVector = typename Eigen::MatrixBase<I>::PlainMatrix;
            using OutputVector = typename Eigen::MatrixBase<O>::PlainMatrix;

            // noop in fixed size problems
            jacobian.derived().resize(fval.size(), xval.size());

            #pragma omp parallel for num_threads(param.threads())
            for(Index i = 0; i < xval.size(); ++i)
            {
                InputVector xvalNext = xval;
                OutputVector fvalNext;
                xvalNext(i) += param.epsilon();
                objective(xvalNext, fvalNext);

                jacobian.col(i) = (fvalNext - fval) / param.epsilon();
            }
        }
    };

    /// Functor to compute backward differences.
    /// Computes the gradient of the objective f(x) as follows:
    ///
    /// grad(x) = (f(x) - f(x - eps)) / eps
    ///
    /// The computation requires len(x) evaluations of the objective.
    struct BackwardDifferences
    {
        template<typename Scalar, typename Objective, typename I, typename O, typename J>
        void operator()(const Eigen::MatrixBase<I> &xval,
                        const Eigen::MatrixBase<O> &fval,
                        const Objective &objective,
                        const FiniteDifferencesParameter<Scalar>& param,
                        Eigen::MatrixBase<J> &jacobian) const
        {
            static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Finite differences only supports non-integer scalars.");

            using InputVector = typename Eigen::MatrixBase<I>::PlainMatrix;
            using OutputVector = typename Eigen::MatrixBase<O>::PlainMatrix;

            // noop in fixed size problems
            jacobian.derived().resize(fval.size(), xval.size());

            #pragma omp parallel for num_threads(param.threads())
            for(Index i = 0; i < xval.size(); ++i)
            {
                InputVector xvalNext = xval;
                OutputVector fvalNext;
                xvalNext(i) -= param.epsilon();
                objective(xvalNext, fvalNext);

                jacobian.col(i) = (fval - fvalNext) / param.epsilon();
            }
        }
    };

    /// Functor to compute central differences.
    /// Computes the gradient of the objective f(x) as follows:
    ///
    /// grad(x) = (f(x + 0.5 eps) - f(x - 0.5 eps)) / eps
    ///
    /// The computation requires 2 * len(x) evaluations of the objective.
    struct CentralDifferences
    {
        template<typename Scalar, typename Objective, typename I, typename O, typename J>
        void operator()(const Eigen::MatrixBase<I> &xval,
                        const Eigen::MatrixBase<O> &fval,
                        const Objective &objective,
                        const FiniteDifferencesParameter<Scalar>& param,
                        Eigen::MatrixBase<J> &jacobian) const
        {
            static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Finite differences only supports non-integer scalars.");

            using InputVector = typename Eigen::MatrixBase<I>::PlainMatrix;
            using OutputVector = typename Eigen::MatrixBase<O>::PlainMatrix;

            // noop in fixed size problems
            jacobian.derived().resize(fval.size(), xval.size());

            #pragma omp parallel for num_threads(param.threads())
            for(Index i = 0; i < xval.size(); ++i)
            {
                InputVector xvalNext = xval;
                OutputVector fvalA;
                OutputVector fvalB;

                xvalNext(i) = xval(i) - param.epsilon() / Scalar{2};
                objective(xvalNext, fvalA);

                xvalNext(i) = xval(i) + param.epsilon() / Scalar{2};
                objective(xvalNext, fvalB);

                jacobian.col(i) = (fvalB - fvalA) / param.epsilon();
            }
        }
    };

    /// Solves for dense linear equation systems using the Jacobi SVD method.
    struct DenseSVDSolver
    {
        /// Solves the linear equation system Ax = b and stores the result in x.
        /// @param A linear equation system matrix
        /// @param b linear equation system vector
        /// @param x computed result
        /// @return true if successful, false otherwise
        template<typename M, typename B, typename X>
        bool operator()(const Eigen::MatrixBase<M> &A,
                        const Eigen::MatrixBase<B> &b,
                        Eigen::MatrixBase<X> &x) const
        {
            using Solver = Eigen::JacobiSVD<M, Eigen::FullPivHouseholderQRPreconditioner>;
            auto solver = Solver(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
            x = solver.solve(b);
            return true;
        }
    };

    /// Solver for dense linear equation systems, which a positive semi-definite, using the Cholesky decomposition.
    struct DenseCholeskySolver
    {
        /// Solves the linear equation system Ax = b and stores the result in x.
        /// @param A linear equation system matrix
        /// @param b linear equation system vector
        /// @param x computed result
        /// @return true if successful, false otherwise
        template<typename M, typename B, typename X>
        bool operator()(const Eigen::MatrixBase<M> &A,
                        const Eigen::MatrixBase<B> &b,
                        Eigen::MatrixBase<X> &x) const
        {
            using Solver = Eigen::LDLT<M>;

            Solver decomp;
            decomp.compute(A);

            if(!decomp.isPositive())
                return false;

            x = decomp.solve(b);
            return true;
        }
    };

    /// Generic class for refining a computed newton step.
    /// The method parameter determines how the step is actually refined, e.g
    /// ArmijoBacktracking, DoglegMethod or WolfeBacktracking.
    template<typename Scalar, int Inputs, int Outputs, typename Method>
    class NewtonStepRefiner { };

    /// Newton step refinement method which applies a constant scaling factor to the newton step.
    struct ConstantStepFactor { };

    /// Parametrization class for the ConstantStepFactor refinement method.
    template<typename _Scalar>
    class ConstantStepFactorParameter
    {
    public:
        using Scalar = _Scalar;

        ConstantStepFactorParameter() = default;

        ConstantStepFactorParameter(const Scalar factor)
            : _factor(factor)
        { }
        /// Sets the constant scaling factor which is applied to the newton step.
        /// @param factor constant newton step scaling factor
        void setFactor(const Scalar factor)
        {
            _factor = factor;
        }

        /// Returns the constant scaling factor which is applied to the newton step.
        /// @return constant newton step scaling factor
        Scalar factor() const
        {
            return _factor;
        }
    private:
        Scalar _factor = Scalar{1};
    };

    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, ConstantStepFactor>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = ConstantStepFactor;
        using Parameter = ConstantStepFactorParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param)
        { }

        /// Refines the given newton step and scales it by a constant factor.
        /// @param step newton step which is scaled.
        template<typename Objective>
        void operator()(const InputVector &,
                        const OutputVector &,
                        const JacobiMatrix &,
                        const GradientVector &,
                        const Objective &,
                        StepVector &step) const
        {
            step *= _param.factor();
        }
    private:
        const Parameter &_param;
    };

    /// Applies Barzilai-Borwein (BB) refinemnt to the newton step.
    /// The functor can either compute the direct or inverse BB step.
    /// The steps are computed as follows:
    ///
    /// s_k = x_k - x_k-1         k >= 1
    /// y_k = step_k - step_k-1   k >= 1
    /// Direct:  stepSize = (s_k^T * s_k) / (y_k^T * s_k)
    /// Inverse: stepSize = (y_k^T * s_k) / (y_k^T * y_k)
    ///
    /// The very first step is computed as a constant.
    struct BarzilaiBorwein
    {
        enum class Mode
        {
            Direct,
            Inverse
        };
    };

    /// Parametrization class for the BarzilaiBorwein refinement method.
    template<typename _Scalar>
    class BarzilaiBorweinParameter
    {
    public:
        using Scalar = _Scalar;

        BarzilaiBorweinParameter() = default;

        BarzilaiBorweinParameter(const BarzilaiBorwein::Mode mode)
            : _mode(mode)
        { }

        BarzilaiBorweinParameter(const Scalar constStep)
            : _constStep(constStep)
        { }

        BarzilaiBorweinParameter(const BarzilaiBorwein::Mode mode, const Scalar constStep)
            : _mode(mode), _constStep(constStep)
        { }

        /// Sets the BarzilaiBorwein operation mode.
        /// @param mode mode
        void setMode(const BarzilaiBorwein::Mode mode)
        {
            _mode = mode;
        }

        /// Returns the BarzilaiBorwein operation mode.
        /// @return mode
        BarzilaiBorwein::Mode mode() const
        {
            return _mode;
        }

        /// Sets the constant step size, which is used when the refiner was not initialized yet.
        /// @param stepSize constant step size
        void setConstantStepSize(const Scalar stepSize)
        {
            _constStep = stepSize;
        }

        /// Returns he constant step size, which is used when the refiner was not initialized yet.
        /// @return constant step size
        Scalar constantStepSize() const
        {
            return _constStep;
        }
    private:
        BarzilaiBorwein::Mode _mode = BarzilaiBorwein::Mode::Direct;
        Scalar _constStep = static_cast<Scalar>(1e-2);
    };

    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, BarzilaiBorwein>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = BarzilaiBorwein;
        using Parameter = BarzilaiBorweinParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param)
        {
            _lastXval.setZero();
            _lastStep.setZero();
        }

        template<typename Objective>
        void operator()(const InputVector &xval,
            const OutputVector &,
            const JacobiMatrix &,
            const GradientVector &,
            const Objective &,
            StepVector &step)
        {
            auto stepSize = Scalar{0};
            if(_lastXval.sum() == Scalar{0})
            {
                stepSize = (Scalar{1} / step.norm()) * _param.constantStepSize();
            }
            else
            {
                switch(_param.mode())
                {
                case BarzilaiBorwein::Mode::Direct:
                    stepSize = directStep(xval, step);
                    break;
                case BarzilaiBorwein::Mode::Inverse:
                    stepSize = inverseStep(xval, step);
                    break;
                default:
                    assert(false);
                    break;
                }
            }

            _lastStep = step;
            _lastXval = xval;

            step *= stepSize;
        }
    private:
        const Parameter &_param;
        InputVector _lastXval = {};
        StepVector _lastStep = {};

        Scalar directStep(const InputVector &xval,
                          const StepVector &step) const
        {
            const auto sk = xval - _lastXval;
            const auto yk = step - _lastStep;
            const auto num = sk.dot(sk);
            const auto denom = sk.dot(yk);

            if(denom == Scalar{0})
                return Scalar{1};
            else
                return num / denom;
        }

        Scalar inverseStep(const InputVector &xval,
                           const StepVector &step) const
        {
            const auto sk = xval - _lastXval;
            const auto yk = step - _lastStep;
            const auto num = sk.dot(yk);
            const auto denom = yk.dot(yk);

            if(denom == Scalar{0})
                return Scalar{1};
            else
                return num / denom;
        }
    };

    /// Step refinement method performs Armijo Linesearch with backtracking.
    /// The line search iteratively decreases the step size until the following
    /// conditions are met:
    ///
    /// Armijo: f(x - stepSize * grad(x)) <= f(x) - c1 * stepSize * grad(x)^T * grad(x)
    ///
    /// If the condition does not hold the step size is decreased:
    ///
    /// stepSize = decrease * stepSize
    struct ArmijoBacktracking { };

    /// Parametrization class for the ArmijoBacktracking refinement method.
    template<typename _Scalar>
    class ArmijoBacktrackingParameter
    {
    public:
        using Scalar = _Scalar;

        ArmijoBacktrackingParameter() = default;

        ArmijoBacktrackingParameter(const Scalar decrease,
                                    const Scalar c1,
                                    const Scalar minStep,
                                    const Scalar maxStep,
                                    const Index iterations)
            : _decrease(decrease), _c1(c1), _minStep(minStep),
            _maxStep(maxStep), _maxIt(iterations)
        {
            assert(decrease > static_cast<Scalar>(0));
            assert(decrease < static_cast<Scalar>(1));
            assert(c1 > static_cast<Scalar>(0));
            assert(c1 < static_cast<Scalar>(0.5));
        }

        /// Set the decreasing factor for backtracking.
        /// Assure that decrease in (0, 1).
        /// @param decrease decreasing factor
        void setBacktrackingDecrease(const Scalar decrease)
        {
            assert(decrease > static_cast<Scalar>(0));
            assert(decrease < static_cast<Scalar>(1));
            _decrease = decrease;
        }

        /// Returns the decreasing factor for backtracking.
        /// The value should always lie within (0, 1).
        /// @return backtracking decrease
        Scalar backtrackingDecrease() const
        {
            return _decrease;
        }

        /// Set the relaxation constant for the Armijo condition (see class description).
        /// Typically c1 is chosen to be quite small, e.g. 1e-4.
        /// Assure that c1 in (0, 0.5).
        /// @param c1 armijo constant
        void setArmijoConstant(const Scalar c1)
        {
            assert(c1 > static_cast<Scalar>(0));
            assert(c1 < static_cast<Scalar>(0.5));
            _c1 = c1;
        }

        /// Returns the the relaxation constant for the Armijo condition (see class description).
        /// The value should always lie within (0, 0.5).
        /// @return armijo constant
        Scalar armijoConstant() const
        {
            return _c1;
        }

        /// Set the bounds for the step size during linesearch.
        /// The final step size is guaranteed to be in [minStep, maxStep].
        /// @param minStep minimum step size
        /// @param maxStep maximum step size
        void setStepBounds(const Scalar minStep, const Scalar maxStep)
        {
            assert(minStep >= 0);
            assert(minStep < maxStep);
            _minStep = minStep;
            _maxStep = maxStep;
        }

        /// Returns the minimum bound for the step size during linesearch.
        /// @return minimum step size bound
        Scalar minimumStepBound() const
        {
            return _minStep;
        }

        /// Returns the maximum bound for the step size during linesearch.
        /// @return maximum step size bound
        Scalar maximumStepBound() const
        {
            return _maxStep;
        }

        /// Set the maximum number of iterations.
        /// Set to 0 or negative for infinite iterations.
        /// @param iterations maximum number of iterations
        void setMaximumIterations(const Index iterations)
        {
            _maxIt = iterations;
        }

        /// Returns the maximum number of iterations.
        /// A value of 0 or negative means infinite iterations.
        /// @return maximum number of iterations
        Index maximumIterations() const
        {
            return _maxIt;
        }
    private:
        Scalar _decrease = static_cast<Scalar>(0.8);
        Scalar _c1 = static_cast<Scalar>(1e-4);
        Scalar _minStep = static_cast<Scalar>(1e-10);
        Scalar _maxStep = static_cast<Scalar>(1);
        Index _maxIt = Index{0};
    };

    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, ArmijoBacktracking>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = ArmijoBacktracking;
        using Parameter = ArmijoBacktrackingParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param)
        { }

        template<typename Objective>
        void operator()(const InputVector &xval,
                        const OutputVector &fval,
                        const JacobiMatrix &,
                        const GradientVector &gradient,
                        const Objective &objective,
                        StepVector &step) const
        {
            JacobiMatrix jacobianN;
            InputVector xvalN;
            OutputVector fvalN;

            const auto error = static_cast<Scalar>(0.5) * fval.squaredNorm();
            const auto stepGrad = gradient.dot(step);
            bool armijoCondition = false;
            auto stepSize = _param.maximumStepBound() / _param.backtrackingDecrease();

            auto iterations = Index{0};
            while((_param.maximumIterations() <= Index{0} || iterations < _param.maximumIterations()) &&
                   stepSize * _param.backtrackingDecrease() >= _param.minimumStepBound() &&
                   !armijoCondition)
            {
                stepSize = _param.backtrackingDecrease() * stepSize;
                xvalN = xval - stepSize * step;
                objective(xvalN, fvalN, jacobianN);

                const auto errorN = static_cast<Scalar>(0.5) * fvalN.squaredNorm();

                armijoCondition = errorN <= error + _param.armijoConstant() * stepSize * stepGrad;

                ++iterations;
            }

            step *= stepSize;
        }
    private:
        const Parameter &_param;
    };

    /// Step refinement method which performs Wolfe Linesearch with backtracking.
    /// The line search iteratively decreases the step size until the following
    /// conditions are met:
    ///
    /// Armijo: f(x - stepSize * grad(x)) <= f(x) - c1 * stepSize * grad(x)^T * grad(x)
    /// Wolfe: grad(x)^T grad(x - stepSize * grad(x)) <= c2 * grad(x)^T * grad(x)
    ///
    /// If either condition does not hold the step size is decreased:
    ///
    /// stepSize = decrease * stepSize
    struct WolfeBacktracking { };

    /// Parametrization class for the WolfeBacktracking refinement method.
    template<typename _Scalar>
    class WolfeBacktrackingParameter
    {
    public:
        using Scalar = _Scalar;

        WolfeBacktrackingParameter() = default;

        WolfeBacktrackingParameter(const Scalar decrease,
                                   const Scalar c1,
                                   const Scalar c2,
                                   const Scalar minStep,
                                   const Scalar maxStep,
                                   const Index iterations)
            : _decrease(decrease), _c1(c1), _c2(c2), _minStep(minStep),
            _maxStep(maxStep), _maxIt(iterations)
        {
            assert(decrease > static_cast<Scalar>(0));
            assert(decrease < static_cast<Scalar>(1));
            assert(c1 > static_cast<Scalar>(0));
            assert(c1 < static_cast<Scalar>(0.5));
            assert(c1 < c2);
            assert(c2 < static_cast<Scalar>(1));
        }

        /// Set the decreasing factor for backtracking.
        /// Assure that decrease in (0, 1).
        /// @param decrease decreasing factor
        void setBacktrackingDecrease(const Scalar decrease)
        {
            assert(decrease > static_cast<Scalar>(0));
            assert(decrease < static_cast<Scalar>(1));
            _decrease = decrease;
        }

        /// Returns the decreasing factor for backtracking.
        /// The value should always lie within (0, 1).
        /// @return backtracking decrease
        Scalar backtrackingDecrease() const
        {
            return _decrease;
        }

        /// Set the wolfe constants for Armijo and Wolfe condition (see class
        /// description).
        /// Assure that c1 < c2 < 1 and c1 in (0, 0.5).
        /// Typically c1 is chosen to be quite small, e.g. 1e-4.
        /// @param c1 armijo constant
        /// @param c2 wolfe constant
        void setWolfeConstants(const Scalar c1, const Scalar c2)
        {
            assert(c1 > static_cast<Scalar>(0));
            assert(c1 < static_cast<Scalar>(0.5));
            assert(c1 < c2);
            assert(c2 < static_cast<Scalar>(1));
            _c1 = c1;
            _c2 = c2;
        }

        /// Returns the the relaxation constant for the Armijo condition (see class description).
        /// The value should always lie within (0, 0.5).
        /// @return armijo constant
        Scalar armijoConstant() const
        {
            return _c1;
        }

        /// Returns the the relaxation constant for the Wolfe condition (see class description).
        /// @return wolfe constant
        Scalar wolfeConstant() const
        {
            return _c2;
        }

        /// Set the bounds for the step size during linesearch.
        /// The final step size is guaranteed to be in [minStep, maxStep].
        /// @param minStep minimum step size
        /// @param maxStep maximum step size
        void setStepBounds(const Scalar minStep, const Scalar maxStep)
        {
            assert(minStep >= Scalar{0});
            assert(minStep < maxStep);
            _minStep = minStep;
            _maxStep = maxStep;
        }

        /// Returns the minimum bound for the step size during linesearch.
        /// @return minimum step size bound
        Scalar minimumStepBound() const
        {
            return _minStep;
        }

        /// Returns the maximum bound for the step size during linesearch.
        /// @return maximum step size bound
        Scalar maximumStepBound() const
        {
            return _maxStep;
        }

        /// Set the maximum number of iterations.
        /// Set to 0 or negative for infinite iterations.
        /// @param iterations maximum number of iterations
        void setMaximumIterations(const Index iterations)
        {
            _maxIt = iterations;
        }

        /// Returns the maximum number of iterations.
        /// A value of 0 or negative means infinite iterations.
        /// @return maximum number of iterations
        Index maximumIterations() const
        {
            return _maxIt;
        }
    private:
        Scalar _decrease = static_cast<Scalar>(0.8);
        Scalar _c1 = static_cast<Scalar>(1e-4);
        Scalar _c2 = static_cast<Scalar>(0.9);
        Scalar _minStep = static_cast<Scalar>(1e-10);
        Scalar _maxStep = static_cast<Scalar>(1.0);
        Index _maxIt = 0;
    };

    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, WolfeBacktracking>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = WolfeBacktracking;
        using Parameter = WolfeBacktrackingParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param)
        {}

        template<typename Objective>
        void operator()(const InputVector &xval,
                        const OutputVector &fval,
                        const JacobiMatrix &,
                        const GradientVector &gradient,
                        const Objective &objective,
                        StepVector &step) const
        {
            auto stepSize = _param.maximumStepBound() / _param.backtrackingDecrease();
            JacobiMatrix jacobianN;
            GradientVector gradientN;
            InputVector xvalN;
            OutputVector fvalN;

            const Scalar error = fval.squaredNorm() / Scalar{2};
            const Scalar stepGrad = gradient.dot(step);
            bool armijoCondition = false;
            bool wolfeCondition = false;

            Index iterations = 0;
            while((_param.maximumIterations() <= 0 || iterations < _param.maximumIterations()) &&
                   stepSize * _param.backtrackingDecrease() >= _param.minimumStepBound() &&
                   !(armijoCondition && wolfeCondition))
            {
                stepSize = _param.backtrackingDecrease() * stepSize;
                xvalN = xval - stepSize * step;
                objective(xvalN, fvalN, jacobianN);
                Scalar errorN = fvalN.squaredNorm() / 2;
                gradientN = jacobianN.transpose() * fvalN;

                armijoCondition = errorN <= error + _param.armijoConstant() * stepSize * stepGrad;
                wolfeCondition = gradientN.dot(step) >= _param.wolfeConstant() * stepGrad;

                ++iterations;
            }

            step *= stepSize;
        }
    private:
        const Parameter &_param;
    };

    /// Step refinement method which implements Powell' Dogleg Method.
    struct DoglegMethod { };

    /// Parametrization class for the DoglegMethod refinement method.
    template<typename _Scalar>
    class DoglegMethodParameter
    {
    public:
        using Scalar = _Scalar;

        DoglegMethodParameter() = default;

        DoglegMethodParameter(const Scalar initRadius,
                              const Scalar maxRadius,
                              const Scalar radiusEps,
                              const Scalar acceptFitness,
                              const Index iterations)
            : _initialRadius(initRadius), _maxRadius(maxRadius), _radiusEps(radiusEps),
            _acceptFitness(acceptFitness), _maxIt(iterations)
        { }

        /// Set maximum iterations of the trust region radius search.
        /// Set to 0 or negative for infinite iterations.
        /// @param iterations maximum iterations for radius search
        void setMaximumIterations(const Index iterations)
        {
            _maxIt = iterations;
        }

        /// Returns the maximum iterations of the trust region radius search.
        /// @return maximum iterations for radius search
        Index maximumIterations() const
        {
            return _maxIt;
        }

        /// Set the minimum fitness value at which a model is accepted.
        /// The model fitness is computed as follows:
        ///
        /// fitness = f(xval) - f(xval + step) / m(0) - m(step)
        ///
        /// Where f(x) is the objective error function and m(x) is the
        /// model function describe by the trust region method.
        ///
        /// @param fitness minimum fitness for step acceptance
        void setAcceptanceFitness(const Scalar fitness)
        {
            _acceptFitness = fitness;
        }

        /// Returns the minimum fitness value at which a model is accepted.
        /// @return minimum fitness for step acceptance
        Scalar acceptanceFitness() const
        {
            return _acceptFitness;
        }

        /// Sets the comparison epsilon on how close the step should be
        /// to the trust region radius to trigger an increase of the radius.
        /// Should usually be picked low, e.g. 1e-8.
        /// @param eps comparison epsilon for radius increase
        void setRadiusEpsilon(const Scalar eps)
        {
            _radiusEps = eps;
        }

        /// Returns the comparison epsilon on how close the step should be
        /// to the trust region radius to trigger an increase of the radius.
        /// @return comparison epsilon for radius increase
        Scalar radiusEpsilon() const
        {
            return _radiusEps;
        }

        /// Sets the maximum radius that is used for trust region search.
        /// @param radius maximum trust region radius
        void setMaximumRadius(const Scalar radius)
        {
            _initialRadius = std::min(_initialRadius, radius);
            _maxRadius = radius;
        }

        /// Returns the maximum radius that is used for trust region search.
        /// @return maximum trust region radius
        Scalar maximumRadius() const
        {
            return _maxRadius;
        }

        /// Sets the radius that is initially used by the dogleg method.
        /// The radius must be greater than zero.
        /// @param radius the initial radius that should be used.
        void setInitialRadius(const Scalar radius)
        {
            assert(radius > 0);
            _initialRadius = radius;
        }

        /// Returns the radius that is initially used by the dogleg method.
        /// @return the initial radius
        Scalar initialRadius() const
        {
            return _initialRadius;
        }

    private:
        Scalar _initialRadius = {1};
        Scalar _maxRadius = Scalar{2};
        Scalar _radiusEps = static_cast<Scalar>(1e-6);
        Scalar _acceptFitness = static_cast<Scalar>(0.25);
        Index _maxIt = 0;
    };

    /// Implementation of Powell's Dogleg Method.
    template<typename _Scalar, int _Inputs, int _Outputs>
    class NewtonStepRefiner<_Scalar, _Inputs, _Outputs, DoglegMethod>
    {
    public:
        using Scalar = _Scalar;
        static constexpr int Inputs = _Inputs;
        static constexpr int Outputs = _Outputs;
        using Method = DoglegMethod;
        using Parameter = DoglegMethodParameter<Scalar>;

        static_assert(Eigen::NumTraits<Scalar>::IsInteger == 0, "Step refinement only supports non-integer scalars");

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using HessianMatrix = Eigen::Matrix<Scalar, Inputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        NewtonStepRefiner(const Parameter &param)
            : _param(param), _radius(param.initialRadius())
        { }

        template<typename Objective>
        void operator()(const InputVector &xval,
                        const OutputVector &fval,
                        const JacobiMatrix &jacobian,
                        const GradientVector &gradient,
                        const Objective &objective,
                        StepVector &step)
        {
            // approximate hessian
            const HessianMatrix hessian = jacobian.transpose() * jacobian;

            // precompute the full step length
            const StepVector fullStep = step;
            const auto fullStepLenSq = fullStep.squaredNorm();

            // compute the cauchy step
            const auto gradientLenSq = gradient.squaredNorm();
            const Scalar curvature = gradient.dot(hessian * gradient);
            const StepVector cauchyStep = (gradientLenSq / curvature) * gradient;
            const auto cauchyStepLenSq = cauchyStep.squaredNorm();

            // compute step diff
            const StepVector diffStep = fullStep - cauchyStep;
            const auto diffLenSq = diffStep.squaredNorm();
            const Scalar diffFac = cauchyStep.dot(diffStep) / diffLenSq;

            auto modelFitness = _param.acceptanceFitness() - Scalar{1};
            Index iteration = 0;

            // keep computing while the model fitness is bad
            while(modelFitness < _param.acceptanceFitness() &&
                  (_param.maximumIterations() <= Index{0} || iteration < _param.maximumIterations()))
            {
                const auto radiusSq = _radius * _radius;

                // if the full step is within the trust region simply
                // use it, it provides a good minimizer
                if(fullStepLenSq <= radiusSq)
                {
                    step = fullStep;
                }
                else
                {
                    // if the cauchy step lies outside the trust region
                    // go towards it until the trust region boundary
                    if(cauchyStepLenSq >= radiusSq)
                    {
                        step = (_radius / std::sqrt(cauchyStepLenSq)) * cauchyStep;
                    }
                    else
                    {
                        const auto secondTerm = std::sqrt(diffFac * diffFac + (radiusSq + cauchyStepLenSq) / diffLenSq);
                        const auto scale1 = -diffFac - secondTerm;
                        const auto scale2 = -diffFac + secondTerm;

                        step = cauchyStep + std::max(scale1, scale2) * (fullStep - cauchyStep);
                    }
                }

                // compute the model fitness to determine the update scheme for
                // the trust region radius
                modelFitness = calulateModelFitness(xval, fval, gradient, hessian, step, objective);

                const auto stepLen = step.norm();

                // if the model fitness is really bad reduce the radius!
                if(modelFitness < static_cast<Scalar>(0.25))
                {
                    _radius = static_cast<Scalar>(0.25) * stepLen;
                }
                // if the model fitness is very good then increase it
                else if(modelFitness > static_cast<Scalar>(0.75) && std::abs(stepLen - _radius) < _param.radiusEpsilon())
                {
                    // use the double radius
                    _radius = 2 * _radius;
                    // maintain radius border if configured
                    if(_param.maximumRadius() > 0 && _radius > _param.maximumRadius())
                        _radius = _param.maximumRadius();
                }

                ++iteration;
            }
        }

    private:
        const Parameter &_param;
        Scalar _radius = Scalar{1};

        template<typename Objective>
        Scalar calulateModelFitness(const InputVector &xval,
                                    const OutputVector &fval,
                                    const GradientVector &gradient,
                                    const HessianMatrix &hessian,
                                    const StepVector &step,
                                    const Objective &objective) const
        {
            const Scalar error = fval.squaredNorm() / Scalar{2};

            // evaluate the error function at the new position
            InputVector xvalNext = xval + step;
            OutputVector fvalNext;
            JacobiMatrix jacobianNext;
            objective(xvalNext, fvalNext, jacobianNext);
            // compute the actual new error
            const Scalar nextError = fvalNext.squaredNorm() / Scalar{2};
            // compute the new error by the model
            const Scalar modelError = error + gradient.dot(step) + step.dot(hessian * step) / Scalar{2};

            return (error - nextError) / (error - modelError);
        }
    };

    /// Parameterization class for the GradientDescent step calculation method.
    struct GradientDescentParameter {};

    /// Gradient descent step calculation method.
    struct GradientDescentMethod
    {
        using Parameter = GradientDescentParameter;

        GradientDescentMethod(const Parameter &)
        { }

        /// Computes the newton step as the gradient of the objective function.
        /// @param gradient gradient of the objective function
        /// @param step computed newton step
        /// @return true on success, otherwise false
        template<typename I, typename O, typename J, typename G, typename Objective, typename S>
        bool operator()(const Eigen::MatrixBase<I>&,
                        const Eigen::MatrixBase<O> &,
                        const Eigen::MatrixBase<J> &,
                        const Eigen::MatrixBase<G> &gradient,
                        const Objective &,
                        Eigen::MatrixBase<S>& step) const
        {
            step = gradient;
            return true;
        }
    };

    /// Parameterization class for the GaussNewton step calculation method.
    struct GaussNewtonParameter {};

    /// Gradient descent step calculation method.
    template<typename _Solver=DenseSVDSolver>
    struct GaussNewtonMethod
    {
        using Parameter = GaussNewtonParameter;
        using Solver = _Solver;

        GaussNewtonMethod(const Parameter &)
        { }

        /// Computes the newton step from a linearized approximation of the objective function.
        /// @param gradient gradient of the objective function
        /// @param step computed newton step
        /// @return true on success, otherwise false
        template<typename I, typename O, typename J, typename G, typename Objective, typename S>
        bool operator()(const Eigen::MatrixBase<I> &,
                        const Eigen::MatrixBase<O> &,
                        const Eigen::MatrixBase<J> &jacobian,
                        const Eigen::MatrixBase<G> &gradient,
                        const Objective &,
                        Eigen::MatrixBase<S>& step) const
        {
            Solver solver;
            const auto A = jacobian.transpose() * jacobian;
            using Matrix = typename decltype(A)::PlainMatrix;
            return solver(Matrix(A), gradient, step);
        }
    };

    /// Parameterization class for the LevenbergMarquardt step calculation method.
    template<typename _Scalar>
    class LevenbergMarquardtParameter
    {
    public:
        using Scalar = _Scalar;

        LevenbergMarquardtParameter() = default;

        LevenbergMarquardtParameter(const Scalar initialLambda,
                                    const Scalar increase,
                                    const Scalar decrease,
                                    const Index iterations)
            : _initialLambda(initialLambda), _increase(increase), _decrease(decrease), _maxIt(iterations)
        {
            assert(initialLambda > Scalar{0});
            assert(decrease < Scalar{1});
            assert(decrease > Scalar{0});
            assert(increase > Scalar{1});
        }

        /// Sets the initial gradient descent factor of levenberg marquardt.
        /// @param lambda gradient descent factor
        void setInitialLambda(const Scalar lambda)
        {
            assert(lambda > Scalar{0});
            _initialLambda = lambda;
        }

        /// Returns the initial gradient descent factor of levenberg marquardt.
        /// @return lambda gradient descent factor
        Scalar initialLambda() const
        {
            return _initialLambda;
        }

        /// Sets the maximum iterations of the levenberg marquardt optimization.
        /// Set to 0 or negative for infinite iterations.
        /// @param iterations maximum iterations for lambda search
        void setMaximumIterations(const Index iterations)
        {
            _maxIt = iterations;
        }

        /// Returns the maximum iterations of the levenberg marquardt optimization.
        /// @return maximum iterations for lambda search
        Index maximumIterations() const
        {
            return _maxIt;
        }

        /// Sets the increase factor for the lambda damping.
        /// The value has to be greater than 1.
        /// @param increase factor for increasing lambda
        void setIncrease(const Scalar increase)
        {
            assert(increase > Scalar{1});
            _increase = increase;
        }

        /// Returns the increase factor for the lambda damping.
        /// @return increase factor for increasing lambda
        Scalar increase() const
        {
            return _increase;
        }

        /// Sets the decrease factor for the lambda damping.
        /// The value has to be in (0, 1).
        /// @param decrease factor for decreasing lambda
        void setDecrease(const Scalar decrease)
        {
            assert(decrease < Scalar{1});
            assert(decrease > Scalar{0});
            _decrease = decrease;
        }

        /// Returns the decrease factor for the lambda damping.
        /// @return factor for increasing lambda
        Scalar decrease() const
        {
            return _decrease;
        }
    private:
        Scalar _initialLambda = Scalar{1};
        Scalar _increase = static_cast<Scalar>(2);
        Scalar _decrease = static_cast<Scalar>(0.5);
        Index _maxIt = 0;
    };

    /// Step refinement method which implements the Levenberg Marquardt method.
    template<typename _Scalar, typename _Solver=DenseSVDSolver>
    class LevenbergMarquardtMethod
    {
    public:
        using Scalar = _Scalar;
        using Solver = _Solver;
        using Parameter = LevenbergMarquardtParameter<Scalar>;

        LevenbergMarquardtMethod(const Parameter &param)
            : _param(param), _lambda(param.initialLambda())
        { }

        /// Computes the newton step from a linearized approximation of the objective function.
        /// @param gradient gradient of the objective function
        /// @param step computed newton step
        /// @return true on success, otherwise false
        template<typename I, typename O, typename J, typename G, typename Objective, typename S>
        bool operator()(const Eigen::MatrixBase<I> &xval,
                        const Eigen::MatrixBase<O> &fval,
                        const Eigen::MatrixBase<J> &jacobian,
                        const Eigen::MatrixBase<G> &gradient,
                        const Objective &objective,
                        Eigen::MatrixBase<S>& step)
        {
            using InputVector = typename Eigen::MatrixBase<I>::PlainMatrix;
            using OutputVector = typename Eigen::MatrixBase<O>::PlainMatrix;
            using JacobiMatrix = typename Eigen::MatrixBase<J>::PlainMatrix;
            using SystemMatrix = typename decltype(jacobian.transpose() * jacobian)::PlainMatrix;

            const auto error = fval.squaredNorm() / 2;
            const SystemMatrix jacobianSq = jacobian.transpose() * jacobian;

            Scalar errorN = error + 1;
            SystemMatrix A;
            InputVector xvalN;
            OutputVector fvalN;
            JacobiMatrix jacobianN;
            Solver solver;

            Index iterations = 0;
            while((_param.maximumIterations() <= 0 || iterations < _param.maximumIterations()) &&
                errorN > error)
            {
                A = jacobianSq;
                // add identity matrix
                for(Index i = 0; i < A.rows(); ++i)
                    A(i, i) += _lambda;

                const auto ret = solver(A, gradient, step);
                if(!ret)
                    return false;

                xvalN = xval - step;
                objective(xvalN, fvalN, jacobianN);
                errorN = fvalN.squaredNorm() / 2;

                if(errorN > error)
                    _lambda *= _param.increase();
                else
                    _lambda *= _param.decrease();

                ++iterations;
            }

            return true;
        }

    private:
        Parameter _param = {};
        Scalar _lambda = Scalar{1};
    };

    namespace internal
    {
        template<bool ComputesJacobian>
        struct ObjectiveEvaluator
        { };

        template<>
        struct ObjectiveEvaluator<true>
        {
            template<typename InputVector, typename Objective, typename FiniteDifferences, typename Params, typename OutputVector, typename JacobiMatrix>
            void operator()(const InputVector &xval,
                            const Objective &objective,
                            const FiniteDifferences&,
                            const Params &,
                            OutputVector &fval,
                            JacobiMatrix &jacobian) const
            {
                objective(xval, fval, jacobian);
            }
        };

        template<>
        struct ObjectiveEvaluator<false>
        {
            template<typename InputVector, typename Objective, typename FiniteDifferences, typename Params, typename OutputVector, typename JacobiMatrix>
            void operator()(const InputVector &xval,
                            const Objective &objective,
                            const FiniteDifferences &finiteDifferences,
                            const Params &param,
                            OutputVector &fval,
                            JacobiMatrix &jacobian) const
            {
                objective(xval, fval);
                finiteDifferences(xval, fval, objective, param, jacobian);
            }
        };

        template <typename T>
        bool almostZero(T x)
        {
            return std::abs(x) < std::numeric_limits<T>::epsilon();
        }

        template<typename Derived>
        std::string makePrettyVector(const Eigen::MatrixBase<Derived> &vec)
        {
            assert(vec.cols() == 1);

            std::stringstream ss1;
            ss1 << std::fixed << std::showpoint << std::setprecision(6);
            std::stringstream ss2;
            ss2 << '[';
            for(Index i = 0; i < vec.rows(); ++i)
            {
                ss1 << vec(i, 0);
                ss2 << std::setfill(' ') << std::setw(10) << ss1.str();
                if(i != vec.rows() - 1)
                    ss2 << ' ';
                ss1.str("");
            }
            ss2 << ']';

            return ss2.str();
        }
    }

    /// Core class for least squares algorithms.
    /// It implements the whole optimization strategy except the step
    /// calculation and its refinement.
    template<typename _Scalar,
             int _Inputs,
             int _Outputs,
             typename _Objective,
             typename _StepMethod,
             typename _RefineMethod,
             typename _FiniteDifferencesMethod>
    class LeastSquaresAlgorithm
    {
    public:
        using Scalar = _Scalar;
        constexpr static auto Inputs = _Inputs;
        constexpr static auto Outputs = _Outputs;
        using Objective = _Objective;

        using StepMethod = _StepMethod;
        using StepMethodParameter = typename StepMethod::Parameter;

        using StepRefiner = NewtonStepRefiner<Scalar, Inputs, Outputs, _RefineMethod>;
        using StepRefinerParameter = typename StepRefiner::Parameter;

        using FiniteDifferencesMethod = _FiniteDifferencesMethod;

        using InputVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using OutputVector = Eigen::Matrix<Scalar, Outputs, 1>;
        using JacobiMatrix = Eigen::Matrix<Scalar, Outputs, Inputs>;
        using HessianMatrix = Eigen::Matrix<Scalar, Inputs, Inputs>;
        using GradientVector = Eigen::Matrix<Scalar, Inputs, 1>;
        using StepVector = Eigen::Matrix<Scalar, Inputs, 1>;

        using Callback = std::function<bool(const Index,
                                            const InputVector&,
                                            const OutputVector&,
                                            const JacobiMatrix&,
                                            const GradientVector&,
                                            const StepVector&)>;

        /// Result data returned after optimization.
        struct Result
        {
            InputVector xval = {};
            OutputVector fval = {};
            Scalar error = Scalar{-1};
            Index iterations = -1;
            bool converged = false;
            bool succeeded = false;
        };

        LeastSquaresAlgorithm() = default;

        /// Set the number of threads used to compute gradients.
        /// This only works if OpenMP is enabled.
        /// Set to 0 to allow automatic detection of thread number.
        /// @param threads number of threads to be used
        void setThreads(const Index threads)
        {
            _finiteDifferencesParam.setThreads(threads);
        }

        /// Set the difference for gradient estimation with finite differences.
        /// @param eps numerical epsilon
        void setNumericalEpsilon(const Scalar eps)
        {
            _finiteDifferencesParam.setNumericalEpsilon(eps);
        }

        /// Sets the parameters for the newton step calculation method, e.g. GaussNewton, LevenbergMarquardt, etc.
        /// @param param parameters that should be set.
        void setMethodParameters(const StepMethodParameter &param)
        {
            _methodParam = param;
        }

        /// Returns the parameters for the newton step calculation method.
        /// @return step calculation parameters
        const StepMethodParameter &methodParameters() const
        {
            return _methodParam;
        }

        /// Sets the parameters for the newton step refinement method, e.g. ArmijoBacktracking, DoglegMethod, etc.
        /// @param param parameters that should be set.
        void setRefinementParameters(const StepRefinerParameter &param)
        {
            _refinerParam = param;
        }

        /// Returns the parameters for the newton step refinement method.
        /// @return step calculation parameters
        const StepRefinerParameter &refinementParameters() const
        {
            return _refinerParam;
        }

        /// Sets the instance values of the custom objective function.
        /// Should be used if the objective function requires custom data parameters.
        /// @param objective instance that should be copied
        void setObjective(const Objective &objective)
        {
            _objective = objective;
        }

        /// Sets the instance functor for iteration callbacks.
        /// @param refiner instance that should be copied
        template<typename T>
        void setCallback(const T &callback)
        {
            _callback = callback;
        }

        /// Sets the maximum number of iterations.
        /// Set to 0 or negative for infinite iterations.
        /// @param iterations maximum number of iterations
        void setMaximumIterations(const Index iterations)
        {
            _maxIt = iterations;
        }

        /// Set the minimum step length between two iterations.
        /// If the step length falls below this value, the optimizer stops.
        /// @param steplen minimum step length
        void setMinimumStepLength(const Scalar steplen)
        {
            _minStepLen = steplen;
        }

        /// Set the minimum gradient length.
        /// If the gradient length falls below this value, the optimizer stops.
        /// @param gradlen minimum gradient length
        void setMinimumGradientLength(const Scalar gradlen)
        {
            _minGradLen = gradlen;
        }

        /// Set the minimum squared error.
        /// If the error falls below this value, the optimizer stops.
        /// @param error minimum error
        void setMinimumError(const Scalar error)
        {
            _minError = error;
        }

        /// Set the level of verbosity to print status information after each
        /// iteration.
        /// Set to 0 to deacticate any output.
        /// @param verbosity level of verbosity
        void setVerbosity(const Index verbosity)
        {
            _verbosity = verbosity;
        }

        /// Sets the output stream that is used to print the optimization progress.
        /// @param output the output stream
        void setOutputStream(std::ostream &output)
        {
            _output = &output;
        }

        /// Minimizes the configured objective starting at the given initial guess.
        Result minimize(const InputVector &initialGuess)
        {
            InputVector xval = initialGuess;
            OutputVector fval;
            JacobiMatrix jacobian;
            GradientVector gradient;
            StepVector step = StepVector::Zero(xval.size());

            auto gradLen = _minGradLen + 1;
            auto error = _minError + 1;
            auto stepLen = _minStepLen + 1;
            bool callbackResult = true;
            bool succeeded = true;

            // create objective lambda function which performs automatically numerical estimation
            // of the gradient if necessary.
            const auto objective = [this](const InputVector &xval, OutputVector &fval, JacobiMatrix &jacobian)
            {
                const auto evaluator = internal::ObjectiveEvaluator<Objective::ComputesJacobian>();
                evaluator(xval, this->_objective, FiniteDifferencesMethod(), this->_finiteDifferencesParam, fval, jacobian);
            };

            auto stepMethod = StepMethod(_methodParam);
            auto stepRefiner = StepRefiner(_refinerParam);

            Index iterations = 0;
            while((_maxIt <= 0 || iterations < _maxIt) &&
                   gradLen >= _minGradLen &&
                   stepLen >= _minStepLen &&
                   error >= _minError &&
                   callbackResult)
            {
                xval -= step;
                objective(xval, fval, jacobian);

                error = fval.squaredNorm() / 2;
                gradient = jacobian.transpose() * fval;
                gradLen = gradient.norm();

                // compute the full newton step according to the current method
                if(!stepMethod(xval, fval, jacobian, gradient, objective, step))
                {
                    succeeded = false;
                    break;
                }

                // refine the step according to the current refiner
                stepRefiner(xval, fval, jacobian, gradient, objective, step);
                stepLen = step.norm();

                // evaluate callback if available
                if(_callback)
                {
                    callbackResult = _callback(iterations + 1, xval, fval, jacobian, gradient, step);
                }

                if(_verbosity > 0)
                {
                    using internal::makePrettyVector;

                    std::stringstream ss;
                    ss << "it=" << std::setfill('0')
                        << std::setw(4) << iterations
                        << std::fixed << std::showpoint << std::setprecision(6)
                        << "    steplen=" << stepLen
                        << "    gradlen=" << gradLen;

                    if(_verbosity > 1)
                        ss << "    callback=" << (callbackResult ? "true" : "false");

                    ss << "    error=" << error;

                    if(_verbosity > 2)
                        ss << "    xval=" << makePrettyVector(xval);
                    if(_verbosity > 3)
                        ss << "    step=" << makePrettyVector(step);
                    if(_verbosity > 4)
                        ss << "    fval=" << makePrettyVector(fval);
                    (*_output) << ss.str() << std::endl;
                }

                ++iterations;
            }

            Result result;
            result.xval = xval;
            result.fval = fval;
            result.error = error;
            result.iterations = iterations;
            result.succeeded = succeeded;
            result.converged = stepLen < _minStepLen ||
                gradLen < _minGradLen ||
                error < _minError;

            return result;
        }

    private:
        Objective _objective = {};
        Callback _callback = {};

        StepMethodParameter _methodParam = {};
        StepRefinerParameter _refinerParam = {};
        FiniteDifferencesParameter<Scalar> _finiteDifferencesParam = {};

        Index _maxIt = 0;
        Scalar _minStepLen = static_cast<Scalar>(1e-9);
        Scalar _minGradLen = static_cast<Scalar>(1e-9);
        Scalar _minError = Scalar{0};
        Index _verbosity = 0;
        std::ostream *_output = &std::cout;
    };

    /// General Gauss Newton algorithm.
    template<typename Scalar,
             int Inputs,
             int Outputs,
             typename Objective,
             typename RefineMethod=ConstantStepFactor,
             typename Solver=DenseSVDSolver,
             typename FiniteDifferencesMethod=CentralDifferences>
    using GaussNewton = LeastSquaresAlgorithm<Scalar,
                                              Inputs,
                                              Outputs,
                                              Objective,
                                              GaussNewtonMethod<Solver>,
                                              RefineMethod,
                                              FiniteDifferencesMethod>;

    /// Gauss Newton algorithm with dynamic problem size.
    template<typename Scalar,
             typename Objective,
             typename RefineMethod=ConstantStepFactor,
             typename Solver=DenseSVDSolver,
             typename FiniteDifferencesMethod=CentralDifferences>
    using GaussNewtonX = GaussNewton<Scalar,
                                     Eigen::Dynamic,
                                     Eigen::Dynamic,
                                     Objective,
                                     RefineMethod,
                                     Solver,
                                     FiniteDifferencesMethod>;

    /// General Gradient Descent algorithm.
    template<typename Scalar,
             int Inputs,
             int Outputs,
             typename Objective,
             typename RefineMethod=ConstantStepFactor,
             typename FiniteDifferencesMethod=CentralDifferences>
    using GradientDescent = LeastSquaresAlgorithm<Scalar,
                                                  Inputs,
                                                  Outputs,
                                                  Objective,
                                                  GradientDescentMethod,
                                                  RefineMethod,
                                                  FiniteDifferencesMethod>;

    /// Gradient Descent algorithm with dynamic problem size.
    template<typename Scalar,
             typename Objective,
             typename RefineMethod=ConstantStepFactor,
             typename FiniteDifferencesMethod=CentralDifferences>
    using GradientDescentX = GradientDescent<Scalar,
                                             Eigen::Dynamic,
                                             Eigen::Dynamic,
                                             Objective,
                                             RefineMethod,
                                             FiniteDifferencesMethod>;

    /// General Levenberg Marquardt algorithm.
    template<typename Scalar,
             int Inputs,
             int Outputs,
             typename Objective,
             typename Solver=DenseSVDSolver,
             typename FiniteDifferencesMethod=CentralDifferences>
    using LevenbergMarquardt = LeastSquaresAlgorithm<Scalar,
                                              Inputs,
                                              Outputs,
                                              Objective,
                                              LevenbergMarquardtMethod<Scalar, Solver>,
                                              ConstantStepFactor,
                                              FiniteDifferencesMethod>;

    template<typename Scalar,
             typename Objective,
             typename Solver=DenseSVDSolver,
             typename FiniteDifferencesMethod=CentralDifferences>
    using LevenbergMarquardtX = LevenbergMarquardt<Scalar,
                                     Eigen::Dynamic,
                                     Eigen::Dynamic,
                                     Objective,
                                     Solver,
                                     FiniteDifferencesMethod>;

    namespace parameter
    {
        /// Encodes a 3x3 rotation matrix into a 3-vector in angle axis representation using the rodrigues' formula.
        /// This parametrization is minimal, free of singularities and gimbal lock.
        /// @note https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
        template<typename Derived>
        Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 1> encodeRotation(const Eigen::MatrixBase<Derived> &rotation)
        {
            using Scalar = typename Eigen::MatrixBase<Derived>::Scalar;
            using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
            assert(rotation.rows() == 3);
            assert(rotation.cols() == 3);

            const auto theta = std::acos((rotation.trace() - Scalar{1}) / Scalar{2});

            if(internal::almostZero(theta))
            {
                return Vector3::Zero();
            }
            else
            {
                const auto fac = theta / (Scalar{2} * std::sin(theta));

                return fac * Vector3(rotation(2, 1) - rotation(1, 2),
                                     rotation(0, 2) - rotation(2, 0),
                                     rotation(1, 0) - rotation(0, 1));
            }

        }

        /// Decodes a 3-vector in angle axis representation into a 3x3 rotation matrix using the rodrigues' formula.
        /// @note https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
        template<typename Derived>
        Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 3> decodeRotation(const Eigen::MatrixBase<Derived> &rotation)
        {
            using Scalar = typename Eigen::MatrixBase<Derived>::Scalar;
            using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
            using Vector3 = Eigen::Matrix<Scalar, 3, 1>;

            assert(rotation.rows() == 3);
            assert(rotation.cols() == 1);

            const auto theta = rotation.norm();
            if(internal::almostZero(theta))
            {
                return Matrix3::Identity();
            }
            else
            {
                const Vector3 w = rotation / theta;

                const auto s = std::sin(theta);
                const auto c = std::cos(theta);

                Matrix3 K;
                K << 0, -w.z(), w.y(),
                    w.z(), 0,  -w.x(),
                    -w.y(), w.x(), 0;

                // Matrix3 result;
                // result << c + wx * wx * (1 - c),      wx * wy * (1 - c) - wz * s, wy * s + wx * wz * (1 - c),
                //           wz * s + wx * wy * (1 - c), c + wy * wy * (1 - c), -wx * s + wy * wz * (1 - c),
                //           -wy * s + wx * wz * (1 - c), wx * s + wy * wz * (1 - c), c + wz * wz * (1 - c);

                return Matrix3::Identity() + s * K + (1 - c) * (w * w.transpose() - Matrix3::Identity());
            }
        }
    }
}

#endif

ext: hpp

max_stars_repo_name: Rookfighter/optimization-cpp

max_issues_repo_name: Rookfighter/optimization-cpp

max_forks_repo_name: Rookfighter/optimization-cpp


text:
#include "logging_loop_functions.h"

#include <controllers/footbot_diffusion/footbot_diffusion.h>
#include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_differentialsteering_control.h>
#include <boost/utility/binary.hpp>
#include <iostream>
#include <fstream>
#include <sstream>

//using namespace std;
/****************************************/
/****************************************/

CVector2 cPosInternalA;
int AgentNumber = 10;
LoggingLoopFunctions::LoggingLoopFunctions()
{

}

/****************************************/
/****************************************/

void LoggingLoopFunctions::Init(TConfigurationNode& t_node) {
    try {

    }
    catch(CARGoSException& ex) {
        THROW_ARGOSEXCEPTION_NESTED("Error parsing loop functions!", ex);
    }
}

/****************************************/
/****************************************/

void LoggingLoopFunctions::Reset() {

}

/****************************************/
/****************************************/

void LoggingLoopFunctions::Destroy() {

}

/****************************************/
/****************************************/

void LoggingLoopFunctions::PreStep() {

    //bool 0Eyes1 = false;
    //std::cout << "Rolling" << std::endl;
    CSpace::TMapPerType &m_cFootbots = GetSpace().GetEntitiesByType("foot-bot");
    CVector2 NeighbourDistance;
    Real Counter = -1;

    for (CSpace::TMapPerType::iterator it = m_cFootbots.begin();

         it != m_cFootbots.end();
         ++it) {
        /* Get handle to foot-bot entity and controller */
        Counter = Counter + 1;

        CFootBotEntity &cFootBot = *any_cast<CFootBotEntity *>(it->second);
        CFootBotDiffusion &cController = dynamic_cast<CFootBotDiffusion &>(cFootBot.GetControllableEntity().GetController());
        //CFootBotDiffusion &cWheels = dynamic_cast<CFootBotDiffusion &>(cFootBot.GetControllableEntity().GetRootEntity());
        /* Get the position of the foot-bot on the ground as a CVector2 */
        //if (cFootBot.GetRootEntity().GetId().c_str() == std::string("fb0")) {

        CRadians yaw, pitch, roll;
        CQuaternion orientation_quaternion = cFootBot.GetEmbodiedEntity().GetOriginAnchor().Orientation;
        orientation_quaternion.ToEulerAngles(yaw, pitch, roll);
        CCI_DifferentialSteeringSensor::SReading WheelData = cController.GetWheelVelocity();
        Real Velocity_Wheels =
                0.5 * (cController.GetSpoofLeftWheelVelocity() + cController.GetSpoofRightWheelVelocity());
        Real Difference_Wheels = cController.GetSpoofLeftWheelVelocity() - cController.GetSpoofRightWheelVelocity();
        CCI_RangeAndBearingSensor::TReadings packets = cController.GetRABSensorReadings();

        std::string OriginID = cFootBot.GetRootEntity().GetId().c_str();
        int RealID = OriginID.at(2) + 32;

        for (CCI_RangeAndBearingSensor::SPacket packet : packets) {

        }
    }

    //std::cout << "AGENTs: " << std::endl;
    /*for (int i = 0; i < Agent.size(); i++ ) {
        if (Agent[i] == -83 && GetSpace().GetSimulationClock() > 10) {
            std::cout << "Agent 83" << std::endl;
            std::cout << Agent[i+1] << std::endl;
            std::cout << AgentCoord[i+1] << std::endl;
            std::cout << Agent[i+2] << std::endl;
            std::cout << AgentCoord[i+2] << std::endl;
            std::cout << Agent[i+3] << std::endl;
            std::cout << AgentCoord[i+3] << std::endl;
            std::cout << Agent[i+4] << std::endl;
            std::cout << AgentCoord[i+4] << std::endl;
            std::cout << Agent[i+5] << std::endl;
            std::cout << AgentCoord[i+5] << std::endl;
            FeatureVectors3->push_back(GetSpace().GetSimulationClock());
            FeatureVectors3->push_back(Agent[i+1]);
            FeatureVectors3->push_back(AgentCoord[i+1]);
            FeatureVectors3->push_back(Agent[i+2]);
            FeatureVectors3->push_back(AgentCoord[i+2]);
            FeatureVectors3->push_back(Agent[i+3]);
            FeatureVectors3->push_back(AgentCoord[i+3]);
            FeatureVectors3->push_back(Agent[i+4]);
            FeatureVectors3->push_back(AgentCoord[i+4]);
            FeatureVectors3->push_back(Agent[i+5]);
            FeatureVectors3->push_back(AgentCoord[i+5]);

        }
        //std::cout << Agent[i] << std::endl;
        //std::cout << AgentCoord[i] << std::endl;

    }*/
    if (FeatureVectors3->size() == FeatureVectors3->capacity()) {
        for (int i = 0; i < FeatureVectors3->size(); i++ ) {

            //std::cout << FeatureVectors3->at(i) << std::endl;
        }
    }

    /*if (Agent.size() == 61 && AgentCoord.size() == 61 && GetSpace().GetSimulationClock() < 3000 && GetSpace().GetSimulationClock() > 504) {
        //FVout0.open ("FeatureVectors0.csv", std::ios_base::app);
        //FVout1.open ("FeatureVectors1.csv", std::ios_base::app);
        //FVout2.open ("FeatureVectors2.csv", std::ios_base::app);
        //FVout3.open("FeatureVectors3.csv", std::ios_base::app);
        //FVout4.open ("FeatureVectors4.csv", std::ios_base::app);
        //FVout << Agent[0] << ", ";
        for (int i = 0; i < Agent.size(); i++) {

            if (Agent[i] == -83) {
                // SOFTWARE HANG //
                *//*if (GetSpace().GetSimulationClock() > 501) {
                    if (hang == 0) {
                        hangF1 = Agent[i + 1];
                        hangF2 = Agent[i + 2];
                        hangF3 = Agent[i + 3];
                        hangF4 = Agent[i + 4];
                        hangF5 = Agent[i + 5];
                        hang = 1;
                    }
                    else {
                        Agent[i + 1] = hangF1;
                        Agent[i + 2] = hangF2;
                        Agent[i + 3] = hangF3;
                        Agent[i + 4] = hangF4;
                        Agent[i + 5] = hangF5;

                    }
                }*//*
                // Power Failure //
                *//*if (GetSpace().GetSimulationClock() > 501) {
                    Agent[i+1] = 0;
                    Agent[i+2] = 0;
                    Agent[i+3] = 0;
                    Agent[i+4] = 0;
                    Agent[i+5] = 0;

                }*//*
                // Complete Sensor Failure //
                *//*if (GetSpace().GetSimulationClock() > 501) {
                    Agent[i+1] = 0;
                    Agent[i+2] = 0;

                }*//*
                //FVout << Agent[i+1];
                //FVout << Agent[i+2];
                //FVout << Agent[i+3];
                //FVout << Agent[i+4];
                //FVout << Agent[i+5];

                *//*FVout3 << Agent[i + 1] << ", ";
                FVout3 << AgentCoord[i + 1] << ", ";
                FVout3 << Agent[i + 2] << ", ";
                FVout3 << AgentCoord[i + 2] << ", ";
                FVout3 << Agent[i + 3] << ", ";
                FVout3 << AgentCoord[i + 3] << ", ";
                FVout3 << Agent[i + 4] << ", ";
                FVout3 << AgentCoord[i + 4] << ", ";
                FVout3 << Agent[i + 5] << ", ";
                FVout3 << AgentCoord[i + 5] << ", ";
                FVout3 << std::endl;*//*

            }

        }
        FVout3.close();

    }*/

}

void LoggingLoopFunctions::PostStep() {

}

/****************************************/
/****************************************/

REGISTER_LOOP_FUNCTIONS(LoggingLoopFunctions, "logging_loop_functions")


text:
/*
 * #%L
 * %%
 * Copyright (C) 2011 - 2016 BMW Car IT GmbH
 * %%
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * #L%
 */
#include <array>

#include <boost/concept_check.hpp>
#include <gtest/gtest.h>

#include "muesli/concepts/InputStream.h"
#include "muesli/streams/StringIStream.h"

using namespace ::testing;

template <typename Stream>
using StringIStreamTest = ::testing::Test;

struct RValueReferenceString
{
    static auto getStream(std::string str)
    {
        return muesli::StringIStream(std::move(str));
    }
};

struct LValueReferenceString
{
    static auto getStream(std::string str)
    {
        return muesli::StringIStream(str);
    }
};

using StringInitializationTypes = ::testing::Types<RValueReferenceString, LValueReferenceString>;
TYPED_TEST_CASE(StringIStreamTest, StringInitializationTypes);

TEST(StringIStreamTest, conceptCheck)
{
    BOOST_CONCEPT_ASSERT((muesli::concepts::InputStream<muesli::StringIStream>));
}

TYPED_TEST(StringIStreamTest, getOnEmptyStringReturnsEof)
{
    auto stream = TypeParam::getStream(std::string());

    ASSERT_EQ('\0', stream.get());
}

TYPED_TEST(StringIStreamTest, getCharsOnEmptyStringReturnsEof)
{
    auto stream = TypeParam::getStream(std::string());
    std::array<muesli::StringIStream::Char, 1> dest;

    stream.get(dest.data(), dest.size());

    ASSERT_EQ('\0', dest.at(0));
}

TYPED_TEST(StringIStreamTest, getCharUntilEof)
{
    auto stream = TypeParam::getStream("12");

    ASSERT_EQ('1', stream.get());
    ASSERT_EQ('2', stream.get());
    ASSERT_EQ('\0', stream.get());
}

TYPED_TEST(StringIStreamTest, getChars_RequestMoreCharsThanAvailable)
{
    auto stream = TypeParam::getStream("12");
    std::array<muesli::StringIStream::Char, 3> dest;

    stream.get(dest.data(), dest.size());

    ASSERT_EQ('1', dest.at(0));
    ASSERT_EQ('2', dest.at(1));
    ASSERT_EQ('\0', dest.at(2));
}

TYPED_TEST(StringIStreamTest, getChars_RequestAllChars)
{
    auto stream = TypeParam::getStream("12");
    std::array<muesli::StringIStream::Char, 2> dest;

    stream.get(dest.data(), dest.size());

    ASSERT_EQ('1', dest.at(0));
    ASSERT_EQ('2', dest.at(1));
}

TYPED_TEST(StringIStreamTest, getCharsThanGetChar)
{
    auto stream = TypeParam::getStream("123");
    std::array<muesli::StringIStream::Char, 2> dest;

    stream.get(dest.data(), dest.size());

    ASSERT_EQ('1', dest.at(0));
    ASSERT_EQ('2', dest.at(1));
    ASSERT_EQ('3', stream.get());
}

TYPED_TEST(StringIStreamTest, getCharThanGetChars)
{
    auto stream = TypeParam::getStream("123");
    std::array<muesli::StringIStream::Char, 2> dest;

    ASSERT_EQ('1', stream.get());

    stream.get(dest.data(), dest.size());

    ASSERT_EQ('2', dest.at(0));
    ASSERT_EQ('3', dest.at(1));
}

TYPED_TEST(StringIStreamTest, tell)
{
    auto stream = TypeParam::getStream("12");

    ASSERT_EQ(0, stream.tell());

    stream.get();
    ASSERT_EQ(1, stream.tell());

    stream.get();
    ASSERT_EQ(2, stream.tell());

    muesli::StringIStream::Char output = stream.get();
    ASSERT_EQ('\0', output);
}

TYPED_TEST(StringIStreamTest, peekDoesNotChangePosition)
{
    auto stream = TypeParam::getStream("12");

    ASSERT_EQ('1', stream.peek());
    ASSERT_EQ(0, stream.tell());

    ASSERT_EQ('1', stream.peek());
    ASSERT_EQ(0, stream.tell());
}

TYPED_TEST(StringIStreamTest, peekReturnsNullTerminalAtTheEnd)
{
    auto stream = TypeParam::getStream("1");

    stream.get();

    ASSERT_EQ('\0', stream.peek());
}

TYPED_TEST(StringIStreamTest, initWithLongString)
{
    auto stream = TypeParam::getStream(std::string(1000, '#'));
    ASSERT_EQ('#', stream.peek());
}

max_stars_repo_name: bmwcarit/muesli

max_issues_repo_name: bmwcarit/muesli

max_forks_repo_name: bmwcarit/muesli