// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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)

#include <sstream>

#include <algorithms/test_correct.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/io/dsv/write.hpp>

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

// Note: 3D/box test cannot be done using WKT because:
// -> wkt-box does not exist
// -> so it is converted to a ring
// -> ring representation of 3d-box is not supported, nor feasible
// -> so it uses DSV which can represent a box
template <typename Geometry>
void test_geometry_dsv(std::string const& wkt, std::string const& expected)
{
    Geometry geometry;

    bg::read_wkt(wkt, geometry);
    bg::correct(geometry);

    std::ostringstream out;
    out << bg::dsv(geometry);

    BOOST_CHECK_EQUAL(out.str(), expected);
}

template <typename P>
void test_ring_polygon()
{
    // Define clockwise and counter clockwise polygon
    std::string cw_ring       = "POLYGON((0 0,0 1,1 1,1 0,0 0))";
    std::string ccw_ring      = "POLYGON((0 0,1 0,1 1,0 1,0 0))";
    std::string cw_open_ring  = "POLYGON((0 0,0 1,1 1,1 0))";
    std::string ccw_open_ring = "POLYGON((0 0,1 0,1 1,0 1))";

    // already cw_ring
    test_geometry<bg::model::ring<P> >(cw_ring, cw_ring);

    // wrong order
    test_geometry<bg::model::ring<P> >(ccw_ring, cw_ring);

    // ccw-ring, input ccw-ring, already correct
    test_geometry<bg::model::ring<P, false> >(ccw_ring, ccw_ring);

    // ccw-ring, input cw-ring, corrected
    test_geometry<bg::model::ring<P, false> >(cw_ring, ccw_ring);

    // open-ring, input ccw-ring, already correct
    test_geometry<bg::model::ring<P, true, false> >(cw_open_ring, cw_open_ring);

    // ccw-ring, input cw-ring, corrected
    test_geometry<bg::model::ring<P, true, false> >(ccw_open_ring, "POLYGON((0 1,1 1,1 0,0 0))");

    // not closed
    test_geometry<bg::model::ring<P> >(
            ccw_open_ring,
            cw_ring);

    // counter clockwise, cw_ring
    test_geometry<bg::model::ring<P, false> >(ccw_ring, ccw_ring);

    test_geometry<bg::model::ring<P, false> >(cw_ring, ccw_ring);

    // polygon: cw_ring
    test_geometry<bg::model::polygon<P> >(cw_ring, cw_ring);
    // wrong order
    test_geometry<bg::model::polygon<P> >(
            "POLYGON((0 0,1 0,1 1,0 1,0 0))",
            cw_ring);
    // wrong order & not closed
    test_geometry<bg::model::polygon<P> >(
            ccw_open_ring,
            cw_ring);

    std::string cw_holey_polygon =
            "POLYGON((0 0,0 4,4 4,4 0,0 0),(1 1,2 1,2 2,1 2,1 1))";
    std::string ccw_holey_polygon =
            "POLYGON((0 0,4 0,4 4,0 4,0 0),(1 1,1 2,2 2,2 1,1 1))";

    // with holes: cw_ring
    test_geometry<bg::model::polygon<P> >(
            cw_holey_polygon,
            cw_holey_polygon);
    // wrong order of main
    test_geometry<bg::model::polygon<P> >(
            "POLYGON((0 0,4 0,4 4,0 4,0 0),(1 1,2 1,2 2,1 2,1 1))",
            cw_holey_polygon);
    // wrong order of hole
    test_geometry<bg::model::polygon<P> >(
            "POLYGON((0 0,0 4,4 4,4 0,0 0),(1 1,1 2,2 2,2 1,1 1))",
            cw_holey_polygon);

    // wrong order of main and hole
    test_geometry<bg::model::polygon<P> >(ccw_holey_polygon, cw_holey_polygon);

    // test the counter-clockwise
    test_geometry<bg::model::polygon<P, false> >(
            ccw_holey_polygon, ccw_holey_polygon);

}

template <typename P>
void test_box()
{
    // Boxes
    std::string proper_box = "POLYGON((0 0,0 2,2 2,2 0,0 0))";
    test_geometry<bg::model::box<P> >(proper_box, proper_box);
    test_geometry<bg::model::box<P> >("BOX(0 0,2 2)", proper_box);
    test_geometry<bg::model::box<P> >("BOX(2 2,0 0)", proper_box);
    test_geometry<bg::model::box<P> >("BOX(0 2,2 0)", proper_box);

    // Cubes
    typedef bg::model::box<bg::model::point<double, 3, bg::cs::cartesian> > box3d;
    std::string proper_3d_dsv_box = "((0, 0, 0), (2, 2, 2))";
    test_geometry_dsv<box3d>("BOX(0 0 0,2 2 2)", proper_3d_dsv_box);
    test_geometry_dsv<box3d>("BOX(2 2 2,0 0 0)", proper_3d_dsv_box);
    test_geometry_dsv<box3d>("BOX(0 2 2,2 0 0)", proper_3d_dsv_box);
    test_geometry_dsv<box3d>("BOX(2 0 2,0 2 0)", proper_3d_dsv_box);
    test_geometry_dsv<box3d>("BOX(0 0 2,2 2 0)", proper_3d_dsv_box);
}

template <typename P>
void test_all()
{
    test_ring_polygon<P>();
    test_box<P>();
}

int test_main(int, char* [])
{
    //test_all<int[2]>();
    //test_all<float[2]>(); not yet because cannot be copied, for polygon
    //test_all<double[2]>();

    test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

    test_ring_polygon<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > >();
    test_ring_polygon<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > >();

#if defined(HAVE_TTMATH)
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


#include <boost/graph/adjacency_list.hpp>
#include <string>
typedef std::string vertex_t;

typedef boost::adjacency_list<
    boost::vecS
    , boost::vecS
    , boost::bidirectionalS
    , vertex_t
> graph_type;

#include <iosfwd>

namespace detail {
    template <class GraphT>
    class vertex_writer {
        const GraphT& g_;

    public:
        explicit vertex_writer(const GraphT& g)
            : g_(g)
        {}

        template <class VertexDescriptorT>
        void operator()(
            std::ostream& out,
            const VertexDescriptorT& d) const
        {
            out << " [label=\""
                << boost::get(boost::vertex_bundle, g_)[d]
                << "\"]";
        }
    }; // vertex_writer
} // namespace detail

#include <boost/graph/graphviz.hpp>

std::ostream& operator<<(std::ostream& out, const graph_type& g) {
    detail::vertex_writer<graph_type> vw(g);
    boost::write_graphviz(out, g, vw);

    return out;
}

int main() {
    graph_type graph;

    static const std::size_t vertex_count = 5;
    graph.m_vertices.reserve(vertex_count);
    /*
    C++ -> STL -> Boost -> C++ guru <- C
    */

    typedef boost::graph_traits<graph_type>::vertex_descriptor descriptor_t;
    descriptor_t cpp
        = boost::add_vertex(vertex_t("C++"), graph);
    descriptor_t stl
        = boost::add_vertex(vertex_t("STL"), graph);
    descriptor_t boost
        = boost::add_vertex(vertex_t("Boost"), graph);
    descriptor_t guru
        = boost::add_vertex(vertex_t("C++ guru"), graph);
    descriptor_t ansic
        = boost::add_vertex(vertex_t("C"), graph);

    BOOST_STATIC_ASSERT((boost::is_same<descriptor_t, std::size_t>::value));

    boost::add_edge(cpp, stl, graph);
    boost::add_edge(stl, boost, graph);
    boost::add_edge(boost, guru, graph);
    boost::add_edge(ansic, guru, graph);

    std::cout << graph;
}


#include "drake/systems/analysis/test_utilities/controlled_spring_mass_system.h"

#include "gtest/gtest.h"
#include <Eigen/Dense>

using std::make_unique;

namespace drake {
namespace systems {
namespace {

const double kSpring = 300.0;  // N/m
const double kMass = 2.0;      // kg
const double kProportionalConstant = 1.0;  // N/m
const double kDerivativeConstant = 1.0;  // N*s/m
const double kIntegralConstant = 1.0;  // N/(m*s)
const double kTargetPosition = 1.0;  // m

// A unit test fixture to evaluate the correct functioning of the
// PidControlledSpringMassSystem example.
class SpringMassSystemTest : public ::testing::Test {
 protected:
  void SetUp() override {
    model_ =
        make_unique<PidControlledSpringMassSystem<double>>(
            kSpring, kMass,
            kProportionalConstant, kIntegralConstant, kDerivativeConstant,
            kTargetPosition);

    model_context_ = model_->CreateDefaultContext();
    output_ = model_->AllocateOutput();

    // Gets the plant subcontext.
    plant_context_ =
        &model_->GetMutableSubsystemContext(
            model_->get_plant(), model_context_.get());
  }

  std::unique_ptr<PidControlledSpringMassSystem<double>> model_;
  std::unique_ptr<Context<double>> model_context_;
  Context<double>* plant_context_;
  std::unique_ptr<SystemOutput<double>> output_;
};

// Tests the correct output from the model.
TEST_F(SpringMassSystemTest, EvalOutput) {
  const double x0 = 2.0;
  const double v0 = -1.0;
  // Sets a non-zero initial condition.
  model_->set_position(model_context_.get(), x0);
  model_->set_velocity(model_context_.get(), v0);

  ASSERT_EQ(1, output_->num_ports());
  model_->CalcOutput(*model_context_, output_.get());

  // Output equals the state of the spring-mass plant being controlled which
  // consists of position, velocity and energy.
  Eigen::Vector3d expected_output(x0, v0, 0.0);

  const BasicVector<double>* output = output_->get_vector_data(0);
  ASSERT_NE(nullptr, output);
  EXPECT_EQ(expected_output[0], output->get_value()[0]);
  EXPECT_EQ(expected_output[1], output->get_value()[1]);
  EXPECT_EQ(expected_output[2], output->get_value()[2]);
}

TEST_F(SpringMassSystemTest, EvalTimeDerivatives) {
  const double x0 = 2.0;
  const double v0 = -1.5;

  // Sets a non-zero initial condition.
  model_->set_position(model_context_.get(), x0);
  model_->set_velocity(model_context_.get(), v0);

  std::unique_ptr<ContinuousState<double>> derivatives =
      model_->AllocateTimeDerivatives();
  model_->CalcTimeDerivatives(*model_context_, derivatives.get());

  // The spring-mass plant has a state vector of size 3. One position, one
  // velocity and one miscellaneous state (energy). Moreover, the model has an
  // additional miscellaneous state corresponding to the integral of the PID
  // controller.Therefore the size of the misc state vector is 2.
  ASSERT_EQ(4, derivatives->size());
  ASSERT_EQ(1, derivatives->get_generalized_position().size());
  ASSERT_EQ(1, derivatives->get_generalized_velocity().size());
  ASSERT_EQ(2, derivatives->get_misc_continuous_state().size());

  // The derivatives of plant.
  const ContinuousState<double>& plant_xcdot =
      model_->GetSubsystemDerivatives(model_->get_plant(), *derivatives);

  // Position derivative.
  EXPECT_EQ(v0, plant_xcdot.get_vector().GetAtIndex(0));

  // Acceleration.
  const double error = x0 - kTargetPosition;
  const double error_rate = v0;  // target velocity is zero.
  const double pid_actuation =
      kProportionalConstant * error +  kDerivativeConstant * error_rate;
  EXPECT_EQ((-kSpring * x0 - pid_actuation) / kMass,
            plant_xcdot.get_vector().GetAtIndex(1));

  // Power.
  EXPECT_EQ(model_->get_plant().EvalConservativePower(*plant_context_),
            plant_xcdot.get_vector().GetAtIndex(2));
}

TEST_F(SpringMassSystemTest, DirectFeedthrough) {
  EXPECT_FALSE(model_->HasAnyDirectFeedthrough());
}

}  // namespace
}  // namespace systems
}  // namespace drake


/**
 * @file inertialMeasurementSim.hpp
 * @author Ezra Tal
 * @brief Inertial measurement unit (IMU) simulator class header file
 *
 */

#ifndef INERTIALMEASUREMENTSIM_H
#define INERTIALMEASUREMENTSIM_H

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <random>

/**
 * @brief Inertial measurement unit (IMU) simulator class
 *
 */
class inertialMeasurementSim
{
    public:
        inertialMeasurementSim(double accMeasNoiseVariance, double gyroMeasNoiseVariance,
                               double accBiasProcessNoiseAutoCorrelation, double gyroBiasProcessNoiseAutoCorrelation);

        void setBias(const Eigen::Vector3d & accBias, const Eigen::Vector3d & gyroBias,
                                     double accBiasProcessNoiseAutoCorrelation,
                                     double gyroBiasProcessNoiseAutoCorrelation);
        void setBias(double accBiasVariance, double gyroBiasVariance,
                                     double accBiasProcessNoiseAutoCorrelation,
                                     double gyroBiasProcessNoiseAutoCorrelation);

        void setBias(double accBiasVariance, double gyroBiasVariance);

        void setNoiseVariance(double accMeasNoiseVariance, double gyroMeasNoiseVariance);

        void setOrientation(const Eigen::Quaterniond & imuOrient);

        void getMeasurement(Eigen::Vector3d & accOutput, Eigen::Vector3d & gyroOutput,
                      const Eigen::Vector3d specificForce, const Eigen::Vector3d angularVelocity);

        void proceedBiasDynamics(double dt_secs);

    private:
        /// @name Std normal RNG
        //@{
        std::default_random_engine randomNumberGenerator_;
        std::normal_distribution<double> standardNormalDistribution_ = std::normal_distribution<double>(0.0,1.0);
        //@}

        /// @name Measurement noise variance
        //@{
        double accMeasNoiseVariance_ = 0.; // m^2/s^4
        double gyroMeasNoiseVariance_ = 0.; // rad^2/s^2
        //@}

        /// @name Bias dynamics process noise auto correlation
        //@{
        double accBiasProcessNoiseAutoCorrelation_ = 0.; // m^2/s^5
        double gyroBiasProcessNoiseAutoCorrelation_ = 0.; // rad^2/s^3
        //@}

        /// @name Bias states
        //@{
        Eigen::Vector3d accBias_ = Eigen::Vector3d::Zero(); // m/s^2
        Eigen::Vector3d gyroBias_ = Eigen::Vector3d::Zero(); // rad/s
        //@}

        /// @name Orientation of IMU wrt body-fixed frame
        //@{
        Eigen::Quaterniond imuOrient_ = Eigen::Quaterniond::Identity();
        //@}
};

#endif // INERTIALMEASUREMENTSIM_H


#include <lwr_controllers/one_task_inverse_kinematics.h>
#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>

#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <Eigen/LU>

#include <math.h>

namespace lwr_controllers
{
    OneTaskInverseKinematics::OneTaskInverseKinematics() {}
    OneTaskInverseKinematics::~OneTaskInverseKinematics() {}

    bool OneTaskInverseKinematics::init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n)
    {
        if( !(KinematicChainControllerBase<hardware_interface::PositionJointInterface>::init(robot, n)) )
        {
            ROS_ERROR("Couldn't initilize OneTaskInverseKinematics controller.");
            return false;
        }

        jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
        fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
        ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
        ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));

        q_cmd_.resize(kdl_chain_.getNrOfJoints());
        J_.resize(kdl_chain_.getNrOfJoints());

        // get joint positions
        for(int i=0; i < joint_handles_.size(); i++)
        {
            joint_msr_states_.q(i) = joint_handles_[i].getPosition();
            joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
            joint_des_states_.q(i) = joint_msr_states_.q(i);
        }

        // computing forward kinematics
        fk_pos_solver_->JntToCart(joint_msr_states_.q, x_);

        //Desired posture is the current one
        x_des_ = x_;

        cmd_flag_ = 0;

        sub_command_ = nh_.subscribe("command", 1, &OneTaskInverseKinematics::command, this);

        return true;
    }

    void OneTaskInverseKinematics::starting(const ros::Time& time)
    {

    }

    void OneTaskInverseKinematics::update(const ros::Time& time, const ros::Duration& period)
    {

        // get joint positions
        for(int i=0; i < joint_handles_.size(); i++)
        {
            joint_msr_states_.q(i) = joint_handles_[i].getPosition();
        }

        if (cmd_flag_)
        {
            // computing Jacobian
            jnt_to_jac_solver_->JntToJac(joint_msr_states_.q, J_);

            // computing J_pinv_
            pseudo_inverse(J_.data, J_pinv_);

            // computing forward kinematics
            fk_pos_solver_->JntToCart(joint_msr_states_.q, x_);

            // end-effector position error
            x_err_.vel = x_des_.p - x_.p;

            // getting quaternion from rotation matrix
            x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
            x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);

            skew_symmetric(quat_des_.v, skew_);

            for (int i = 0; i < skew_.rows(); i++)
            {
                v_temp_(i) = 0.0;
                for (int k = 0; k < skew_.cols(); k++)
                    v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
            }

            // end-effector orientation error
            x_err_.rot = quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v - v_temp_;

            // computing q_dot
            for (int i = 0; i < J_pinv_.rows(); i++)
            {
                joint_des_states_.qdot(i) = 0.0;
                for (int k = 0; k < J_pinv_.cols(); k++)
                    joint_des_states_.qdot(i) += J_pinv_(i,k)*x_err_(k); //removed scaling factor of .7

            }

            // integrating q_dot -> getting q (Euler method)
            for (int i = 0; i < joint_handles_.size(); i++)
                joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);

            // joint limits saturation
            for (int i =0;  i < joint_handles_.size(); i++)
            {
                if (joint_des_states_.q(i) < joint_limits_.min(i))
                    joint_des_states_.q(i) = joint_limits_.min(i);
                if (joint_des_states_.q(i) > joint_limits_.max(i))
                    joint_des_states_.q(i) = joint_limits_.max(i);
            }

            if (Equal(x_, x_des_, 0.005))
            {
                ROS_INFO("On target");
                cmd_flag_ = 0;
            }
        }

        // set controls for joints
        for (int i = 0; i < joint_handles_.size(); i++)
        {
            joint_handles_[i].setCommand(joint_des_states_.q(i));
        }
    }

    void OneTaskInverseKinematics::command(const lwr_controllers::PoseRPY::ConstPtr &msg)
    {
        KDL::Frame frame_des_;

        switch(msg->id)
        {
            case 0:
            frame_des_ = KDL::Frame(
                    KDL::Rotation::RPY(msg->orientation.roll,
                                      msg->orientation.pitch,
                                      msg->orientation.yaw),
                    KDL::Vector(msg->position.x,
                                msg->position.y,
                                msg->position.z));
            break;

            case 1: // position only
            frame_des_ = KDL::Frame(
                KDL::Vector(msg->position.x,
                            msg->position.y,
                            msg->position.z));
            break;

            case 2: // orientation only
            frame_des_ = KDL::Frame(
                KDL::Rotation::RPY(msg->orientation.roll,
                                   msg->orientation.pitch,
                                   msg->orientation.yaw));
            break;

            default:
            ROS_INFO("Wrong message ID");
            return;
        }

        x_des_ = frame_des_;
        cmd_flag_ = 1;
    }
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::OneTaskInverseKinematics, controller_interface::ControllerBase)


/*
 * Copyright (c) 2010, Anima Games, Benjamin Karaban, Laurent Schneider,
 * Jérémie Comarmond, Didier Colin.
 * 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 copyright holder 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.
 */
#include "Random.h"
#include <Core/Math/MathTools.h>

#include <ctime>
#include <boost/random.hpp>
#include <boost/utility/base_from_member.hpp>

namespace Core
{

LM_ENUM_1 (EUniform_t, UNIFORM)
LM_ENUM_1 (ENormal_t, NORMAL)

template <typename SuperType> class RandomBase_engine : public SuperType
{
public:
    typedef SuperType super_type;

public:
    RandomBase_engine(uint32 seed) :
        super_type(seed)
    {
    }
};

template <typename SuperType> class RandomBase_distribution : public SuperType
{
public:
    typedef SuperType super_type;
    typedef typename super_type::input_type value_type;

public:
    RandomBase_distribution(const value_type minValue, const value_type maxValue) :
        super_type(minValue, maxValue)
    {
    }
};

template <typename Engine, typename Distribution> class Random :
    public boost::base_from_member<RandomBase_engine<Engine> >,
    public boost::base_from_member<RandomBase_distribution<Distribution> >,
    public boost::variate_generator<Engine&, Distribution>
{
public:
    typedef Engine engine_type;
    typedef Distribution distribution_type;
    typedef typename boost::base_from_member<RandomBase_engine<engine_type> > pbase_engine_type;
    typedef typename boost::base_from_member<RandomBase_distribution<distribution_type> > pbase_distribution_type;
    typedef typename boost::variate_generator<engine_type&, distribution_type> super_type;
    typedef typename Distribution::input_type value_type;

public:
    Random(EUniform_t type, uint32 seed, const value_type minValue, const value_type maxValue) :
        pbase_engine_type(seed), pbase_distribution_type(minValue, maxValue),
                super_type(pbase_engine_type::member, pbase_distribution_type::member)
    {
    }

    Random(ENormal_t type, uint32 seed, const value_type mean, const value_type sigma) :
            pbase_engine_type(seed), pbase_distribution_type(mean, sigma),
                    super_type(pbase_engine_type::member, pbase_distribution_type::member)
    {
    }

    ~Random()
    {
    }
};

typedef Random<boost::minstd_rand, boost::uniform_real<double> > RandomDouble;
typedef Random<boost::minstd_rand, boost::uniform_real<float> > RandomFloat;
typedef Random<boost::minstd_rand, boost::uniform_int<int32> > RandomInt32;

static RandomDouble drandom(UNIFORM, static_cast<uint32>(std::time(0)), 0, 1.f);
static RandomFloat frandom(UNIFORM, static_cast<uint32>(std::time(0)), 0, 1.f);
static RandomInt32 irandom(UNIFORM, static_cast<uint32>(std::time(0)), 0, INT32_MAX);
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
double drand()
{
    return drandom();
}
//-----------------------------------------------------------------------------
double drand(const double min, const double max)
{
    return drandom() * (max-min) + min;
}
//-----------------------------------------------------------------------------
float frand()
{
    return frandom();
}
//-----------------------------------------------------------------------------
float frand(const float min, const float max)
{
    return frandom() * (max-min) + min;
}
//-----------------------------------------------------------------------------
int32 irand()
{
    return irandom();
}
//-----------------------------------------------------------------------------
int32 irand(const int32 min, const int32 max)
{
    int32 range = max - min;

    if(range != 0)
        return min + irandom() % (range + 1);
    else
        return min;
}
//-----------------------------------------------------------------------------
//*****************************************************************************
//-----------------------------------------------------------------------------
RandDistributionf::RandDistributionf(const ERandomDistribution type, const int64 seed, const float min, const float max, const RandomDistributionfData* dData):
    _type(type),
    _min(min),
    _max(max)
{
    float mean, sigma;

    //on initialise les parametres
    if (dData==NULL) //parametres par defaut
    {
        switch (_type)
        {
        case RD_UNIFORM:
            //pas de parametre
        break;
        case RD_NORMAL:
            //pas de parametre
        break;
        case RD_GAUSSIAN:
            _dData.gaussian.mean = 0.5f * (max - min);
            _dData.gaussian.sigma = (max - _dData.gaussian.mean) / 3.0f;
        break;
        }
    }
    else
    {
        _dData = *dData;
    }

    //on cree le random
    switch (_type)
    {
        case RD_UNIFORM:
        {
            _pRand = static_cast<void*> (new Random<boost::minstd_rand, boost::uniform_real<float> > (UNIFORM, (uint32)seed, min, max));
        }
        break;
        case RD_NORMAL:
        {
            mean = (_min+_max)/2.f;
            sigma = (_max-mean)/3.f;
            _pRand = static_cast<void*> (new Random<boost::minstd_rand, boost::normal_distribution<float> > (NORMAL, (uint32)seed, mean,  sigma));
        }
        break;
        case RD_GAUSSIAN:
        {
            mean = _dData.gaussian.mean;
            sigma = _dData.gaussian.sigma;
            _pRand = static_cast<void*> (new Random<boost::minstd_rand, boost::normal_distribution<float> > (NORMAL, (uint32)seed, mean,  sigma));
        }
        break;
    }
}
//-----------------------------------------------------------------------------
RandDistributionf::~RandDistributionf()
{
    switch (_type)
    {
        case RD_UNIFORM:
        {
            typedef Random<boost::minstd_rand, boost::uniform_real<float> > Random_t;
            delete static_cast<Random_t*>(_pRand);
        }
        break;
        case RD_NORMAL:
        {
            typedef Random<boost::minstd_rand, boost::normal_distribution<float> > Random_t;
            delete static_cast<Random_t*>(_pRand);
        }
        break;
        case RD_GAUSSIAN:
        {
            typedef Random<boost::minstd_rand, boost::normal_distribution<float> > Random_t;
            delete static_cast<Random_t*>(_pRand);
        }
        break;
    }
}
//-----------------------------------------------------------------------------
float RandDistributionf::operator() ()
{
    float ret;

    switch (_type)
    {
        case RD_UNIFORM:
        {
            typedef Random<boost::minstd_rand, boost::uniform_real<float> > Random_t;
            ret = (*static_cast<Random_t*>(_pRand))();
        }
        break;
        case RD_NORMAL:
        {
            typedef Random<boost::minstd_rand, boost::normal_distribution<float> > Random_t;
            ret = clamp(_min, _max, (*static_cast<Random_t*>(_pRand))());
        }
        break;
        case RD_GAUSSIAN:
        {
            typedef Random<boost::minstd_rand, boost::normal_distribution<float> > Random_t;
            ret = clamp(_min, _max, (*static_cast<Random_t*>(_pRand))());
        }
        break;
    }

    return ret;
}
//-----------------------------------------------------------------------------
} //namespace Core


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (c) 2021, Institut de Robotica i Informatica Industrial (CSIC-UPC)
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef EAGLE_MPC_TRAJECTORY_HPP_
#define EAGLE_MPC_TRAJECTORY_HPP_

#include <iostream>
#include <map>
#include <string>
#include <vector>

#include <Eigen/Dense>

#include "boost/enable_shared_from_this.hpp"

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"

#include "crocoddyl/core/actuation/squashing/smooth-sat.hpp"
#include "crocoddyl/core/actuation/actuation-squashing.hpp"
#include "crocoddyl/core/optctrl/shooting.hpp"
#include "crocoddyl/core/diff-action-base.hpp"

#include "crocoddyl/multibody/actuations/multicopter-base.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

#include "eagle_mpc/multicopter-base-params.hpp"
#include "eagle_mpc/stage.hpp"
#include "eagle_mpc/utils/params_server.hpp"
#include "eagle_mpc/utils/parser_yaml.hpp"
#include "eagle_mpc/factory/diff-action.hpp"
#include "eagle_mpc/factory/int-action.hpp"

namespace eagle_mpc
{
class Stage;
class DifferentialActionModelFactory;
class IntegratedActionModelFactory;

class Trajectory : public boost::enable_shared_from_this<Trajectory>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    ~Trajectory();
    static boost::shared_ptr<Trajectory> create();

    void autoSetup(const std::string& yaml_path);

    boost::shared_ptr<crocoddyl::ShootingProblem> createProblem() const;
    boost::shared_ptr<crocoddyl::ShootingProblem> createProblem(const std::size_t& dt,
                                                                const bool&        squash,
                                                                const std::string& integration_method) const;

    void removeStage(const std::size_t& idx_stage);

    void set_initial_state(const Eigen::VectorXd& initial_state);

    const std::vector<boost::shared_ptr<Stage>>&                       get_stages() const;
    const boost::shared_ptr<pinocchio::Model>&                         get_robot_model() const;
    const boost::shared_ptr<MultiCopterBaseParams>&                    get_platform_params() const;
    const boost::shared_ptr<crocoddyl::StateMultibody>&                get_robot_state() const;
    const boost::shared_ptr<crocoddyl::ActuationModelMultiCopterBase>& get_actuation() const;
    const boost::shared_ptr<crocoddyl::SquashingModelSmoothSat>&       get_squash() const;
    const boost::shared_ptr<crocoddyl::ActuationSquashingModel>&       get_actuation_squash() const;
    const std::string&                                                 get_robot_model_path() const;
    const Eigen::VectorXd&                                             get_initial_state() const;
    const boost::shared_ptr<ParamsServer>&                             get_params_server() const;
    const bool&                                                        get_has_contact() const;
    const std::size_t&                                                 get_duration() const;

    private:
    Trajectory();
    std::vector<boost::shared_ptr<Stage>> stages_;

    boost::shared_ptr<pinocchio::Model>      robot_model_;
    boost::shared_ptr<MultiCopterBaseParams> platform_params_;
    std::string                              robot_model_path_;

    boost::shared_ptr<crocoddyl::StateMultibody>                robot_state_;
    boost::shared_ptr<crocoddyl::ActuationModelMultiCopterBase> actuation_;
    boost::shared_ptr<crocoddyl::SquashingModelSmoothSat>       squash_;
    boost::shared_ptr<crocoddyl::ActuationSquashingModel>       actuation_squash_;

    Eigen::VectorXd initial_state_;

    bool        has_contact_;
    std::size_t duration_;

    boost::shared_ptr<ParamsServer> params_server_;

    boost::shared_ptr<DifferentialActionModelFactory> dam_factory_;
    boost::shared_ptr<IntegratedActionModelFactory>   iam_factory_;

    struct ProblemParams {
        bool        use_squash;
        int         dt;
        std::string integrator;
    } problem_params_;
};
}  // namespace eagle_mpc

#endif


// Copyright (C) 2014 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unordered_map>
#include <vector>

#include "gtest/gtest.h"
#include "theia/math/util.h"
#include "theia/util/hash.h"
#include "theia/util/map_util.h"
#include "theia/util/random.h"
#include "theia/sfm/filter_view_pairs_from_relative_translation.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/view_graph.h"

namespace theia {

namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

RandomNumberGenerator rng(57);

void CreateViewsWithRandomPoses(
    const int num_views,
    std::unordered_map<ViewId, Vector3d>* orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  (*orientations)[0] = Vector3d::Zero();
  (*positions)[0] = Vector3d::Zero();
  for (int i = 1; i < num_views; i++) {
    (*orientations)[i] = rng.RandVector3d();
    (*positions)[i] = rng.RandVector3d();
  }
}

TwoViewInfo CreateTwoViewInfo(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    const std::unordered_map<ViewId, Vector3d>& positions,
    const ViewIdPair& view_id_pair) {
  TwoViewInfo info;

  Matrix3d orientation1, orientation2;
  ceres::AngleAxisToRotationMatrix(
      FindOrDie(orientations, view_id_pair.first).data(), orientation1.data());
  ceres::AngleAxisToRotationMatrix(
      FindOrDie(orientations, view_id_pair.second).data(), orientation2.data());
  const Matrix3d relative_rotation_mat =
      orientation2 * orientation1.transpose();
  ceres::RotationMatrixToAngleAxis(relative_rotation_mat.data(),
                                   info.rotation_2.data());

  const Vector3d position =
      (FindOrDie(positions, view_id_pair.second) -
       FindOrDie(positions, view_id_pair.first)).normalized();
  info.position_2 = orientation1 * position;

  return info;
}

void CreateValidViewPairs(
    const int num_valid_view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    const std::unordered_map<ViewId, Vector3d>& positions,
    ViewGraph* view_graph) {
  // Add a skeletal graph.
  std::vector<ViewId> view_ids;
  view_ids.push_back(0);
  for (int i = 1; i < orientations.size(); i++) {
    const ViewIdPair view_id_pair(i - 1, i);
    const TwoViewInfo info = CreateTwoViewInfo(orientations,
                                               positions,
                                               view_id_pair);
    view_graph->AddEdge(i - 1, i, info);
    view_ids.push_back(i);
  }

  // Add extra edges.
  while (view_graph->NumEdges() < num_valid_view_pairs) {
    std::random_shuffle(view_ids.begin(), view_ids.end());
    const ViewIdPair view_id_pair(view_ids[0], view_ids[1]);
    if (view_id_pair.first > view_id_pair.second ||
        view_graph->HasEdge(view_id_pair.first, view_id_pair.second)) {
      continue;
    }
    const TwoViewInfo info =
        CreateTwoViewInfo(orientations, positions, view_id_pair);
    view_graph->AddEdge(view_id_pair.first, view_id_pair.second, info);
  }
}

void CreateInvalidViewPairs(
    const int num_invalid_view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    const std::unordered_map<ViewId, Vector3d>& positions,
    ViewGraph* view_graph) {
  const int final_num_view_pairs =
      view_graph->NumEdges() + num_invalid_view_pairs;
  while (view_graph->NumEdges() < final_num_view_pairs) {
    // Choose a random view pair id.
    const ViewIdPair view_id_pair(rng.RandInt(0, orientations.size() - 1),
                                  rng.RandInt(0, orientations.size() - 1));
    if (view_id_pair.first >= view_id_pair.second ||
        view_graph->HasEdge(view_id_pair.first, view_id_pair.second)) {
      continue;
    }

    // Create a valid view pair.
    TwoViewInfo info =
        CreateTwoViewInfo(orientations, positions, view_id_pair);
    // Add a lot of noise to it.
    info.rotation_2 += Vector3d::Ones();
    info.position_2 = rng.RandVector3d().normalized();
    view_graph->AddEdge(view_id_pair.first, view_id_pair.second, info);
  }
}

void TestFilterViewPairsFromRelativeTranslation(
    const int num_views,
    const int num_valid_view_pairs,
    const int num_invalid_view_pairs) {
  std::unordered_map<ViewId, Vector3d> orientations;
  std::unordered_map<ViewId, Vector3d> positions;
  CreateViewsWithRandomPoses(num_views, &orientations, &positions);
  ViewGraph view_graph;
  CreateValidViewPairs(num_valid_view_pairs,
                       orientations,
                       positions,
                       &view_graph);
  CreateInvalidViewPairs(num_invalid_view_pairs,
                         orientations,
                         positions,
                         &view_graph);
  FilterViewPairsFromRelativeTranslationOptions options;
  options.rng = std::make_shared<RandomNumberGenerator>(169);
  FilterViewPairsFromRelativeTranslation(options, orientations, &view_graph);
  EXPECT_GE(view_graph.NumEdges(), num_valid_view_pairs);
}

}  // namespace

TEST(FilterViewPairsFromRelativeTranslation, LineTest) {
  static const int kNumViews = 4;
  static const int kValidViewPairs = 3;
  std::unordered_map<ViewId, Vector3d> orientations;
  std::unordered_map<ViewId, Vector3d> positions;
  for (int i = 0; i < kNumViews; i++) {
    orientations[i] = Vector3d::Zero();
    positions[i] = Vector3d(i, 0, 0);
  }

  ViewGraph view_graph;
  CreateValidViewPairs(kValidViewPairs,
                       orientations,
                       positions,
                       &view_graph);

  // Add two invalid view pairs.
  const ViewIdPair invalid_view_pair(0, 3);
  TwoViewInfo invalid_info;
  // Force the bad translations to be really bad.
  invalid_info.position_2 = Vector3d(-1, -1, -1).normalized();
  view_graph.AddEdge(0, 3, invalid_info);

  FilterViewPairsFromRelativeTranslationOptions options;
  options.rng = std::make_shared<RandomNumberGenerator>(199);
  options.translation_projection_tolerance = 0.1;
  FilterViewPairsFromRelativeTranslation(options, orientations, &view_graph);
  EXPECT_EQ(view_graph.NumEdges(), kValidViewPairs);
}

TEST(FilterViewPairsFromRelativeTranslation, NoBadRotations) {
  TestFilterViewPairsFromRelativeTranslation(10, 30, 0);
}

TEST(FilterViewPairsFromRelativeTranslation, FewBadRotations) {
  TestFilterViewPairsFromRelativeTranslation(10, 30, 5);
}

TEST(FilterViewPairsFromRelativeTranslation, ManyBadRotations) {
  TestFilterViewPairsFromRelativeTranslation(30, 100, 30);
}

}  // namespace theia


/////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2007-2014
//
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/intrusive for documentation.
//
/////////////////////////////////////////////////////////////////////////////
//
// The option that yields to non-floating point 1/sqrt(2) alpha is taken
// from the scapegoat tree implementation of the PSPP library.
//
/////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_INTRUSIVE_SGTREE_HPP
#define BOOST_INTRUSIVE_SGTREE_HPP

#include <boost/intrusive/detail/config_begin.hpp>
#include <boost/intrusive/intrusive_fwd.hpp>
#include <boost/intrusive/detail/assert.hpp>
#include <boost/static_assert.hpp>
#include <boost/intrusive/bs_set_hook.hpp>
#include <boost/intrusive/bstree.hpp>
#include <boost/intrusive/detail/tree_node.hpp>
#include <boost/intrusive/pointer_traits.hpp>
#include <boost/intrusive/detail/mpl.hpp>
#include <boost/intrusive/detail/math.hpp>
#include <boost/intrusive/detail/get_value_traits.hpp>
#include <boost/intrusive/sgtree_algorithms.hpp>
#include <boost/intrusive/detail/key_nodeptr_comp.hpp>
#include <boost/intrusive/link_mode.hpp>

#include <boost/move/utility_core.hpp>
#include <boost/move/adl_move_swap.hpp>

#include <cstddef>
#include <boost/intrusive/detail/minimal_less_equal_header.hpp>
#include <boost/intrusive/detail/minimal_pair_header.hpp>   //std::pair
#include <cmath>
#include <cstddef>

#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

namespace boost {
namespace intrusive {

/// @cond

namespace detail{

/////////////////////////////////////////////////////////////
//
//       Halpha for fixed floating_point<false> option
//
/////////////////////////////////////////////////////////////

//! Returns floor(log2(n)/log2(sqrt(2))) -> floor(2*log2(n))
//! Undefined if N is 0.
//!
//! This function does not use float point operations.
inline std::size_t calculate_h_sqrt2 (std::size_t n)
{
   std::size_t f_log2 = detail::floor_log2(n);
   return (2*f_log2) + static_cast<std::size_t>(n >= detail::sqrt2_pow_2xplus1(f_log2));
}

struct h_alpha_sqrt2_t
{
   h_alpha_sqrt2_t(void){}
   std::size_t operator()(std::size_t n) const
   {  return calculate_h_sqrt2(n);  }
};

struct alpha_0_75_by_max_size_t
{
   alpha_0_75_by_max_size_t(void){}

   std::size_t operator()(std::size_t max_tree_size) const
   {
      const std::size_t max_tree_size_limit = ((~std::size_t(0))/std::size_t(3));
      return max_tree_size > max_tree_size_limit ? max_tree_size/4*3 : max_tree_size*3/4;
   }
};

/////////////////////////////////////////////////////////////
//
//       Halpha for fixed floating_point<true> option
//
/////////////////////////////////////////////////////////////

struct h_alpha_t
{
   explicit h_alpha_t(float inv_minus_logalpha)
      :  inv_minus_logalpha_(inv_minus_logalpha)
   {}

   std::size_t operator()(std::size_t n) const
   {
      ////////////////////////////////////////////////////////////
      // This function must return "floor(log2(1/alpha(n)))" ->
      //    floor(log2(n)/log(1/alpha)) ->
      //    floor(log2(n)/-log2(alpha))
      //    floor(log2(n)*(1/-log2(alpha)))
      ////////////////////////////////////////////////////////////
      return static_cast<std::size_t>(detail::fast_log2(float(n))*inv_minus_logalpha_);
   }

   private:
   //Since the function will be repeatedly called
   //precalculate constant data to avoid repeated
   //calls to log and division.
   //This will store 1/(-std::log2(alpha_))
   float inv_minus_logalpha_;
};

struct alpha_by_max_size_t
{
   explicit alpha_by_max_size_t(float alpha)
      :  alpha_(alpha)
   {}

   float operator()(std::size_t max_tree_size) const
   {  return float(max_tree_size)*alpha_;   }

   private:
   float alpha_;
};

template<bool Activate, class SizeType>
struct alpha_holder
{
   typedef boost::intrusive::detail::h_alpha_t           h_alpha_t;
   typedef boost::intrusive::detail::alpha_by_max_size_t multiply_by_alpha_t;

   alpha_holder()
      : max_tree_size_()
   {  set_alpha(0.70711f);   } // ~1/sqrt(2)

   float get_alpha() const
   {  return alpha_;  }

   void set_alpha(float alpha)
   {
      alpha_ = alpha;
      inv_minus_logalpha_ = 1/(-detail::fast_log2(alpha));
   }

   h_alpha_t get_h_alpha_t() const
   {  return h_alpha_t(inv_minus_logalpha_);  }

   multiply_by_alpha_t get_multiply_by_alpha_t() const
   {  return multiply_by_alpha_t(alpha_);  }

   SizeType &get_max_tree_size()
   {  return max_tree_size_;  }

   protected:
   float alpha_;
   float inv_minus_logalpha_;
   SizeType max_tree_size_;
};

template<class SizeType>
struct alpha_holder<false, SizeType>
{
   //This specialization uses alpha = 1/sqrt(2)
   //without using floating point operations
   //Downside: alpha CAN't be changed.
   typedef boost::intrusive::detail::h_alpha_sqrt2_t           h_alpha_t;
   typedef boost::intrusive::detail::alpha_0_75_by_max_size_t  multiply_by_alpha_t;

   alpha_holder()
      : max_tree_size_()
   {}

   float get_alpha() const
   {  return 0.70710677f;  }

   void set_alpha(float)
   {  //alpha CAN't be changed.
      BOOST_INTRUSIVE_INVARIANT_ASSERT(0);
   }

   h_alpha_t get_h_alpha_t() const
   {  return h_alpha_t();  }

   multiply_by_alpha_t get_multiply_by_alpha_t() const
   {  return multiply_by_alpha_t();  }

   SizeType &get_max_tree_size()
   {  return max_tree_size_;  }

   protected:
   SizeType max_tree_size_;
};

}  //namespace detail{

struct sgtree_defaults
   : bstree_defaults
{
   static const bool floating_point = true;
};

/// @endcond

//! The class template sgtree is an intrusive scapegoat tree container, that
//! is used to construct intrusive sg_set and sg_multiset containers.
//! The no-throw guarantee holds only, if the value_compare object
//! doesn't throw.
//!
//! The template parameter \c T is the type to be managed by the container.
//! The user can specify additional options and if no options are provided
//! default options are used.
//!
//! The container supports the following options:
//! \c base_hook<>/member_hook<>/value_traits<>,
//! \c floating_point<>, \c size_type<> and
//! \c compare<>.
#if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED)
template<class T, class ...Options>
#else
template<class ValueTraits, class VoidOrKeyOfValue, class VoidOrKeyComp, class SizeType, bool FloatingPoint, typename HeaderHolder>
#endif
class sgtree_impl
   /// @cond
   :  public bstree_impl<ValueTraits, VoidOrKeyOfValue, VoidOrKeyComp, SizeType, true, SgTreeAlgorithms, HeaderHolder>
   ,  public detail::alpha_holder<FloatingPoint, SizeType>
   /// @endcond
{
   public:
   typedef ValueTraits                                               value_traits;
   /// @cond
   typedef bstree_impl< ValueTraits, VoidOrKeyOfValue, VoidOrKeyComp, SizeType
                      , true, SgTreeAlgorithms, HeaderHolder>        tree_type;
   typedef tree_type                                                 implementation_defined;

   /// @endcond

   typedef typename implementation_defined::pointer                  pointer;
   typedef typename implementation_defined::const_pointer            const_pointer;
   typedef typename implementation_defined::value_type               value_type;
   typedef typename implementation_defined::key_type                 key_type;
   typedef typename implementation_defined::key_of_value             key_of_value;
   typedef typename implementation_defined::reference                reference;
   typedef typename implementation_defined::const_reference          const_reference;
   typedef typename implementation_defined::difference_type          difference_type;
   typedef typename implementation_defined::size_type                size_type;
   typedef typename implementation_defined::value_compare            value_compare;
   typedef typename implementation_defined::key_compare              key_compare;
   typedef typename implementation_defined::iterator                 iterator;
   typedef typename implementation_defined::const_iterator           const_iterator;
   typedef typename implementation_defined::reverse_iterator         reverse_iterator;
   typedef typename implementation_defined::const_reverse_iterator   const_reverse_iterator;
   typedef typename implementation_defined::node_traits              node_traits;
   typedef typename implementation_defined::node                     node;
   typedef typename implementation_defined::node_ptr                 node_ptr;
   typedef typename implementation_defined::const_node_ptr           const_node_ptr;
   typedef BOOST_INTRUSIVE_IMPDEF(sgtree_algorithms<node_traits>)    node_algorithms;

   static const bool constant_time_size      = implementation_defined::constant_time_size;
   static const bool floating_point          = FloatingPoint;
   static const bool stateful_value_traits   = implementation_defined::stateful_value_traits;

   /// @cond
   private:

   //noncopyable
   typedef detail::alpha_holder<FloatingPoint, SizeType>    alpha_traits;
   typedef typename alpha_traits::h_alpha_t                 h_alpha_t;
   typedef typename alpha_traits::multiply_by_alpha_t       multiply_by_alpha_t;

   BOOST_MOVABLE_BUT_NOT_COPYABLE(sgtree_impl)
   BOOST_STATIC_ASSERT(((int)value_traits::link_mode != (int)auto_unlink));

   enum { safemode_or_autounlink  =
            (int)value_traits::link_mode == (int)auto_unlink   ||
            (int)value_traits::link_mode == (int)safe_link     };

   /// @endcond

   public:

   typedef BOOST_INTRUSIVE_IMPDEF(typename node_algorithms::insert_commit_data) insert_commit_data;

   //! @copydoc ::boost::intrusive::bstree::bstree()
   sgtree_impl()
      :  tree_type()
   {}

   //! @copydoc ::boost::intrusive::bstree::bstree(const key_compare &,const value_traits &)
   explicit sgtree_impl( const key_compare &cmp, const value_traits &v_traits = value_traits())
      :  tree_type(cmp, v_traits)
   {}

   //! @copydoc ::boost::intrusive::bstree::bstree(bool,Iterator,Iterator,const key_compare &,const value_traits &)
   template<class Iterator>
   sgtree_impl( bool unique, Iterator b, Iterator e
              , const key_compare &cmp     = key_compare()
              , const value_traits &v_traits = value_traits())
      : tree_type(cmp, v_traits)
   {
      if(unique)
         this->insert_unique(b, e);
      else
         this->insert_equal(b, e);
   }

   //! @copydoc ::boost::intrusive::bstree::bstree(bstree &&)
   sgtree_impl(BOOST_RV_REF(sgtree_impl) x)
      :  tree_type(BOOST_MOVE_BASE(tree_type, x)), alpha_traits(x.get_alpha_traits())
   {  ::boost::adl_move_swap(this->get_alpha_traits(), x.get_alpha_traits());   }

   //! @copydoc ::boost::intrusive::bstree::operator=(bstree &&)
   sgtree_impl& operator=(BOOST_RV_REF(sgtree_impl) x)
   {
      this->get_alpha_traits() = x.get_alpha_traits();
      return static_cast<sgtree_impl&>(tree_type::operator=(BOOST_MOVE_BASE(tree_type, x)));
   }

   /// @cond
   private:

   const alpha_traits &get_alpha_traits() const
   {  return *this;  }

   alpha_traits &get_alpha_traits()
   {  return *this;  }

   h_alpha_t get_h_alpha_func() const
   {  return this->get_alpha_traits().get_h_alpha_t();  }

   multiply_by_alpha_t get_alpha_by_max_size_func() const
   {  return this->get_alpha_traits().get_multiply_by_alpha_t(); }

   /// @endcond

   public:

   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree::~bstree()
   ~sgtree_impl();

   //! @copydoc ::boost::intrusive::bstree::begin()
   iterator begin();

   //! @copydoc ::boost::intrusive::bstree::begin()const
   const_iterator begin() const;

   //! @copydoc ::boost::intrusive::bstree::cbegin()const
   const_iterator cbegin() const;

   //! @copydoc ::boost::intrusive::bstree::end()
   iterator end();

   //! @copydoc ::boost::intrusive::bstree::end()const
   const_iterator end() const;

   //! @copydoc ::boost::intrusive::bstree::cend()const
   const_iterator cend() const;

   //! @copydoc ::boost::intrusive::bstree::rbegin()
   reverse_iterator rbegin();

   //! @copydoc ::boost::intrusive::bstree::rbegin()const
   const_reverse_iterator rbegin() const;

   //! @copydoc ::boost::intrusive::bstree::crbegin()const
   const_reverse_iterator crbegin() const;

   //! @copydoc ::boost::intrusive::bstree::rend()
   reverse_iterator rend();

   //! @copydoc ::boost::intrusive::bstree::rend()const
   const_reverse_iterator rend() const;

   //! @copydoc ::boost::intrusive::bstree::crend()const
   const_reverse_iterator crend() const;

   //! @copydoc ::boost::intrusive::bstree::root()
   iterator root();

   //! @copydoc ::boost::intrusive::bstree::root()const
   const_iterator root() const;

   //! @copydoc ::boost::intrusive::bstree::croot()const
   const_iterator croot() const;

   //! @copydoc ::boost::intrusive::bstree::container_from_end_iterator(iterator)
   static sgtree_impl &container_from_end_iterator(iterator end_iterator);

   //! @copydoc ::boost::intrusive::bstree::container_from_end_iterator(const_iterator)
   static const sgtree_impl &container_from_end_iterator(const_iterator end_iterator);

   //! @copydoc ::boost::intrusive::bstree::container_from_iterator(iterator)
   static sgtree_impl &container_from_iterator(iterator it);

   //! @copydoc ::boost::intrusive::bstree::container_from_iterator(const_iterator)
   static const sgtree_impl &container_from_iterator(const_iterator it);

   //! @copydoc ::boost::intrusive::bstree::key_comp()const
   key_compare key_comp() const;

   //! @copydoc ::boost::intrusive::bstree::value_comp()const
   value_compare value_comp() const;

   //! @copydoc ::boost::intrusive::bstree::empty()const
   bool empty() const;

   //! @copydoc ::boost::intrusive::bstree::size()const
   size_type size() const;

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED

   //! @copydoc ::boost::intrusive::bstree::swap
   void swap(sgtree_impl& other)
   {
      //This can throw
      this->tree_type::swap(static_cast<tree_type&>(other));
      ::boost::adl_move_swap(this->get_alpha_traits(), other.get_alpha_traits());
   }

   //! @copydoc ::boost::intrusive::bstree::clone_from(const bstree&,Cloner,Disposer)
   //! Additional notes: it also copies the alpha factor from the source container.
   template <class Cloner, class Disposer>
   void clone_from(const sgtree_impl &src, Cloner cloner, Disposer disposer)
   {
      tree_type::clone_from(src, cloner, disposer);
      this->get_alpha_traits() = src.get_alpha_traits();
   }

   //! @copydoc ::boost::intrusive::bstree::clone_from(bstree&&,Cloner,Disposer)
   //! Additional notes: it also copies the alpha factor from the source container.
   template <class Cloner, class Disposer>
   void clone_from(BOOST_RV_REF(sgtree_impl) src, Cloner cloner, Disposer disposer)
   {
      tree_type::clone_from(BOOST_MOVE_BASE(tree_type, src), cloner, disposer);
      this->get_alpha_traits() = ::boost::move(src.get_alpha_traits());
   }

   //! @copydoc ::boost::intrusive::bstree::insert_equal(reference)
   iterator insert_equal(reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_ptr p = node_algorithms::insert_equal_upper_bound
         (this->tree_type::header_ptr(), to_insert, this->key_node_comp(this->key_comp())
         , (size_type)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
      return iterator(p, this->priv_value_traits_ptr());
   }

   //! @copydoc ::boost::intrusive::bstree::insert_equal(const_iterator,reference)
   iterator insert_equal(const_iterator hint, reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_ptr p = node_algorithms::insert_equal
         ( this->tree_type::header_ptr(), hint.pointed_node(), to_insert, this->key_node_comp(this->key_comp())
         , (std::size_t)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
      return iterator(p, this->priv_value_traits_ptr());
   }

   //! @copydoc ::boost::intrusive::bstree::insert_equal(Iterator,Iterator)
   template<class Iterator>
   void insert_equal(Iterator b, Iterator e)
   {
      iterator iend(this->end());
      for (; b != e; ++b)
         this->insert_equal(iend, *b);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique(reference)
   std::pair<iterator, bool> insert_unique(reference value)
   {
      insert_commit_data commit_data;
      std::pair<iterator, bool> ret = this->insert_unique_check
         (key_of_value()(value), this->key_comp(), commit_data);
      if(!ret.second)
         return ret;
      return std::pair<iterator, bool> (this->insert_unique_commit(value, commit_data), true);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique(const_iterator,reference)
   iterator insert_unique(const_iterator hint, reference value)
   {
      insert_commit_data commit_data;
      std::pair<iterator, bool> ret = this->insert_unique_check
         (hint, key_of_value()(value), this->key_comp(), commit_data);
      if(!ret.second)
         return ret.first;
      return this->insert_unique_commit(value, commit_data);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_check(const KeyType&,KeyTypeKeyCompare,insert_commit_data&)
   template<class KeyType, class KeyTypeKeyCompare>
   BOOST_INTRUSIVE_DOC1ST(std::pair<iterator BOOST_INTRUSIVE_I bool>
      , typename detail::disable_if_convertible
         <KeyType BOOST_INTRUSIVE_I const_iterator BOOST_INTRUSIVE_I
         std::pair<iterator BOOST_INTRUSIVE_I bool> >::type)
      insert_unique_check
      (const KeyType &key, KeyTypeKeyCompare comp, insert_commit_data &commit_data)
   {
      std::pair<node_ptr, bool> ret =
         node_algorithms::insert_unique_check
            (this->tree_type::header_ptr(), key, this->key_node_comp(comp), commit_data);
      return std::pair<iterator, bool>(iterator(ret.first, this->priv_value_traits_ptr()), ret.second);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_check(const_iterator,const KeyType&,KeyTypeKeyCompare,insert_commit_data&)
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<iterator, bool> insert_unique_check
      (const_iterator hint, const KeyType &key
      ,KeyTypeKeyCompare comp, insert_commit_data &commit_data)
   {
      std::pair<node_ptr, bool> ret =
         node_algorithms::insert_unique_check
            (this->tree_type::header_ptr(), hint.pointed_node(), key, this->key_node_comp(comp), commit_data);
      return std::pair<iterator, bool>(iterator(ret.first, this->priv_value_traits_ptr()), ret.second);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_check(const key_type&,insert_commit_data&)
   std::pair<iterator, bool> insert_unique_check
      (const key_type &key, insert_commit_data &commit_data)
   {  return this->insert_unique_check(key, this->key_comp(), commit_data);   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_check(const_iterator,const key_type&,insert_commit_data&)
   std::pair<iterator, bool> insert_unique_check
      (const_iterator hint, const key_type &key, insert_commit_data &commit_data)
   {  return this->insert_unique_check(hint, key, this->key_comp(), commit_data);   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_commit
   iterator insert_unique_commit(reference value, const insert_commit_data &commit_data)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_algorithms::insert_unique_commit
         ( this->tree_type::header_ptr(), to_insert, commit_data
         , (std::size_t)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
      return iterator(to_insert, this->priv_value_traits_ptr());
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique(Iterator,Iterator)
   template<class Iterator>
   void insert_unique(Iterator b, Iterator e)
   {
      if(this->empty()){
         iterator iend(this->end());
         for (; b != e; ++b)
            this->insert_unique(iend, *b);
      }
      else{
         for (; b != e; ++b)
            this->insert_unique(*b);
      }
   }

   //! @copydoc ::boost::intrusive::bstree::insert_before
   iterator insert_before(const_iterator pos, reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_ptr p = node_algorithms::insert_before
         ( this->tree_type::header_ptr(), pos.pointed_node(), to_insert
         , (size_type)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
      return iterator(p, this->priv_value_traits_ptr());
   }

   //! @copydoc ::boost::intrusive::bstree::push_back
   void push_back(reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_algorithms::push_back
         ( this->tree_type::header_ptr(), to_insert
         , (size_type)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
   }

   //! @copydoc ::boost::intrusive::bstree::push_front
   void push_front(reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_algorithms::push_front
         ( this->tree_type::header_ptr(), to_insert
         , (size_type)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
   }

   //! @copydoc ::boost::intrusive::bstree::erase(const_iterator)
   iterator erase(const_iterator i)
   {
      const_iterator ret(i);
      ++ret;
      node_ptr to_erase(i.pointed_node());
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(!node_algorithms::unique(to_erase));
      std::size_t max_tree_size = this->max_tree_size_;
      node_algorithms::erase
         ( this->tree_type::header_ptr(), to_erase, (std::size_t)this->size()
         , max_tree_size, this->get_alpha_by_max_size_func());
      this->max_tree_size_ = (size_type)max_tree_size;
      this->tree_type::sz_traits().decrement();
      if(safemode_or_autounlink)
         node_algorithms::init(to_erase);
      return ret.unconst();
   }

   //! @copydoc ::boost::intrusive::bstree::erase(const_iterator,const_iterator)
   iterator erase(const_iterator b, const_iterator e)
   {  size_type n;   return private_erase(b, e, n);   }

   //! @copydoc ::boost::intrusive::bstree::erase(const key_type &)
   size_type erase(const key_type &key)
   {  return this->erase(key, this->key_comp());   }

   //! @copydoc ::boost::intrusive::bstree::erase(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   BOOST_INTRUSIVE_DOC1ST(size_type
      , typename detail::disable_if_convertible<KeyTypeKeyCompare BOOST_INTRUSIVE_I const_iterator BOOST_INTRUSIVE_I size_type>::type)
      erase(const KeyType& key, KeyTypeKeyCompare comp)
   {
      std::pair<iterator,iterator> p = this->equal_range(key, comp);
      size_type n;
      private_erase(p.first, p.second, n);
      return n;
   }

   //! @copydoc ::boost::intrusive::bstree::erase_and_dispose(const_iterator,Disposer)
   template<class Disposer>
   iterator erase_and_dispose(const_iterator i, Disposer disposer)
   {
      node_ptr to_erase(i.pointed_node());
      iterator ret(this->erase(i));
      disposer(this->get_value_traits().to_value_ptr(to_erase));
      return ret;
   }

   #if !defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED)
   template<class Disposer>
   iterator erase_and_dispose(iterator i, Disposer disposer)
   {  return this->erase_and_dispose(const_iterator(i), disposer);   }
   #endif

   //! @copydoc ::boost::intrusive::bstree::erase_and_dispose(const_iterator,const_iterator,Disposer)
   template<class Disposer>
   iterator erase_and_dispose(const_iterator b, const_iterator e, Disposer disposer)
   {  size_type n;   return private_erase(b, e, n, disposer);   }

   //! @copydoc ::boost::intrusive::bstree::erase_and_dispose(const key_type &, Disposer)
   template<class Disposer>
   size_type erase_and_dispose(const key_type &key, Disposer disposer)
   {
      std::pair<iterator,iterator> p = this->equal_range(key);
      size_type n;
      private_erase(p.first, p.second, n, disposer);
      return n;
   }

   //! @copydoc ::boost::intrusive::bstree::erase_and_dispose(const KeyType&,KeyTypeKeyCompare,Disposer)
   template<class KeyType, class KeyTypeKeyCompare, class Disposer>
   BOOST_INTRUSIVE_DOC1ST(size_type
      , typename detail::disable_if_convertible<KeyTypeKeyCompare BOOST_INTRUSIVE_I const_iterator BOOST_INTRUSIVE_I size_type>::type)
      erase_and_dispose(const KeyType& key, KeyTypeKeyCompare comp, Disposer disposer)
   {
      std::pair<iterator,iterator> p = this->equal_range(key, comp);
      size_type n;
      private_erase(p.first, p.second, n, disposer);
      return n;
   }

   //! @copydoc ::boost::intrusive::bstree::clear
   void clear()
   {
      tree_type::clear();
      this->max_tree_size_ = 0;
   }

   //! @copydoc ::boost::intrusive::bstree::clear_and_dispose
   template<class Disposer>
   void clear_and_dispose(Disposer disposer)
   {
      tree_type::clear_and_dispose(disposer);
      this->max_tree_size_ = 0;
   }

   #if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED)
   //! @copydoc ::boost::intrusive::bstree::merge_unique
   template<class T, class ...Options2> void merge_unique(sgtree<T, Options2...> &);
   #else
   template<class Compare2>
   void merge_unique(sgtree_impl
      <ValueTraits, VoidOrKeyOfValue, Compare2, SizeType, FloatingPoint, HeaderHolder> &source)
   #endif
   {
      node_ptr it   (node_algorithms::begin_node(source.header_ptr()))
             , itend(node_algorithms::end_node  (source.header_ptr()));

      while(it != itend){
         node_ptr const p(it);
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(!safemode_or_autounlink || !node_algorithms::unique(p));
         it = node_algorithms::next_node(it);

         std::size_t max_tree1_size = this->max_tree_size_;
         std::size_t max_tree2_size = source.get_max_tree_size();
         if( node_algorithms::transfer_unique
               ( this->header_ptr(), this->key_node_comp(this->key_comp()), this->size(), max_tree1_size
               , source.header_ptr(), p, source.size(), max_tree2_size
               , this->get_h_alpha_func(), this->get_alpha_by_max_size_func()) ){
            this->max_tree_size_  = (size_type)max_tree1_size;
            this->sz_traits().increment();
            source.get_max_tree_size() = (size_type)max_tree2_size;
            source.sz_traits().decrement();
         }
      }
   }

   #if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED)
   //! @copydoc ::boost::intrusive::bstree::merge_equal
   template<class T, class ...Options2> void merge_equal(sgtree<T, Options2...> &);
   #else
   template<class Compare2>
   void merge_equal(sgtree_impl
      <ValueTraits, VoidOrKeyOfValue, Compare2, SizeType, FloatingPoint, HeaderHolder> &source)
   #endif
   {
      node_ptr it   (node_algorithms::begin_node(source.header_ptr()))
             , itend(node_algorithms::end_node  (source.header_ptr()));

      while(it != itend){
         node_ptr const p(it);
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(!safemode_or_autounlink || !node_algorithms::unique(p));
         it = node_algorithms::next_node(it);
         std::size_t max_tree1_size = this->max_tree_size_;
         std::size_t max_tree2_size = source.get_max_tree_size();
         node_algorithms::transfer_equal
            ( this->header_ptr(), this->key_node_comp(this->key_comp()), this->size(), max_tree1_size
            , source.header_ptr(), p, source.size(), max_tree2_size
            , this->get_h_alpha_func(), this->get_alpha_by_max_size_func());
         this->max_tree_size_  = (size_type)max_tree1_size;
         this->sz_traits().increment();
         source.get_max_tree_size() = (size_type)max_tree2_size;
         source.sz_traits().decrement();
      }
   }

   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree::count(const key_type &)const
   size_type count(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::count(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   size_type count(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::lower_bound(const key_type &)
   iterator lower_bound(const key_type &key);

   //! @copydoc ::boost::intrusive::bstree::lower_bound(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   iterator lower_bound(const KeyType& key, KeyTypeKeyCompare comp);

   //! @copydoc ::boost::intrusive::bstree::lower_bound(const key_type &)const
   const_iterator lower_bound(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::lower_bound(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   const_iterator lower_bound(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::upper_bound(const key_type &)
   iterator upper_bound(const key_type &key);

   //! @copydoc ::boost::intrusive::bstree::upper_bound(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   iterator upper_bound(const KeyType& key, KeyTypeKeyCompare comp);

   //! @copydoc ::boost::intrusive::bstree::upper_bound(const key_type &)const
   const_iterator upper_bound(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::upper_bound(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   const_iterator upper_bound(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::find(const key_type &)
   iterator find(const key_type &key);

   //! @copydoc ::boost::intrusive::bstree::find(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   iterator find(const KeyType& key, KeyTypeKeyCompare comp);

   //! @copydoc ::boost::intrusive::bstree::find(const key_type &)const
   const_iterator find(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::find(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   const_iterator find(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::equal_range(const key_type &)
   std::pair<iterator,iterator> equal_range(const key_type &key);

   //! @copydoc ::boost::intrusive::bstree::equal_range(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<iterator,iterator> equal_range(const KeyType& key, KeyTypeKeyCompare comp);

   //! @copydoc ::boost::intrusive::bstree::equal_range(const key_type &)const
   std::pair<const_iterator, const_iterator>
      equal_range(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::equal_range(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<const_iterator, const_iterator>
      equal_range(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::bounded_range(const key_type &,const key_type &,bool,bool)
   std::pair<iterator,iterator> bounded_range
      (const key_type &lower_key, const key_type &upper_key, bool left_closed, bool right_closed);

   //! @copydoc ::boost::intrusive::bstree::bounded_range(const KeyType&,const KeyType&,KeyTypeKeyCompare,bool,bool)
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<iterator,iterator> bounded_range
      (const KeyType& lower_key, const KeyType& upper_key, KeyTypeKeyCompare comp, bool left_closed, bool right_closed);

   //! @copydoc ::boost::intrusive::bstree::bounded_range(const key_type &,const key_type &,bool,bool)const
   std::pair<const_iterator, const_iterator>
      bounded_range(const key_type &lower_key, const key_type &upper_key, bool left_closed, bool right_closed) const;

   //! @copydoc ::boost::intrusive::bstree::bounded_range(const KeyType&,const KeyType&,KeyTypeKeyCompare,bool,bool)const
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<const_iterator, const_iterator> bounded_range
         (const KeyType& lower_key, const KeyType& upper_key, KeyTypeKeyCompare comp, bool left_closed, bool right_closed) const;

   //! @copydoc ::boost::intrusive::bstree::s_iterator_to(reference)
   static iterator s_iterator_to(reference value);

   //! @copydoc ::boost::intrusive::bstree::s_iterator_to(const_reference)
   static const_iterator s_iterator_to(const_reference value);

   //! @copydoc ::boost::intrusive::bstree::iterator_to(reference)
   iterator iterator_to(reference value);

   //! @copydoc ::boost::intrusive::bstree::iterator_to(const_reference)const
   const_iterator iterator_to(const_reference value) const;

   //! @copydoc ::boost::intrusive::bstree::init_node(reference)
   static void init_node(reference value);

   //! @copydoc ::boost::intrusive::bstree::unlink_leftmost_without_rebalance
   pointer unlink_leftmost_without_rebalance();

   //! @copydoc ::boost::intrusive::bstree::replace_node
   void replace_node(iterator replace_this, reference with_this);

   //! @copydoc ::boost::intrusive::bstree::remove_node
   void remove_node(reference value);

   //! @copydoc ::boost::intrusive::bstree::rebalance
   void rebalance();

   //! @copydoc ::boost::intrusive::bstree::rebalance_subtree
   iterator rebalance_subtree(iterator root);

   friend bool operator< (const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator==(const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator!= (const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator>(const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator<=(const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator>=(const sgtree_impl &x, const sgtree_impl &y);

   friend void swap(sgtree_impl &x, sgtree_impl &y);

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED

   //! <b>Returns</b>: The balance factor (alpha) used in this tree
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Complexity</b>: Constant.
   float balance_factor() const
   {  return this->get_alpha_traits().get_alpha(); }

   //! <b>Requires</b>: new_alpha must be a value between 0.5 and 1.0
   //!
   //! <b>Effects</b>: Establishes a new balance factor (alpha) and rebalances
   //!   the tree if the new balance factor is stricter (less) than the old factor.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Complexity</b>: Linear to the elements in the subtree.
   void balance_factor(float new_alpha)
   {
      //The alpha factor CAN't be changed if the fixed, floating operation-less
      //1/sqrt(2) alpha factor option is activated
      BOOST_STATIC_ASSERT((floating_point));
      BOOST_INTRUSIVE_INVARIANT_ASSERT((new_alpha > 0.5f && new_alpha < 1.0f));
      if(new_alpha >= 0.5f && new_alpha < 1.0f){
         float old_alpha = this->get_alpha_traits().get_alpha();
         this->get_alpha_traits().set_alpha(new_alpha);
         if(new_alpha < old_alpha){
            this->max_tree_size_ = this->size();
            this->rebalance();
         }
      }
   }

   /// @cond
   private:
   template<class Disposer>
   iterator private_erase(const_iterator b, const_iterator e, size_type &n, Disposer disposer)
   {
      for(n = 0; b != e; ++n)
        this->erase_and_dispose(b++, disposer);
      return b.unconst();
   }

   iterator private_erase(const_iterator b, const_iterator e, size_type &n)
   {
      for(n = 0; b != e; ++n)
        this->erase(b++);
      return b.unconst();
   }
   /// @endcond
};

//! Helper metafunction to define a \c sgtree that yields to the same type when the
//! same options (either explicitly or implicitly) are used.
#if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED) || defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
template<class T, class ...Options>
#else
template<class T, class O1 = void, class O2 = void
                , class O3 = void, class O4 = void
                , class O5 = void, class O6 = void>
#endif
struct make_sgtree
{
   /// @cond
   typedef typename pack_options
      < sgtree_defaults,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
      O1, O2, O3, O4, O5, O6
      #else
      Options...
      #endif
      >::type packed_options;

   typedef typename detail::get_value_traits
      <T, typename packed_options::proto_value_traits>::type value_traits;

   typedef sgtree_impl
         < value_traits
         , typename packed_options::key_of_value
         , typename packed_options::compare
         , typename packed_options::size_type
         , packed_options::floating_point
         , typename packed_options::header_holder_type
         > implementation_defined;
   /// @endcond
   typedef implementation_defined type;
};

#ifndef BOOST_INTRUSIVE_DOXYGEN_INVOKED

#if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
template<class T, class O1, class O2, class O3, class O4, class O5, class O6>
#else
template<class T, class ...Options>
#endif
class sgtree
   :  public make_sgtree<T,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
      O1, O2, O3, O4, O5, O6
      #else
      Options...
      #endif
      >::type
{
   typedef typename make_sgtree
      <T,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
      O1, O2, O3, O4, O5, O6
      #else
      Options...
      #endif
      >::type   Base;
   BOOST_MOVABLE_BUT_NOT_COPYABLE(sgtree)

   public:
   typedef typename Base::key_compare        key_compare;
   typedef typename Base::value_traits       value_traits;
   typedef typename Base::iterator           iterator;
   typedef typename Base::const_iterator     const_iterator;
   typedef typename Base::reverse_iterator           reverse_iterator;
   typedef typename Base::const_reverse_iterator     const_reverse_iterator;

   //Assert if passed value traits are compatible with the type
   BOOST_STATIC_ASSERT((detail::is_same<typename value_traits::value_type, T>::value));

   sgtree()
      :  Base()
   {}

   explicit sgtree(const key_compare &cmp, const value_traits &v_traits = value_traits())
      :  Base(cmp, v_traits)
   {}

   template<class Iterator>
   sgtree( bool unique, Iterator b, Iterator e
         , const key_compare &cmp = key_compare()
         , const value_traits &v_traits = value_traits())
      :  Base(unique, b, e, cmp, v_traits)
   {}

   sgtree(BOOST_RV_REF(sgtree) x)
      :  Base(BOOST_MOVE_BASE(Base, x))
   {}

   sgtree& operator=(BOOST_RV_REF(sgtree) x)
   {  return static_cast<sgtree &>(this->Base::operator=(BOOST_MOVE_BASE(Base, x)));  }

   template <class Cloner, class Disposer>
   void clone_from(const sgtree &src, Cloner cloner, Disposer disposer)
   {  Base::clone_from(src, cloner, disposer);  }

   template <class Cloner, class Disposer>
   void clone_from(BOOST_RV_REF(sgtree) src, Cloner cloner, Disposer disposer)
   {  Base::clone_from(BOOST_MOVE_BASE(Base, src), cloner, disposer);  }

   static sgtree &container_from_end_iterator(iterator end_iterator)
   {  return static_cast<sgtree &>(Base::container_from_end_iterator(end_iterator));   }

   static const sgtree &container_from_end_iterator(const_iterator end_iterator)
   {  return static_cast<const sgtree &>(Base::container_from_end_iterator(end_iterator));   }

   static sgtree &container_from_iterator(iterator it)
   {  return static_cast<sgtree &>(Base::container_from_iterator(it));   }

   static const sgtree &container_from_iterator(const_iterator it)
   {  return static_cast<const sgtree &>(Base::container_from_iterator(it));   }
};

#endif

} //namespace intrusive
} //namespace boost

#include <boost/intrusive/detail/config_end.hpp>

#endif //BOOST_INTRUSIVE_SGTREE_HPP


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#ifndef BOGUS_EIGEN_MATRIX_TRAITS_HPP
#define BOGUS_EIGEN_MATRIX_TRAITS_HPP

#include <Eigen/Core>

#include "EigenLinearSolvers.hpp"
#include "EigenSparseLinearSolvers.hpp"

#include "../Block/ScalarBindings.hpp"

namespace bogus
{

template < typename _MatrixType >
struct MatrixTraits
{
	typedef _MatrixType MatrixType ;
	typedef typename MatrixType::Scalar Scalar ;

	typedef LU  < Eigen::MatrixBase< MatrixType > > LUType ;
	typedef LDLT< Eigen::MatrixBase< MatrixType > > LDLTType ;

	static const MatrixType& asConstMatrix ( const MatrixType& src )
	{ return src; }
} ;

template < typename _Scalar, int _Options, typename _Index >
struct MatrixTraits< Eigen::SparseMatrix< _Scalar, _Options, _Index > >
{
	typedef _Scalar Scalar ;
	typedef Eigen::SparseMatrix< Scalar, _Options, _Index > MatrixType ;

	typedef LU< Eigen::SparseMatrixBase< Eigen::SparseMatrix< Scalar, _Options, _Index > > > LUType ;
	typedef LDLT< Eigen::SparseMatrixBase< Eigen::SparseMatrix< Scalar, _Options, _Index > > > LDLTType ;

	static const MatrixType& asConstMatrix ( const MatrixType& src )
	{ return src; }
} ;

#define BOGUS_PROCESS_SCALAR( Scalar ) \
template < > struct MatrixTraits< Scalar > : public MatrixTraits< Eigen::Matrix< Scalar, 1, 1> > {\
	static Eigen::Matrix< Scalar, 1, 1> asConstMatrix ( const Scalar src ) {\
		Eigen::Matrix< Scalar, 1, 1>  mat ;\
		mat(0,0) = src ;\
		return mat ; \
	}\
};
BOGUS_BLOCK_SCALAR_TYPES
#undef BOGUS_PROCESS_SCALAR

} //bogus

#endif


// Copyright 2020 Tier IV, Inc.
//
// 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.

#include "detection_by_tracker/utils.hpp"

#include <boost/geometry.hpp>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/utils.h>

#include <algorithm>
#include <vector>

namespace utils
{
void toPolygon2d(
  const autoware_perception_msgs::msg::DynamicObject & object, autoware_utils::Polygon2d & output);
bool isClockWise(const autoware_utils::Polygon2d & polygon);
autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon);

double getArea(const autoware_perception_msgs::msg::Shape & shape)
{
  double area = 0.0;
  if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
    area = getRectangleArea(shape.dimensions);
  } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) {
    area = getCircleArea(shape.dimensions);
  } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) {
    area = getPolygonArea(shape.footprint);
  }
  return area;
}

double getPolygonArea(const geometry_msgs::msg::Polygon & footprint)
{
  double area = 0.0;

  for (size_t i = 0; i < footprint.points.size(); ++i) {
    size_t j = (i + 1) % footprint.points.size();
    area += 0.5 * (footprint.points.at(i).x * footprint.points.at(j).y -
                   footprint.points.at(j).x * footprint.points.at(i).y);
  }

  return area;
}

double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions)
{
  return static_cast<double>(dimensions.x * dimensions.y);
}

double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions)
{
  return static_cast<double>((dimensions.x / 2.0) * (dimensions.x / 2.0) * M_PI);
}

double get2dIoU(
  const autoware_perception_msgs::msg::DynamicObject & object1,
  const autoware_perception_msgs::msg::DynamicObject & object2)
{
  autoware_utils::Polygon2d polygon1, polygon2;
  toPolygon2d(object1, polygon1);
  toPolygon2d(object2, polygon2);

  std::vector<autoware_utils::Polygon2d> union_polygons;
  std::vector<autoware_utils::Polygon2d> intersection_polygons;
  boost::geometry::union_(polygon1, polygon2, union_polygons);
  boost::geometry::intersection(polygon1, polygon2, intersection_polygons);

  double union_area = 0.0;
  double intersection_area = 0.0;
  for (const auto & union_polygon : union_polygons) {
    union_area += boost::geometry::area(union_polygon);
  }
  for (const auto & intersection_polygon : intersection_polygons) {
    intersection_area += boost::geometry::area(intersection_polygon);
  }
  const double iou = union_area < 0.01 ? 0.0 : std::min(1.0, intersection_area / union_area);
  return iou;
}

double get2dPrecision(
  const autoware_perception_msgs::msg::DynamicObject & source_object,
  const autoware_perception_msgs::msg::DynamicObject & target_object)
{
  autoware_utils::Polygon2d source_polygon, target_polygon;
  toPolygon2d(source_object, source_polygon);
  toPolygon2d(target_object, target_polygon);

  std::vector<autoware_utils::Polygon2d> intersection_polygons;
  boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons);

  double intersection_area = 0.0;
  double source_area = 0.0;
  for (const auto & intersection_polygon : intersection_polygons) {
    intersection_area += boost::geometry::area(intersection_polygon);
  }
  source_area = boost::geometry::area(source_polygon);
  const double precision = std::min(1.0, intersection_area / source_area);
  return precision;
}

double get2dRecall(
  const autoware_perception_msgs::msg::DynamicObject & source_object,
  const autoware_perception_msgs::msg::DynamicObject & target_object)
{
  autoware_utils::Polygon2d source_polygon, target_polygon;
  toPolygon2d(source_object, source_polygon);
  toPolygon2d(target_object, target_polygon);

  std::vector<autoware_utils::Polygon2d> intersection_polygons;
  // boost::geometry::union_(source_polygon, target_polygon, intersection_polygons);    // 原来的
  boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); // 我改的

  double intersection_area = 0.0;
  double target_area = 0.0;
  for (const auto & intersection_polygon : intersection_polygons) {
    intersection_area += boost::geometry::area(intersection_polygon);
  }
  target_area += boost::geometry::area(target_polygon);
  const double recall = std::min(1.0, intersection_area / target_area);
  return recall;
}

autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon)
{
  autoware_utils::Polygon2d inverted_polygon;
  for (int i = polygon.outer().size() - 1; 0 <= i; --i) {
    inverted_polygon.outer().push_back(polygon.outer().at(i));
  }
  return inverted_polygon;
}

bool isClockWise(const autoware_utils::Polygon2d & polygon)
{
  const int n = polygon.outer().size();
  const double x_offset = polygon.outer().at(0).x();
  const double y_offset = polygon.outer().at(0).y();
  double sum = 0.0;
  for (std::size_t i = 0; i < polygon.outer().size(); ++i) {
    sum +=
      (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) -
      (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset);
  }

  return sum < 0.0;
}

void toPolygon2d(
  const autoware_perception_msgs::msg::DynamicObject & object, autoware_utils::Polygon2d & output)
{
  if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
    const auto & pose = object.state.pose_covariance.pose;
    const double yaw = autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation));
    Eigen::Matrix2d rotation;
    rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
    Eigen::Vector2d offset0, offset1, offset2, offset3;
    offset0 = rotation *
              Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
    offset1 = rotation *
              Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
    offset2 = rotation *
              Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
    offset3 = rotation *
              Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
    output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
      pose.position.x + offset0.x(), pose.position.y + offset0.y()));
    output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
      pose.position.x + offset1.x(), pose.position.y + offset1.y()));
    output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
      pose.position.x + offset2.x(), pose.position.y + offset2.y()));
    output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
      pose.position.x + offset3.x(), pose.position.y + offset3.y()));
    output.outer().push_back(output.outer().front());
  } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) {
    const auto & center = object.state.pose_covariance.pose.position;
    const auto & radius = object.shape.dimensions.x * 0.5;
    constexpr int n = 6;
    for (int i = 0; i < n; ++i) {
      Eigen::Vector2d point;
      point.x() = std::cos(
                    (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
                    M_PI / static_cast<double>(n)) *
                    radius +
                  center.x;
      point.y() = std::sin(
                    (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
                    M_PI / static_cast<double>(n)) *
                    radius +
                  center.y;
      output.outer().push_back(
        boost::geometry::make<autoware_utils::Point2d>(point.x(), point.y()));
    }
    output.outer().push_back(output.outer().front());
  } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) {
    const auto & pose = object.state.pose_covariance.pose;
    for (const auto & point : object.shape.footprint.points) {
      output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
        pose.position.x + point.x, pose.position.y + point.y));
    }
    output.outer().push_back(output.outer().front());
  }
  output = isClockWise(output) ? output : inverseClockWise(output);
}

}  // namespace utils


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SCALAR_POW_ABS_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_POW_ABS_HPP_INCLUDED

#include <nt2/exponential/functions/pow_abs.hpp>
#include <nt2/exponential/functions/details/pow_kernel.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/log2_em1.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/oneo_16.hpp>
#include <nt2/include/constants/powlargelim.hpp>
#include <nt2/include/constants/powlowlim.hpp>
#include <nt2/include/constants/sixteen.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/floor.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/fms.hpp>
#include <nt2/include/functions/frexp.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/ldexp.hpp>
#include <nt2/include/functions/pow2.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_real.hpp>

#include <boost/dispatch/attributes.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/functions/is_nan.hpp>
#endif

namespace nt2 { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  ( pow_abs_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< floating_<A0> >)
                              (scalar_< floating_<A1> >)
                            )
  {
    typedef A0 result_type;

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename meta::as_integer<A0>::type  iA0;
      A0 x =  nt2::abs(a0);
      if (x == One<A0>()) return x;
      if (is_eqz(x)) return is_eqz(a1);
      #ifndef BOOST_SIMD_NO_INFINITIES
      if(x == a1 && a1 == Inf<A0>())  return Inf<A0>();
      if(x == Inf<A0>() && a1 == Minf<A0>()) return Zero<A0>();
      if(a1 == Inf<A0>()) return (x < One<A0>()) ? Zero<A0>() : Inf<A0>();
      if(a1 == Minf<A0>()) return (x >  One<A0>()) ? Zero<A0>() : Inf<A0>();
      if(x == Inf<A0>()) return (a1 < Zero<A0>()) ? Zero<A0>() : ((a1 == Zero<A0>()) ? One<A0>() : Inf<A0>());
      #endif
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(a0) || is_nan(a1)) return Nan<A0>();
      #endif

      iA0 e;
      x = frexp(x, e );
      iA0 i  = details::pow_kernel<A0>::select(x);
      A0 z = sqr(x);
      A0 w = details::pow_kernel<A0>::pow1(x, z);
      w = fma(Mhalf<A0>(), z, w);
      w = fma(Log2_em1<A0>(), w, w);
      z = fma(Log2_em1<A0>(), x, w);
      z += x;
      w =  fma(A0(-i), Oneo_16<A0>(), A0(e));
      A0 ya = reduc(a1);
      A0 yb = a1 - ya;
      A0 W = fma(z, a1, w*yb);
      A0 Wa = reduc(W);
      A0 Wb = W - Wa;
      W = fma(w, ya, Wa);
      Wa = reduc(W);
      A0 u = W - Wa;
      W = Wb + u;
      Wb = reduc(W);
      w = Sixteen<A0>()*(Wa + Wb);

      // Test the power of 2 for overflow
      if( w > Powlargelim<A0>() ) return Inf<A0>();
      if( w < Powlowlim<A0>()   ) return Zero<A0>();
      e = w;
      Wb = W - Wb;  //
      if( Wb > 0.0f ) //
      {
        ++e;
        Wb -= Oneo_16<A0>(); //
      }
      z = details::pow_kernel<A0>::pow2(Wb)*Wb; //
      i = (e/Sixteen<iA0>()) + (e>=0);
      e = fms(i, Sixteen<iA0>(), e);
      w =  details::pow_kernel<A0>::twomio16(e);
      z = fma(w, z, w);
      z = ldexp( z, i );
      return z;
    }
  private :
    static BOOST_FORCEINLINE A0 reduc(const A0& x)
    {
      // Find a multiple of 1/16 that is within 1/16 of x.
      return Oneo_16<A0>()*floor(Sixteen<A0>()*x);
    }
  };
} }

#endif