<commit_before>// maketable.hpp
//
#pragma once
#ifndef __SDL_SYSTEM_MAKETABLE_HPP__
#define __SDL_SYSTEM_MAKETABLE_HPP__

namespace sdl { namespace db { namespace make {

template<class this_table, class record>
record make_query<this_table, record>::find_with_index(key_type const & key) {
    if (m_cluster) {
        auto const db = m_table.get_db();
        if (auto const id = make::index_tree<key_type>(db, m_cluster).find_page(key)) {
            if (page_head const * const h = db->load_page_head(id)) {
                SDL_ASSERT(h->is_data());
                const datapage data(h);
                if (!data.empty()) {
                    size_t const slot = data.lower_bound(
                        [this, &key](row_head const * const row, size_t) {
                        return (this->read_key(row) < key);
                    });
                    if (slot < data.size()) {
                        row_head const * const head = data[slot];
                        if (!(key < read_key(head))) {
                            return record(&m_table, head);
                        }
                    }
                }
            }
        }
    }
    return {};
}

#if maketable_select_old_code
template<class this_table, class record>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::select(select_key_list in, ignore_index, unique_false) {
    record_range result;
    if (in.size()) {
        result.reserve(in.size());
        for (auto p : m_table) { // scan all table
            A_STATIC_CHECK_TYPE(record, p);
            auto const key = make_query::read_key(p);
            for (auto const & k : in) {
                if (key == k) {
                    result.push_back(p);
                }
            }
        }
    }
    return result;
}

template<class this_table, class record>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::select(select_key_list in, ignore_index, unique_true) {
    record_range result;
    if (in.size()) {
        result.reserve(in.size());
        std::vector<const key_type *> look(in.size());
        for (size_t i = 0; i < in.size(); ++i) {
            look[i] = in.begin() + i;
        }
        size_t size = in.size();
        for (auto p : m_table) { // scan table and filter found keys
            A_STATIC_CHECK_TYPE(record, p);
            auto const key = make_query::read_key(p);
            for (size_t i = 0; i < size; ++i) {
                if (key == *look[i]) {
                    result.push_back(p);
                    if (!(--size))
                        return result;
                    look[i] = look[size];
                }
            }
        }
    }
    return result; // not all keys found
}

template<class this_table, class record>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::select(select_key_list in, use_index, unique_true) {
    record_range result;
    if (in.size()) {
        result.reserve(in.size());
        for (auto const & key : in) {
            if (auto p = find_with_index(key)) {
                result.push_back(p);
            }
        }
    }
    return result;
}

/*template<class this_table, class record>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::select(select_key_list in, enum_index const v1, enum_unique const v2) {
    if (enum_index::ignore_index == v1) {
        if (enum_unique::unique_false == v2) {
            return select(in, enum_index_t<ignore_index>(), enum_unique_t<unique_false>());
        }
        SDL_ASSERT(enum_unique::unique_true == v2);
        return select(in, enum_index_t<ignore_index>(), enum_unique_t<unique_true>());
    }
    SDL_ASSERT(enum_index::use_index == v1);
    SDL_ASSERT(enum_unique::unique_true == v2);
    return select(in, enum_index_t<use_index>(), enum_unique_t<unique_true>());
}*/

/*template<typename T, typename... Ts>
void select_n(where<T> col, Ts const & ... params) {
    enum { col_found = TL::IndexOf<typename this_table::type_list, T>::value };
    enum { key_found = meta::cluster_col_find<KEY_TYPE_LIST, T>::value };
    static_assert(col_found != -1, "");
    using type_list = typename TL::Seq<T, Ts...>::Type; // test
    static_assert(TL::Length<type_list>::value == sizeof...(params) + 1, "");
    //SDL_TRACE(typeid(type_list).name());
    SDL_ASSERT(where<T>::name() == T::name()); // same memory
    SDL_TRACE(
        "col:", col_found,
        " key:", key_found,
        " name:", T::name(),
        " value:", col.value);
    select_n(params...);
}*/
#endif

#if 0
template<class this_table, class record>
struct make_query<this_table, record>::sub_expr_fun
{
    template<class T> // T = where_::SEARCH
    void operator()(identity<T>) {
        if (0) {
            SDL_TRACE(
                T::col::name(), " INDEX::", where_::index_name<T::hint>(), " ",
                T::col::PK ? "PK" : "");
        }
        static_assert((T::hint != where_::INDEX::USE) || T::col::PK, "INDEX::USE");
    }
    template<class T, sortorder ord>
    void operator()(identity<where_::ORDER_BY<T, ord>>) {
    }
    template<class F>
    void operator()(identity<where_::SELECT_IF<F>>) {
    }
};
#endif

namespace make_query_ {

//--------------------------------------------------------------
#if maketable_reverse_order
template <class TList, size_t Count> struct reserse_order;

template <size_t Count>
struct reserse_order<NullType, Count>
{
    using Result = NullType;
};

template <size_t i, class Tail, size_t Count>
struct reserse_order<Typelist<Int2Type<i>, Tail>, Count> {
private:
    using reverse_i = Typelist<Int2Type<Count-1-i>, NullType>;
    static_assert(i < Count, "reserse_order");
public:
    using Result = typename TL::Append<reverse_i, typename reserse_order<Tail, Count>::Result>::Result;
};
#endif
//--------------------------------------------------------------

template<class T, where_::condition cond = T::cond>
struct use_index {
    static_assert((T::hint != where_::INDEX::USE) || T::col::PK, "INDEX::USE");
    enum { value = T::col::PK && (T::hint != where_::INDEX::IGNORE) };
};

template<class T>
struct use_index<T, where_::condition::_lambda> {
    enum { value = false };
};

template<class T>
struct use_index<T, where_::condition::_order> {
    enum { value = false };
};

//--------------------------------------------------------------

template <class TList, class OList, size_t> struct search_use_index;
template <size_t i> struct search_use_index<NullType, NullType, i>
{
    using Index = NullType;
    using Types = NullType;
    using OList = NullType;
};

template <class Head, class Tail, where_::operator_ OP, class NextOP, size_t i>
struct search_use_index<Typelist<Head, Tail>, where_::operator_list<OP, NextOP>, i> {
private:
    enum { flag = use_index<Head>::value };
    using indx_i = typename Select<flag, Typelist<Int2Type<i>, NullType>, NullType>::Result;
    using type_i = typename Select<flag, Typelist<Head, NullType>, NullType>::Result;
    using oper_i = typename Select<flag, Typelist<where_::operator_t<OP>, NullType>, NullType>::Result;
public:
    using Types = typename TL::Append<type_i, typename search_use_index<Tail, NextOP, i + 1>::Types>::Result;
    using Index = typename TL::Append<indx_i, typename search_use_index<Tail, NextOP, i + 1>::Index>::Result;
    using OList = typename TL::Append<oper_i, typename search_use_index<Tail, NextOP, i + 1>::OList>::Result;
};

//--------------------------------------------------------------

template<class TList> struct process_push_back;
template<> struct process_push_back<NullType>
{
    template<class T>
    static void push_back(T &){}
};

template<size_t i, class Tail>
struct process_push_back<Typelist<Int2Type<i>, Tail>>
{
    template<class T>
    static void push_back(T & dest){
        A_STATIC_ASSERT_TYPE(size_t, typename T::value_type);
        dest.push_back(i);
        process_push_back<Tail>::push_back(dest);
    }
};

template<class TList, class T>
inline void push_back(T & dest) {
    dest.reserve(TL::Length<TList>::value);
    process_push_back<TList>::push_back(dest);
}

//--------------------------------------------------------------

template<size_t i, class T, where_::operator_ op>
struct search_key
{
    enum { pos = i };
    using type = T;
    static const where_::operator_ OP = op;
};

template<class Index, class TList, class OList> struct make_search_list;
template<> struct make_search_list<NullType, NullType, NullType>
{
    using Result = NullType;
};

template<
    size_t i, class NextIndex,
    class T, class NextType,
    where_::operator_ OP,  class NextOP>
struct make_search_list<
    Typelist<Int2Type<i>, NextIndex>,
    Typelist<T, NextType>,
    Typelist<where_::operator_t<OP>, NextOP>>
{
private:
    using Item = Typelist<search_key<i, T, OP>,  NullType>;
    using Next = typename make_search_list<NextIndex, NextType, NextOP>::Result;
public:
    using Result = typename TL::Append<Item, Next>::Result;
};

template<class sub_expr_type>
struct SEARCH_USE_INDEX {
private:
    using use_index = make_query_::search_use_index<
                                        typename sub_expr_type::type_list,
                                        typename sub_expr_type::oper_list,
                                        0>;
    using Index = typename use_index::Index;
    using Types = typename use_index::Types;
    using OList = typename use_index::OList;
public:
    using Result = typename make_query_::make_search_list<Index, Types, OList>::Result;

    static_assert(TL::Length<Index>::value == TL::Length<Types>::value, "");
    static_assert(TL::Length<Index>::value == TL::Length<OList>::value, "");
    static_assert(TL::Length<Result>::value == TL::Length<OList>::value, "");
};

//--------------------------------------------------------------

using where_::condition;
using where_::condition_t;

template <class T> // T = search_key
struct SELECT_RECORD_WITH_INDEX : is_static
{
private:
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::WHERE>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::IN>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::NOT>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::LESS>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::GREATER>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::LESS_EQ>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::GREATER_EQ>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::BETWEEN>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
public:
    template<class sub_expr_type>
    static void select(sub_expr_type const & expr) {
        if (0) {
            SDL_TRACE("[", T::pos, "] = ",
                where_::operator_name<T::OP>(), " ",
                where_::condition_name<T::type::cond>()
                );
        }
        auto value = expr.get(Size2Type<T::pos>());
        SELECT_RECORD_WITH_INDEX::select_cond(value, condition_t<T::type::cond>());
    }
};

template <class search_list> struct SELECT_WITH_INDEX;
template <> struct SELECT_WITH_INDEX<NullType>
{
    template<class sub_expr_type> static void select(sub_expr_type const & ) {}
};

template<class T, class NextType> // T = search_key
struct SELECT_WITH_INDEX<Typelist<T, NextType>> {
public:
    template<class sub_expr_type>
    static void select(sub_expr_type const & expr) {
        SELECT_RECORD_WITH_INDEX<T>::select(expr);
        SELECT_WITH_INDEX<NextType>::select(expr);
    }
};

//--------------------------------------------------------------

} // make_query_

template<class this_table, class record>
template<class sub_expr_type>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::VALUES(sub_expr_type const & expr)
{
    SDL_TRACE("\nVALUES:");
    if (1) {
        where_::trace_::trace_sub_expr(expr);
    }
#if maketable_reverse_order
    using use_index = make_query_::search_use_index<
                                        typename sub_expr_type::reverse_type_list,
                                        typename sub_expr_type::reverse_oper_list,
                                        0>;
    using Index = typename make_query_::reserse_order<typename use_index::Index, sub_expr_type::type_size>::Result;
    using Types = typename use_index::Types;
    using OList = typename use_index::OList;
    using SL = typename make_query_::make_search_list<Index, Types, OList>::Result;
#else
    /*using use_index = make_query_::search_use_index<
                                        typename sub_expr_type::type_list,
                                        typename sub_expr_type::oper_list,
                                        0>;
    using Index = typename use_index::Index;
    using Types = typename use_index::Types;
    using OList = typename use_index::OList;
    using SL = typename make_query_::make_search_list<Index, Types, OList>::Result;*/
#endif
    using SL = typename make_query_::SEARCH_USE_INDEX<sub_expr_type>::Result;
    meta::trace_typelist<SL>();
    make_query_::SELECT_WITH_INDEX<SL>::select(expr);
    return {};
}

//std::vector<size_t> test;
//make_query_::push_back<Index>(test);

} // make
} // db
} // sdl

#endif // __SDL_SYSTEM_MAKETABLE_HPP__
<commit_msg>select_::sub_expr<commit_after>// maketable.hpp
//
#pragma once
#ifndef __SDL_SYSTEM_MAKETABLE_HPP__
#define __SDL_SYSTEM_MAKETABLE_HPP__

namespace sdl { namespace db { namespace make {

template<class this_table, class record>
record make_query<this_table, record>::find_with_index(key_type const & key) {
    if (m_cluster) {
        auto const db = m_table.get_db();
        if (auto const id = make::index_tree<key_type>(db, m_cluster).find_page(key)) {
            if (page_head const * const h = db->load_page_head(id)) {
                SDL_ASSERT(h->is_data());
                const datapage data(h);
                if (!data.empty()) {
                    size_t const slot = data.lower_bound(
                        [this, &key](row_head const * const row, size_t) {
                        return (this->read_key(row) < key);
                    });
                    if (slot < data.size()) {
                        row_head const * const head = data[slot];
                        if (!(key < read_key(head))) {
                            return record(&m_table, head);
                        }
                    }
                }
            }
        }
    }
    return {};
}

#if maketable_select_old_code
template<class this_table, class record>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::select(select_key_list in, ignore_index, unique_false) {
    record_range result;
    if (in.size()) {
        result.reserve(in.size());
        for (auto p : m_table) { // scan all table
            A_STATIC_CHECK_TYPE(record, p);
            auto const key = make_query::read_key(p);
            for (auto const & k : in) {
                if (key == k) {
                    result.push_back(p);
                }
            }
        }
    }
    return result;
}

template<class this_table, class record>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::select(select_key_list in, ignore_index, unique_true) {
    record_range result;
    if (in.size()) {
        result.reserve(in.size());
        std::vector<const key_type *> look(in.size());
        for (size_t i = 0; i < in.size(); ++i) {
            look[i] = in.begin() + i;
        }
        size_t size = in.size();
        for (auto p : m_table) { // scan table and filter found keys
            A_STATIC_CHECK_TYPE(record, p);
            auto const key = make_query::read_key(p);
            for (size_t i = 0; i < size; ++i) {
                if (key == *look[i]) {
                    result.push_back(p);
                    if (!(--size))
                        return result;
                    look[i] = look[size];
                }
            }
        }
    }
    return result; // not all keys found
}

template<class this_table, class record>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::select(select_key_list in, use_index, unique_true) {
    record_range result;
    if (in.size()) {
        result.reserve(in.size());
        for (auto const & key : in) {
            if (auto p = find_with_index(key)) {
                result.push_back(p);
            }
        }
    }
    return result;
}

/*template<class this_table, class record>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::select(select_key_list in, enum_index const v1, enum_unique const v2) {
    if (enum_index::ignore_index == v1) {
        if (enum_unique::unique_false == v2) {
            return select(in, enum_index_t<ignore_index>(), enum_unique_t<unique_false>());
        }
        SDL_ASSERT(enum_unique::unique_true == v2);
        return select(in, enum_index_t<ignore_index>(), enum_unique_t<unique_true>());
    }
    SDL_ASSERT(enum_index::use_index == v1);
    SDL_ASSERT(enum_unique::unique_true == v2);
    return select(in, enum_index_t<use_index>(), enum_unique_t<unique_true>());
}*/

/*template<typename T, typename... Ts>
void select_n(where<T> col, Ts const & ... params) {
    enum { col_found = TL::IndexOf<typename this_table::type_list, T>::value };
    enum { key_found = meta::cluster_col_find<KEY_TYPE_LIST, T>::value };
    static_assert(col_found != -1, "");
    using type_list = typename TL::Seq<T, Ts...>::Type; // test
    static_assert(TL::Length<type_list>::value == sizeof...(params) + 1, "");
    //SDL_TRACE(typeid(type_list).name());
    SDL_ASSERT(where<T>::name() == T::name()); // same memory
    SDL_TRACE(
        "col:", col_found,
        " key:", key_found,
        " name:", T::name(),
        " value:", col.value);
    select_n(params...);
}*/
#endif

#if 0
template<class this_table, class record>
struct make_query<this_table, record>::sub_expr_fun
{
    template<class T> // T = where_::SEARCH
    void operator()(identity<T>) {
        if (0) {
            SDL_TRACE(
                T::col::name(), " INDEX::", where_::index_name<T::hint>(), " ",
                T::col::PK ? "PK" : "");
        }
        static_assert((T::hint != where_::INDEX::USE) || T::col::PK, "INDEX::USE");
    }
    template<class T, sortorder ord>
    void operator()(identity<where_::ORDER_BY<T, ord>>) {
    }
    template<class F>
    void operator()(identity<where_::SELECT_IF<F>>) {
    }
};
#endif

namespace make_query_ {

//--------------------------------------------------------------

template<class T, where_::condition cond = T::cond>
struct use_index {
    static_assert((T::hint != where_::INDEX::USE) || T::col::PK, "INDEX::USE");
    enum { value = T::col::PK && (T::hint != where_::INDEX::IGNORE) };
};

template<class T>
struct use_index<T, where_::condition::_lambda> {
    enum { value = false };
};

template<class T>
struct use_index<T, where_::condition::_order> {
    enum { value = false };
};

//--------------------------------------------------------------

template <class TList, class OList, size_t> struct search_use_index;
template <size_t i> struct search_use_index<NullType, NullType, i>
{
    using Index = NullType;
    using Types = NullType;
    using OList = NullType;
};

template <class Head, class Tail, where_::operator_ OP, class NextOP, size_t i>
struct search_use_index<Typelist<Head, Tail>, where_::operator_list<OP, NextOP>, i> {
private:
    enum { flag = use_index<Head>::value };
    using indx_i = typename Select<flag, Typelist<Int2Type<i>, NullType>, NullType>::Result;
    using type_i = typename Select<flag, Typelist<Head, NullType>, NullType>::Result;
    using oper_i = typename Select<flag, Typelist<where_::operator_t<OP>, NullType>, NullType>::Result;
public:
    using Types = typename TL::Append<type_i, typename search_use_index<Tail, NextOP, i + 1>::Types>::Result;
    using Index = typename TL::Append<indx_i, typename search_use_index<Tail, NextOP, i + 1>::Index>::Result;
    using OList = typename TL::Append<oper_i, typename search_use_index<Tail, NextOP, i + 1>::OList>::Result;
};

//--------------------------------------------------------------

template<class TList> struct process_push_back;
template<> struct process_push_back<NullType>
{
    template<class T>
    static void push_back(T &){}
};

template<size_t i, class Tail>
struct process_push_back<Typelist<Int2Type<i>, Tail>>
{
    template<class T>
    static void push_back(T & dest){
        A_STATIC_ASSERT_TYPE(size_t, typename T::value_type);
        dest.push_back(i);
        process_push_back<Tail>::push_back(dest);
    }
};

template<class TList, class T>
inline void push_back(T & dest) {
    dest.reserve(TL::Length<TList>::value);
    process_push_back<TList>::push_back(dest);
}

//--------------------------------------------------------------

template<size_t i, class T, where_::operator_ op>
struct search_key
{
    enum { pos = i };
    using type = T;
    static const where_::operator_ OP = op;
};

template<class Index, class TList, class OList> struct make_search_list;
template<> struct make_search_list<NullType, NullType, NullType>
{
    using Result = NullType;
};

template<
    size_t i, class NextIndex,
    class T, class NextType,
    where_::operator_ OP,  class NextOP>
struct make_search_list<
    Typelist<Int2Type<i>, NextIndex>,
    Typelist<T, NextType>,
    Typelist<where_::operator_t<OP>, NextOP>>
{
private:
    using Item = Typelist<search_key<i, T, OP>,  NullType>;
    using Next = typename make_search_list<NextIndex, NextType, NextOP>::Result;
public:
    using Result = typename TL::Append<Item, Next>::Result;
};

template<class sub_expr_type>
struct SEARCH_USE_INDEX {
private:
    using use_index = make_query_::search_use_index<
                                        typename sub_expr_type::type_list,
                                        typename sub_expr_type::oper_list,
                                        0>;
    using Index = typename use_index::Index;
    using Types = typename use_index::Types;
    using OList = typename use_index::OList;
public:
    using Result = typename make_query_::make_search_list<Index, Types, OList>::Result;

    static_assert(TL::Length<Index>::value == TL::Length<Types>::value, "");
    static_assert(TL::Length<Index>::value == TL::Length<OList>::value, "");
    static_assert(TL::Length<Result>::value == TL::Length<OList>::value, "");
};

//--------------------------------------------------------------

using where_::condition;
using where_::condition_t;

template <class T> // T = search_key
struct SELECT_RECORD_WITH_INDEX : is_static
{
private:
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::WHERE>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::IN>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::NOT>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::LESS>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::GREATER>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::LESS_EQ>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::GREATER_EQ>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
    template<class value_type>
    static void select_cond(value_type const * expr, condition_t<condition::BETWEEN>) {
        SDL_ASSERT(!expr->value.values.empty());
    }
public:
    template<class sub_expr_type>
    static void select(sub_expr_type const & expr) {
        if (0) {
            SDL_TRACE("[", T::pos, "] = ",
                where_::operator_name<T::OP>(), " ",
                where_::condition_name<T::type::cond>()
                );
        }
        auto value = expr.get(Size2Type<T::pos>());
        SELECT_RECORD_WITH_INDEX::select_cond(value, condition_t<T::type::cond>());
    }
};

template <class search_list> struct SELECT_WITH_INDEX;
template <> struct SELECT_WITH_INDEX<NullType>
{
    template<class sub_expr_type> static void select(sub_expr_type const & ) {}
};

template<class T, class NextType> // T = search_key
struct SELECT_WITH_INDEX<Typelist<T, NextType>> {
public:
    template<class sub_expr_type>
    static void select(sub_expr_type const & expr) {
        SELECT_RECORD_WITH_INDEX<T>::select(expr);
        SELECT_WITH_INDEX<NextType>::select(expr);
    }
};

//--------------------------------------------------------------

} // make_query_

template<class this_table, class record>
template<class sub_expr_type>
typename make_query<this_table, record>::record_range
make_query<this_table, record>::VALUES(sub_expr_type const & expr)
{
    SDL_TRACE("\nVALUES:");
    if (1) {
        where_::trace_::trace_sub_expr(expr);
    }
    using SL = typename make_query_::SEARCH_USE_INDEX<sub_expr_type>::Result;
    meta::trace_typelist<SL>();
    make_query_::SELECT_WITH_INDEX<SL>::select(expr);
    return {};
}

//std::vector<size_t> test;
//make_query_::push_back<Index>(test);

} // make
} // db
} // sdl

#endif // __SDL_SYSTEM_MAKETABLE_HPP__
<|endoftext|>


<commit_before>#ifndef CTHREADPOOL
#define CTHREADPOOL
#include<functional>	//bind, function
#include<thread>	//thread::hardware_concurrency, thread::id
#include<type_traits>	//result_of
#include<utility>	//forward
#include"../../lib/header/tool/CPimpl.hpp"

namespace nThread
{
	//1. a fixed-sized threadpool
	//2. cannot return value
	class CThreadPool
	{
	public:
		typedef std::result_of<decltype(std::thread::hardware_concurrency)&()>::type size_type;
		typedef std::thread::id thread_id;
	private:
		struct Impl;
		nTool::CPimpl<Impl> impl_;
		thread_id add_(std::function<void()> &&);
		void add_and_detach_(std::function<void()> &&);
	public:
		//call CThreadPool(std::thread::hardware_concurrency())
		CThreadPool();
		//1. determine how many threads you want to use
		//2. the value you pass will always equal to size
		explicit CThreadPool(size_type);
		//of course, why do you need to copy or move CThreadPool?
		CThreadPool(const CThreadPool &)=delete;
		//1.
		//execute func if available!=0
		//otherwise, waiting until 0<available and execute
		//2.
		//after calling add, available reduce 1
		//3.
		//you have to call join, join_any or join_all after calling add
		//otherwise, there is no available threads even after threads complete the job
		template<class Func,class ... Args>
		inline thread_id add(Func &&func,Args &&...args)
		{
			return add_(std::bind(std::forward<Func>(func),std::forward<Args>(args)...));
		}
		//1.
		//execute func if available!=0
		//otherwise, waiting until 0<available and execute
		//2. after calling add, available reduce 1
		//3. when the func is completed, available increase 1
		//4. if you don't know what "detach" means, you should look std::thread::detach
		template<class Func,class ... Args>
		inline void add_and_detach(Func &&func,Args &&...args)
		{
			add_and_detach_(std::bind(std::forward<Func>(func),std::forward<Args>(args)...));
		}
		//1. return how many threads can be used now
		//2. reduce 1 after calling add or add_and_detach
		size_type available() const noexcept;
		//do not combine join and join_any together in your code
		//it will make some join_any cannot get notification
		//for more details, see example.cpp
		void join(thread_id);
		//1.
		//join_all and wait_until_all_available are different
		//join_all will not wait any threads which belong to detach (add_and_detach)
		//2. it will not block add, you have to control by yourself
		//3. join_all will block join_all
		void join_all();
		//1.
		//do not combine join and join_any together in your code
		//it will make some join_any cannot get notification
		//for more details, see example.cpp
		//2. join_any must return value, because I have implemented add_and_detach already
		thread_id join_any();
		//1. check whether the thread_id of thread is joinable
		//2. after calling add, the id return by add, will make the thread_id of thread joinable
		//3. only when you call join, join_all or join_any (or destructor) will make the thread_id of thread not joinable
		bool joinable(thread_id) const noexcept;
		//1. return total threads can be used
		//2. the size is fixed after constructing
		size_type size() const noexcept;
		//1. wait until available equal to size
		//2.
		//wait_until_all_available and join_all are different
		//join_all will not wait any threads which are belong to detach
		//wait_until_all_available means "wait until available equal to size"
		//3. wait_until_all_available will block wait_until_all_available
		void wait_until_all_available() const;
		//of course, why do you need to copy or move CThreadPool?
		CThreadPool& operator=(const CThreadPool &)=delete;
		//will join all thread when destroying
		~CThreadPool();
	};
}

#endif
<commit_msg>replace std::thread::id with IThreadPoolItemBase::id<commit_after>#ifndef CTHREADPOOL
#define CTHREADPOOL
#include<functional>	//bind, function
#include<thread>	//thread::hardware_concurrency
#include<type_traits>	//result_of
#include<utility>	//forward
#include"../../lib/header/tool/CPimpl.hpp"
#include"IThreadPoolItemBase.hpp"

namespace nThread
{
	//1. a fixed-sized threadpool
	//2. cannot return value
	class CThreadPool
	{
	public:
		typedef std::result_of<decltype(std::thread::hardware_concurrency)&()>::type size_type;
		typedef IThreadPoolItemBase::id thread_id;
	private:
		struct Impl;
		nTool::CPimpl<Impl> impl_;
		thread_id add_(std::function<void()> &&);
		void add_and_detach_(std::function<void()> &&);
	public:
		//call CThreadPool(std::thread::hardware_concurrency())
		CThreadPool();
		//1. determine how many threads you want to use
		//2. the value you pass will always equal to size
		explicit CThreadPool(size_type);
		//of course, why do you need to copy or move CThreadPool?
		CThreadPool(const CThreadPool &)=delete;
		//1.
		//execute func if available!=0
		//otherwise, waiting until 0<available and execute
		//2.
		//after calling add, available reduce 1
		//3.
		//you have to call join, join_any or join_all after calling add
		//otherwise, there is no available threads even after threads complete the job
		template<class Func,class ... Args>
		inline thread_id add(Func &&func,Args &&...args)
		{
			return add_(std::bind(std::forward<Func>(func),std::forward<Args>(args)...));
		}
		//1.
		//execute func if available!=0
		//otherwise, waiting until 0<available and execute
		//2. after calling add, available reduce 1
		//3. when the func is completed, available increase 1
		//4. if you don't know what "detach" means, you should look std::thread::detach
		template<class Func,class ... Args>
		inline void add_and_detach(Func &&func,Args &&...args)
		{
			add_and_detach_(std::bind(std::forward<Func>(func),std::forward<Args>(args)...));
		}
		//1. return how many threads can be used now
		//2. reduce 1 after calling add or add_and_detach
		size_type available() const noexcept;
		//do not combine join and join_any together in your code
		//it will make some join_any cannot get notification
		//for more details, see example.cpp
		void join(thread_id);
		//1.
		//join_all and wait_until_all_available are different
		//join_all will not wait any threads which belong to detach (add_and_detach)
		//2. it will not block add, you have to control by yourself
		//3. join_all will block join_all
		void join_all();
		//1.
		//do not combine join and join_any together in your code
		//it will make some join_any cannot get notification
		//for more details, see example.cpp
		//2. join_any must return value, because I have implemented add_and_detach already
		thread_id join_any();
		//1. check whether the thread_id of thread is joinable
		//2. after calling add, the id return by add, will make the thread_id of thread joinable
		//3. only when you call join, join_all or join_any (or destructor) will make the thread_id of thread not joinable
		bool joinable(thread_id) const noexcept;
		//1. return total threads can be used
		//2. the size is fixed after constructing
		size_type size() const noexcept;
		//1. wait until available equal to size
		//2.
		//wait_until_all_available and join_all are different
		//join_all will not wait any threads which are belong to detach
		//wait_until_all_available means "wait until available equal to size"
		//3. wait_until_all_available will block wait_until_all_available
		void wait_until_all_available() const;
		//of course, why do you need to copy or move CThreadPool?
		CThreadPool& operator=(const CThreadPool &)=delete;
		//will join all thread when destroying
		~CThreadPool();
	};
}

#endif
<|endoftext|>


<commit_before>// Copyright (C) 2013 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)

// These functions are largely based off the the Ceres solver polynomial
// functions which are not available through the public interface. The license
// is below:
//
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// 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 Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: moll.markus@arcor.de (Markus Moll)
//         sameeragarwal@google.com (Sameer Agarwal)

#include "theia/math/polynomial.h"

#include <Eigen/Core>
#include <glog/logging.h>

#include <cmath>
#include <limits>

#include "theia/math/find_polynomial_roots_companion_matrix.h"

namespace theia {

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXcd;

bool FindPolynomialRoots(const VectorXd& polynomial,
                         VectorXd* real,
                         VectorXd* imaginary) {
  return FindPolynomialRootsCompanionMatrix(polynomial, real, imaginary);
}

// Remove leading terms with zero coefficients.
VectorXd RemoveLeadingZeros(const VectorXd& polynomial_in) {
  int i = 0;
  while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0) {
    ++i;
  }
  return polynomial_in.tail(polynomial_in.size() - i);
}

VectorXd DifferentiatePolynomial(const VectorXd& polynomial) {
  const int degree = polynomial.rows() - 1;
  CHECK_GE(degree, 0);

  // Degree zero polynomials are constants, and their derivative does
  // not result in a smaller degree polynomial, just a degree zero
  // polynomial with value zero.
  if (degree == 0) {
    return VectorXd::Zero(1);
  }

  VectorXd derivative(degree);
  for (int i = 0; i < degree; ++i) {
    derivative(i) = (degree - i) * polynomial(i);
  }

  return derivative;
}

VectorXd MultiplyPolynomials(const VectorXd& poly1, const VectorXd& poly2) {
  VectorXd multiplied_poly = VectorXd::Zero(poly1.size() + poly2.size() - 1);;
  for (int i = 0; i < poly1.size(); i++) {
    for (int j = 0; j < poly2.size(); j++) {
      multiplied_poly.reverse()(i + j) +=
          poly1.reverse()(i) * poly2.reverse()(j);
    }
  }
  return multiplied_poly;
}

void DividePolynomial(const VectorXd& polynomial,
                      const VectorXd& divisor,
                      VectorXd* quotient,
                      VectorXd* remainder) {
  // If the divisor is higher degree than the polynomial then it cannot be
  // divided so we simply return the remainder.
  if (polynomial.size() < divisor.size()) {
    *quotient = VectorXd::Zero(1);
    *remainder = polynomial;
    return;
  }

  VectorXd numerator = RemoveLeadingZeros(polynomial);
  VectorXd denominator;
  *quotient = VectorXd::Zero(numerator.size() - divisor.size() + 1);
  while (numerator.size() >= divisor.size()) {
    denominator = VectorXd::Zero(numerator.size());
    denominator.head(divisor.size()) = divisor;

    const double quotient_scalar = numerator(0) / denominator(0);
    quotient->reverse()(numerator.size() - divisor.size()) =
        quotient_scalar;
    denominator = denominator * quotient_scalar;
    numerator = numerator - denominator;
    // Sometimes there are floating point errors that result in a non-zero first
    // value.
    numerator(0) = 0;
    numerator = RemoveLeadingZeros(numerator);
  }
  *remainder = numerator;
}

VectorXd AddPolynomials(const VectorXd& poly1, const VectorXd& poly2) {
  if (poly1.size() > poly2.size()) {
    VectorXd sum = poly1;
    sum.tail(poly2.size()) += poly2;
    return sum;
  } else {
    VectorXd sum = poly2;
    sum.tail(poly1.size()) += poly1;
    return sum;
  }
}

void FindLinearPolynomialRoots(const VectorXd& polynomial,
                               VectorXd* real,
                               VectorXd* imaginary) {
  CHECK_EQ(polynomial.size(), 2);
  if (real != NULL) {
    real->resize(1);
    (*real)(0) = -polynomial(1) / polynomial(0);
  }

  if (imaginary != NULL) {
    imaginary->setZero(1);
  }
}

void FindQuadraticPolynomialRoots(const VectorXd& polynomial,
                                  VectorXd* real,
                                  VectorXd* imaginary) {
  CHECK_EQ(polynomial.size(), 3);
  const double a = polynomial(0);
  const double b = polynomial(1);
  const double c = polynomial(2);
  const double D = b * b - 4 * a * c;
  const double sqrt_D = sqrt(fabs(D));
  if (real != NULL) {
    real->setZero(2);
  }
  if (imaginary != NULL) {
    imaginary->setZero(2);
  }

  // Real roots.
  if (D >= 0) {
    if (real != NULL) {
      // Stable quadratic roots according to BKP Horn.
      // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
      if (b >= 0) {
        (*real)(0) = (-b - sqrt_D) / (2.0 * a);
        (*real)(1) = (2.0 * c) / (-b - sqrt_D);
      } else {
        (*real)(0) = (2.0 * c) / (-b + sqrt_D);
        (*real)(1) = (-b + sqrt_D) / (2.0 * a);
      }
    }
    return;
  }

  // Use the normal quadratic formula for the complex case.
  if (real != NULL) {
    (*real)(0) = -b / (2.0 * a);
    (*real)(1) = -b / (2.0 * a);
  }
  if (imaginary != NULL) {
    (*imaginary)(0) = sqrt_D / (2.0 * a);
    (*imaginary)(1) = -sqrt_D / (2.0 * a);
  }
}

void MinimizePolynomial(const VectorXd& polynomial,
                        const double x_min,
                        const double x_max,
                        double* optimal_x,
                        double* optimal_value) {
  // Find the minimum of the polynomial at the two ends.
  //
  // We start by inspecting the middle of the interval. Technically
  // this is not needed, but we do this to make this code as close to
  // the minFunc package as possible.
  *optimal_x = (x_min + x_max) / 2.0;
  *optimal_value = EvaluatePolynomial(polynomial, *optimal_x);

  const double x_min_value = EvaluatePolynomial(polynomial, x_min);
  if (x_min_value < *optimal_value) {
    *optimal_value = x_min_value;
    *optimal_x = x_min;
  }

  const double x_max_value = EvaluatePolynomial(polynomial, x_max);
  if (x_max_value < *optimal_value) {
    *optimal_value = x_max_value;
    *optimal_x = x_max;
  }

  // If the polynomial is linear or constant, we are done.
  if (polynomial.rows() <= 2) {
    return;
  }

  const VectorXd derivative = DifferentiatePolynomial(polynomial);
  VectorXd roots_real;
  if (!FindPolynomialRoots(derivative, &roots_real, NULL)) {
    LOG(WARNING) << "Unable to find the critical points of "
                 << "the interpolating polynomial.";
    return;
  }

  // This is a bit of an overkill, as some of the roots may actually
  // have a complex part, but its simpler to just check these values.
  for (int i = 0; i < roots_real.rows(); ++i) {
    const double root = roots_real(i);
    if ((root < x_min) || (root > x_max)) {
      continue;
    }

    const double value = EvaluatePolynomial(polynomial, root);
    if (value < *optimal_value) {
      *optimal_value = value;
      *optimal_x = root;
    }
  }
}

// An iterative solver to find the closest root based on an initial guess. We
// use Laguerre's method, which is a polynomial root finding method that
// converges to a root with very high certainty. For multiple roots, the
// convergence is linear, otherwise it is cubic.
double FindRootIterativeLaguerre(const VectorXd& polynomial,
                                 const double x0,
                                 const double epsilon,
                                 const int max_iter) {
  const double kSmallestValue = 1e-10;

  // Constant symbolic derivitives.
  const VectorXd f_prime = DifferentiatePolynomial(polynomial);
  const VectorXd f_prime_prime = DifferentiatePolynomial(f_prime);
  const double k = static_cast<double>(polynomial.size());

  double x = x0;

  for (int i = 0; i < max_iter; i++) {
    const double f_of_x = EvaluatePolynomial(polynomial, x);
    if (std::abs(f_of_x) < kSmallestValue) {
      break;
    }

    const double g = EvaluatePolynomial(f_prime, x) / f_of_x;
    const double h = g * g - EvaluatePolynomial(f_prime_prime, x) / f_of_x;
    const double denom_part = std::sqrt(std::abs((k - 1.0) * (k * h - g * g)));
    const double denom = (g < 0) ? g - denom_part : g + denom_part;
    const double delta =  k / denom;
    if (std::abs(delta) < epsilon) {
      break;
    }

    x -= delta;
  }

  return x;
}

double FindRootIterativeNewton(const Eigen::VectorXd& polynomial,
                               const double x0,
                               const double epsilon,
                               const int max_iterations) {
  double root = x0;
  const Eigen::VectorXd derivative = DifferentiatePolynomial(polynomial);
  double prev = std::numeric_limits<double>::max();
  for (int i = 0; i < max_iterations && std::abs(prev - root) > epsilon; i++) {
    prev = root;
    root -= EvaluatePolynomial(polynomial, root) /
            EvaluatePolynomial(derivative, root);
  }
  return root;
}

}  // namespace theia
<commit_msg>Add proper reverse operator<commit_after>// Copyright (C) 2013 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)

// These functions are largely based off the the Ceres solver polynomial
// functions which are not available through the public interface. The license
// is below:
//
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// 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 Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: moll.markus@arcor.de (Markus Moll)
//         sameeragarwal@google.com (Sameer Agarwal)

#include "theia/math/polynomial.h"

#include <Eigen/Core>
#include <glog/logging.h>

#include <cmath>
#include <limits>

#include "theia/math/find_polynomial_roots_companion_matrix.h"

namespace theia {

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXcd;

bool FindPolynomialRoots(const VectorXd& polynomial,
                         VectorXd* real,
                         VectorXd* imaginary) {
  return FindPolynomialRootsCompanionMatrix(polynomial, real, imaginary);
}

// Remove leading terms with zero coefficients.
VectorXd RemoveLeadingZeros(const VectorXd& polynomial_in) {
  int i = 0;
  while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0) {
    ++i;
  }
  return polynomial_in.tail(polynomial_in.size() - i);
}

VectorXd DifferentiatePolynomial(const VectorXd& polynomial) {
  const int degree = polynomial.rows() - 1;
  CHECK_GE(degree, 0);

  // Degree zero polynomials are constants, and their derivative does
  // not result in a smaller degree polynomial, just a degree zero
  // polynomial with value zero.
  if (degree == 0) {
    return VectorXd::Zero(1);
  }

  VectorXd derivative(degree);
  for (int i = 0; i < degree; ++i) {
    derivative(i) = (degree - i) * polynomial(i);
  }

  return derivative;
}

VectorXd MultiplyPolynomials(const VectorXd& poly1, const VectorXd& poly2) {
  VectorXd multiplied_poly = VectorXd::Zero(poly1.size() + poly2.size() - 1);;
  for (int i = 0; i < poly1.size(); i++) {
    for (int j = 0; j < poly2.size(); j++) {
      multiplied_poly.reverse().operator()(i + j) +=
          poly1.reverse()(i) * poly2.reverse()(j);
    }
  }
  return multiplied_poly;
}

void DividePolynomial(const VectorXd& polynomial,
                      const VectorXd& divisor,
                      VectorXd* quotient,
                      VectorXd* remainder) {
  // If the divisor is higher degree than the polynomial then it cannot be
  // divided so we simply return the remainder.
  if (polynomial.size() < divisor.size()) {
    *quotient = VectorXd::Zero(1);
    *remainder = polynomial;
    return;
  }

  VectorXd numerator = RemoveLeadingZeros(polynomial);
  VectorXd denominator;
  *quotient = VectorXd::Zero(numerator.size() - divisor.size() + 1);
  while (numerator.size() >= divisor.size()) {
    denominator = VectorXd::Zero(numerator.size());
    denominator.head(divisor.size()) = divisor;

    const double quotient_scalar = numerator(0) / denominator(0);
    quotient->reverse().operator()(numerator.size() - divisor.size()) =
        quotient_scalar;
    denominator = denominator * quotient_scalar;
    numerator = numerator - denominator;
    // Sometimes there are floating point errors that result in a non-zero first
    // value.
    numerator(0) = 0;
    numerator = RemoveLeadingZeros(numerator);
  }
  *remainder = numerator;
}

VectorXd AddPolynomials(const VectorXd& poly1, const VectorXd& poly2) {
  if (poly1.size() > poly2.size()) {
    VectorXd sum = poly1;
    sum.tail(poly2.size()) += poly2;
    return sum;
  } else {
    VectorXd sum = poly2;
    sum.tail(poly1.size()) += poly1;
    return sum;
  }
}

void FindLinearPolynomialRoots(const VectorXd& polynomial,
                               VectorXd* real,
                               VectorXd* imaginary) {
  CHECK_EQ(polynomial.size(), 2);
  if (real != NULL) {
    real->resize(1);
    (*real)(0) = -polynomial(1) / polynomial(0);
  }

  if (imaginary != NULL) {
    imaginary->setZero(1);
  }
}

void FindQuadraticPolynomialRoots(const VectorXd& polynomial,
                                  VectorXd* real,
                                  VectorXd* imaginary) {
  CHECK_EQ(polynomial.size(), 3);
  const double a = polynomial(0);
  const double b = polynomial(1);
  const double c = polynomial(2);
  const double D = b * b - 4 * a * c;
  const double sqrt_D = sqrt(fabs(D));
  if (real != NULL) {
    real->setZero(2);
  }
  if (imaginary != NULL) {
    imaginary->setZero(2);
  }

  // Real roots.
  if (D >= 0) {
    if (real != NULL) {
      // Stable quadratic roots according to BKP Horn.
      // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
      if (b >= 0) {
        (*real)(0) = (-b - sqrt_D) / (2.0 * a);
        (*real)(1) = (2.0 * c) / (-b - sqrt_D);
      } else {
        (*real)(0) = (2.0 * c) / (-b + sqrt_D);
        (*real)(1) = (-b + sqrt_D) / (2.0 * a);
      }
    }
    return;
  }

  // Use the normal quadratic formula for the complex case.
  if (real != NULL) {
    (*real)(0) = -b / (2.0 * a);
    (*real)(1) = -b / (2.0 * a);
  }
  if (imaginary != NULL) {
    (*imaginary)(0) = sqrt_D / (2.0 * a);
    (*imaginary)(1) = -sqrt_D / (2.0 * a);
  }
}

void MinimizePolynomial(const VectorXd& polynomial,
                        const double x_min,
                        const double x_max,
                        double* optimal_x,
                        double* optimal_value) {
  // Find the minimum of the polynomial at the two ends.
  //
  // We start by inspecting the middle of the interval. Technically
  // this is not needed, but we do this to make this code as close to
  // the minFunc package as possible.
  *optimal_x = (x_min + x_max) / 2.0;
  *optimal_value = EvaluatePolynomial(polynomial, *optimal_x);

  const double x_min_value = EvaluatePolynomial(polynomial, x_min);
  if (x_min_value < *optimal_value) {
    *optimal_value = x_min_value;
    *optimal_x = x_min;
  }

  const double x_max_value = EvaluatePolynomial(polynomial, x_max);
  if (x_max_value < *optimal_value) {
    *optimal_value = x_max_value;
    *optimal_x = x_max;
  }

  // If the polynomial is linear or constant, we are done.
  if (polynomial.rows() <= 2) {
    return;
  }

  const VectorXd derivative = DifferentiatePolynomial(polynomial);
  VectorXd roots_real;
  if (!FindPolynomialRoots(derivative, &roots_real, NULL)) {
    LOG(WARNING) << "Unable to find the critical points of "
                 << "the interpolating polynomial.";
    return;
  }

  // This is a bit of an overkill, as some of the roots may actually
  // have a complex part, but its simpler to just check these values.
  for (int i = 0; i < roots_real.rows(); ++i) {
    const double root = roots_real(i);
    if ((root < x_min) || (root > x_max)) {
      continue;
    }

    const double value = EvaluatePolynomial(polynomial, root);
    if (value < *optimal_value) {
      *optimal_value = value;
      *optimal_x = root;
    }
  }
}

// An iterative solver to find the closest root based on an initial guess. We
// use Laguerre's method, which is a polynomial root finding method that
// converges to a root with very high certainty. For multiple roots, the
// convergence is linear, otherwise it is cubic.
double FindRootIterativeLaguerre(const VectorXd& polynomial,
                                 const double x0,
                                 const double epsilon,
                                 const int max_iter) {
  const double kSmallestValue = 1e-10;

  // Constant symbolic derivitives.
  const VectorXd f_prime = DifferentiatePolynomial(polynomial);
  const VectorXd f_prime_prime = DifferentiatePolynomial(f_prime);
  const double k = static_cast<double>(polynomial.size());

  double x = x0;

  for (int i = 0; i < max_iter; i++) {
    const double f_of_x = EvaluatePolynomial(polynomial, x);
    if (std::abs(f_of_x) < kSmallestValue) {
      break;
    }

    const double g = EvaluatePolynomial(f_prime, x) / f_of_x;
    const double h = g * g - EvaluatePolynomial(f_prime_prime, x) / f_of_x;
    const double denom_part = std::sqrt(std::abs((k - 1.0) * (k * h - g * g)));
    const double denom = (g < 0) ? g - denom_part : g + denom_part;
    const double delta =  k / denom;
    if (std::abs(delta) < epsilon) {
      break;
    }

    x -= delta;
  }

  return x;
}

double FindRootIterativeNewton(const Eigen::VectorXd& polynomial,
                               const double x0,
                               const double epsilon,
                               const int max_iterations) {
  double root = x0;
  const Eigen::VectorXd derivative = DifferentiatePolynomial(polynomial);
  double prev = std::numeric_limits<double>::max();
  for (int i = 0; i < max_iterations && std::abs(prev - root) > epsilon; i++) {
    prev = root;
    root -= EvaluatePolynomial(polynomial, root) /
            EvaluatePolynomial(derivative, root);
  }
  return root;
}

}  // namespace theia
<|endoftext|>


<commit_before>#include "task_manager_mpi.hpp"

namespace TaskDistribution {
  MPITaskManager::MPITaskManager(boost::mpi::communicator& world,
      MPIHandler& handler, MPIObjectArchive<Key>& archive,
      MPIComputingUnitManager& unit_manager):
    MPITaskManager(Tags(), world, handler, archive, unit_manager) { }

  MPITaskManager::MPITaskManager(Tags const& tags,
      boost::mpi::communicator& world, MPIHandler& handler,
      MPIObjectArchive<Key>& archive, MPIComputingUnitManager& unit_manager):
    TaskManager(archive, unit_manager),
    tags_(tags),
    world_(world),
    handler_(handler),
    archive_(archive),
    unit_manager_(unit_manager),
    finished_(false),
    tasks_per_node_(world_.size()-1, 0) {
      handler.insert(tags_.finish,
          std::bind(&MPITaskManager::process_finish, this,
            std::placeholders::_1, tags.finish));

      handler.insert(tags_.key_update,
          std::bind(&MPITaskManager::process_key_update, this,
            std::placeholders::_1, tags.key_update));

      clear_task_creation_handler();
      clear_task_begin_handler();
      clear_task_end_handler();

      archive_.set_insert_filter(
        [](Key const& key, boost::mpi::communicator& world)
        { return key.is_valid() && world.rank() == 0; });
    }

  MPITaskManager::~MPITaskManager() { }

  void MPITaskManager::run() {
    if (world_.size() > 1) {
      if (world_.rank() == 0)
        run_master();
      else
        run_slave();
    } else
      run_single();
  }

  void MPITaskManager::run_master() {
    size_t n_running = 0;

    // Process whatever is left for MPI first
    handler_.run();

    n_running += allocate_tasks();

    while (!ready_.empty() || n_running != 0) {
      // Process MPI stuff until a task has ended
      unit_manager_.clear_tasks_ended();

      do {
        handler_.run();
      } while (unit_manager_.get_tasks_ended().empty());

      MPIComputingUnitManager::TasksList const& finished_tasks =
        unit_manager_.get_tasks_ended();

      for (auto& it : finished_tasks) {
        task_completed(it.first);
        int slave = it.second;
        --tasks_per_node_[slave-1];
        --n_running;
      }

      n_running += allocate_tasks();
    }

    broadcast_finish();
  }

  size_t MPITaskManager::allocate_tasks() {
    size_t n_running = 0;

    bool allocated_a_task = true;

    while (!ready_.empty() && allocated_a_task) {
      allocated_a_task = false;

      for (int i = 1; i < world_.size(); i++) {
        // Maximum fixed allocation. TODO: not fixed
        if (tasks_per_node_[i-1] < 1) {

          // Tries to send task to slave. Fails if ready_.empty() after running
          // local tasks.
          if (!send_next_task(i))
            break;

          allocated_a_task = true;
          tasks_per_node_[i-1]++;
          n_running++;
        }
      }
    }

    return n_running;
  }

  void MPITaskManager::run_slave() {
    while (!finished_)
      unit_manager_.process_remote();
  }

  bool MPITaskManager::send_next_task(int slave) {
    bool got_task_for_remote = false;
    Key task_key;
    TaskEntry entry;

    while (!got_task_for_remote) {
      if (ready_.empty())
        return false;

      task_key = ready_.front();
      ready_.pop_front();

      archive_.load(task_key, entry);

      // If we already computed this task, gets the next one
      if (!entry.result_key.is_valid()) {
        if (entry.run_locally) {
          task_begin_handler_(task_key);
          unit_manager_.process_local(entry);
          task_completed(task_key);
        }
        else
          got_task_for_remote = true;
      }
    }

    task_begin_handler_(task_key);
    unit_manager_.send_remote(entry, slave);
    return true;
  }

  bool MPITaskManager::process_finish(int source, int tag) {
    world_.recv(source, tag, finished_);

    return true;
  }

  bool MPITaskManager::process_key_update(int source, int tag) {
    size_t key;
    world_.recv(source, tag, key);
    Key::next_obj = std::max(Key::next_obj, key);

    return true;
  }

  void MPITaskManager::broadcast_finish() {
    for (int i = 1; i < world_.size(); i++)
      world_.send(i, tags_.finish, true);
  }

  size_t MPITaskManager::id() const {
    return world_.rank();
  }

  void MPITaskManager::update_used_keys(
      std::map<int, size_t> const& used_keys) {
    if (world_.size() == 1)
      TaskManager::update_used_keys(used_keys);
    else {
      for (auto it = used_keys.begin(); it != used_keys.end(); ++it) {
        if (it->first == world_.rank())
          Key::next_obj = std::max(Key::next_obj, it->second + 1);
        else
          world_.send(it->first, tags_.key_update, it->second + 1);
      }
    }
  }

  Key MPITaskManager::new_key(Key::Type type) {
    return Key::new_key(world_, type);
  }
};
<commit_msg>Added documentation to task_manager_mpi.cpp.<commit_after>#include "task_manager_mpi.hpp"

namespace TaskDistribution {
  MPITaskManager::MPITaskManager(boost::mpi::communicator& world,
      MPIHandler& handler, MPIObjectArchive<Key>& archive,
      MPIComputingUnitManager& unit_manager):
    MPITaskManager(Tags(), world, handler, archive, unit_manager) { }

  MPITaskManager::MPITaskManager(Tags const& tags,
      boost::mpi::communicator& world, MPIHandler& handler,
      MPIObjectArchive<Key>& archive, MPIComputingUnitManager& unit_manager):
    TaskManager(archive, unit_manager),
    tags_(tags),
    world_(world),
    handler_(handler),
    archive_(archive),
    unit_manager_(unit_manager),
    finished_(false),
    tasks_per_node_(world_.size()-1, 0) {
      // Set-up handlers
      handler.insert(tags_.finish,
          std::bind(&MPITaskManager::process_finish, this,
            std::placeholders::_1, tags.finish));

      handler.insert(tags_.key_update,
          std::bind(&MPITaskManager::process_key_update, this,
            std::placeholders::_1, tags.key_update));

      clear_task_creation_handler();
      clear_task_begin_handler();
      clear_task_end_handler();

      // Only stores things with valid keys and into the master
      archive_.set_insert_filter(
        [](Key const& key, boost::mpi::communicator& world)
        { return key.is_valid() && world.rank() == 0; });
    }

  MPITaskManager::~MPITaskManager() { }

  void MPITaskManager::run() {
    if (world_.size() > 1) {
      if (world_.rank() == 0)
        run_master();
      else
        run_slave();
    } else
      run_single();
  }

  void MPITaskManager::run_master() {
    size_t n_running = 0;

    // Process whatever is left for MPI first
    handler_.run();

    n_running += allocate_tasks();

    while (!ready_.empty() || n_running != 0) {
      // Process MPI stuff until a task has ended
      unit_manager_.clear_tasks_ended();

      do {
        handler_.run();
      } while (unit_manager_.get_tasks_ended().empty());

      MPIComputingUnitManager::TasksList const& finished_tasks =
        unit_manager_.get_tasks_ended();

      for (auto& it : finished_tasks) {
        task_completed(it.first);
        int slave = it.second;
        --tasks_per_node_[slave-1];
        --n_running;
      }

      n_running += allocate_tasks();
    }

    broadcast_finish();
  }

  size_t MPITaskManager::allocate_tasks() {
    size_t n_running = 0;

    bool allocated_a_task = true;

    while (!ready_.empty() && allocated_a_task) {
      allocated_a_task = false;

      for (int i = 1; i < world_.size(); i++) {
        // Maximum fixed allocation. TODO: not fixed
        if (tasks_per_node_[i-1] < 1) {

          // Tries to send task to slave. Fails if ready_.empty() after running
          // local tasks.
          if (!send_next_task(i))
            break;

          allocated_a_task = true;
          tasks_per_node_[i-1]++;
          n_running++;
        }
      }
    }

    return n_running;
  }

  void MPITaskManager::run_slave() {
    while (!finished_)
      unit_manager_.process_remote();
  }

  bool MPITaskManager::send_next_task(int slave) {
    bool got_task_for_remote = false;
    Key task_key;
    TaskEntry entry;

    while (!got_task_for_remote) {
      if (ready_.empty())
        return false;

      task_key = ready_.front();
      ready_.pop_front();

      archive_.load(task_key, entry);

      // If we already computed this task, gets the next one
      if (!entry.result_key.is_valid()) {
        if (entry.run_locally) {
          task_begin_handler_(task_key);
          unit_manager_.process_local(entry);
          task_completed(task_key);
        }
        else
          got_task_for_remote = true;
      }
    }

    task_begin_handler_(task_key);
    unit_manager_.send_remote(entry, slave);
    return true;
  }

  bool MPITaskManager::process_finish(int source, int tag) {
    world_.recv(source, tag, finished_);

    return true;
  }

  bool MPITaskManager::process_key_update(int source, int tag) {
    size_t key;
    world_.recv(source, tag, key);
    Key::next_obj = std::max(Key::next_obj, key);

    return true;
  }

  void MPITaskManager::broadcast_finish() {
    for (int i = 1; i < world_.size(); i++)
      world_.send(i, tags_.finish, true);
  }

  size_t MPITaskManager::id() const {
    return world_.rank();
  }

  void MPITaskManager::update_used_keys(
      std::map<int, size_t> const& used_keys) {
    if (world_.size() == 1)
      TaskManager::update_used_keys(used_keys);
    else {
      for (auto it = used_keys.begin(); it != used_keys.end(); ++it) {
        if (it->first == world_.rank())
          Key::next_obj = std::max(Key::next_obj, it->second + 1);
        else
          world_.send(it->first, tags_.key_update, it->second + 1);
      }
    }
  }

  Key MPITaskManager::new_key(Key::Type type) {
    return Key::new_key(world_, type);
  }
};
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/common/trajectory/trajectory_stitcher.h"

#include <algorithm>
#include <list>
#include <utility>

#include "modules/common/configs/config_gflags.h"
#include "modules/common/log.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::math::Vec2d;
using apollo::common::util::DistanceXY;

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const VehicleState& vehicle_state) {
  TrajectoryPoint init_point;
  init_point.mutable_path_point()->set_x(vehicle_state.x());
  init_point.mutable_path_point()->set_y(vehicle_state.y());
  init_point.mutable_path_point()->set_z(vehicle_state.z());
  init_point.mutable_path_point()->set_theta(vehicle_state.heading());
  init_point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  init_point.set_v(vehicle_state.linear_velocity());
  init_point.set_a(vehicle_state.linear_acceleration());
  init_point.set_relative_time(0.0);

  return std::vector<TrajectoryPoint>(1, init_point);
}

// only used in navigation mode
void TrajectoryStitcher::TransformLastPublishedTrajectory(const double x_diff,
    const double y_diff, const double theta_diff,
    PublishableTrajectory* prev_trajectory) {

  if (!prev_trajectory) {
    return;
  }

  // R^-1
  auto cos_theta = std::cos(theta_diff);
  auto sin_theta = std::sin(-theta_diff);

  // -R^-1 * t
  auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
  auto ty = -(sin_theta * x_diff + cos_theta * y_diff);

  auto trajectory_points = prev_trajectory->trajectory_points();
  std::for_each(trajectory_points.begin(), trajectory_points.end(),
      [&cos_theta, &sin_theta, &tx, &ty, &theta_diff]
       (common::TrajectoryPoint& p) {
        auto x = p.path_point().x();
        auto y = p.path_point().y();
        auto theta = p.path_point().theta();

        auto x_new = cos_theta * x - sin_theta * y + tx;
        auto y_new = sin_theta * x + cos_theta * y + ty;
        auto theta_new = common::math::WrapAngle(theta + theta_diff);

        p.mutable_path_point()->set_x(x_new);
        p.mutable_path_point()->set_y(y_new);
        p.mutable_path_point()->set_theta(theta_new);
      });
}

// Planning from current vehicle state:
// if 1. the auto-driving mode is off or
//    2. we don't have the trajectory from last planning cycle or
//    3. the position deviation from actual and target is too high
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time,
    const PublishableTrajectory* prev_trajectory, bool* is_replan) {
  *is_replan = true;
  if (!FLAGS_enable_trajectory_stitcher) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (!prev_trajectory) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  std::size_t matched_index = prev_trajectory->QueryNearestPoint(veh_rel_time);

  if (matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "current time smaller than the previous trajectory's first time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (matched_index + 1 >= prev_trajectory_size) {
    AWARN << "current time beyond the previous trajectory's last time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto matched_point = prev_trajectory->Evaluate(veh_rel_time);

  if (!matched_point.has_path_point()) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(), *prev_trajectory);

  auto lon_diff = std::fabs(matched_point.path_point().s() - frenet_sd.first);
  auto lat_diff = std::fabs(frenet_sd.second);

  ADEBUG << "Control lateral diff: " << lat_diff
         << ", longitudinal diff: " << lon_diff;

  if (lat_diff > FLAGS_replan_lateral_distance_threshold ||
      lon_diff > FLAGS_replan_longitudinal_distance_threshold) {
    AERROR << "the distance between matched point and actual position is too "
              "large. Replan is triggered. lat_diff = "
           << lat_diff << ", lon_diff = " << lon_diff;
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  double forward_rel_time =
      prev_trajectory->TrajectoryPointAt(matched_index).relative_time() +
      planning_cycle_time;

  std::size_t forward_index =
      prev_trajectory->QueryNearestPoint(forward_rel_time);

  ADEBUG << "matched_index: " << matched_index;
  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->trajectory_points().begin() +
          std::max(0, static_cast<int>(matched_index - 1)),
      prev_trajectory->trajectory_points().begin() + forward_index + 1);

  const double zero_s = matched_point.path_point().s();

  for (auto& tp : stitching_trajectory) {
    if (!tp.has_path_point()) {
      return ComputeReinitStitchingTrajectory(vehicle_state);
    }
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  *is_replan = false;
  return stitching_trajectory;
}

std::pair<double, double> TrajectoryStitcher::ComputePositionProjection(
    const double x, const double y,
    const PublishableTrajectory& prev_trajectory) {
  auto index = prev_trajectory.QueryNearestPoint({x, y});
  auto p = prev_trajectory.TrajectoryPointAt(index);

  common::math::Vec2d v(x - p.path_point().x(), y - p.path_point().y());
  common::math::Vec2d n(std::cos(p.path_point().theta()),
      std::sin(p.path_point().theta()));

  std::pair<double, double> frenet_sd;
  frenet_sd.first = v.InnerProd(n) + p.path_point().s();
  frenet_sd.second = v.CrossProd(n);
  return frenet_sd;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: bug fix for rotation correction<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/common/trajectory/trajectory_stitcher.h"

#include <algorithm>
#include <list>
#include <utility>

#include "modules/common/configs/config_gflags.h"
#include "modules/common/log.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::math::Vec2d;
using apollo::common::util::DistanceXY;

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const VehicleState& vehicle_state) {
  TrajectoryPoint init_point;
  init_point.mutable_path_point()->set_x(vehicle_state.x());
  init_point.mutable_path_point()->set_y(vehicle_state.y());
  init_point.mutable_path_point()->set_z(vehicle_state.z());
  init_point.mutable_path_point()->set_theta(vehicle_state.heading());
  init_point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  init_point.set_v(vehicle_state.linear_velocity());
  init_point.set_a(vehicle_state.linear_acceleration());
  init_point.set_relative_time(0.0);

  return std::vector<TrajectoryPoint>(1, init_point);
}

// only used in navigation mode
void TrajectoryStitcher::TransformLastPublishedTrajectory(const double x_diff,
    const double y_diff, const double theta_diff,
    PublishableTrajectory* prev_trajectory) {

  if (!prev_trajectory) {
    return;
  }

  // R^-1
  auto cos_theta = std::cos(theta_diff);
  auto sin_theta = std::sin(-theta_diff);

  // -R^-1 * t
  auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
  auto ty = -(sin_theta * x_diff + cos_theta * y_diff);

  auto trajectory_points = prev_trajectory->trajectory_points();
  std::for_each(trajectory_points.begin(), trajectory_points.end(),
      [&cos_theta, &sin_theta, &tx, &ty, &theta_diff]
       (common::TrajectoryPoint& p) {
        auto x = p.path_point().x();
        auto y = p.path_point().y();
        auto theta = p.path_point().theta();

        auto x_new = cos_theta * x - sin_theta * y + tx;
        auto y_new = sin_theta * x + cos_theta * y + ty;
        auto theta_new = common::math::WrapAngle(theta - theta_diff);

        p.mutable_path_point()->set_x(x_new);
        p.mutable_path_point()->set_y(y_new);
        p.mutable_path_point()->set_theta(theta_new);
      });
}

// Planning from current vehicle state:
// if 1. the auto-driving mode is off or
//    2. we don't have the trajectory from last planning cycle or
//    3. the position deviation from actual and target is too high
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time,
    const PublishableTrajectory* prev_trajectory, bool* is_replan) {
  *is_replan = true;
  if (!FLAGS_enable_trajectory_stitcher) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (!prev_trajectory) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  std::size_t matched_index = prev_trajectory->QueryNearestPoint(veh_rel_time);

  if (matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "current time smaller than the previous trajectory's first time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (matched_index + 1 >= prev_trajectory_size) {
    AWARN << "current time beyond the previous trajectory's last time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto matched_point = prev_trajectory->Evaluate(veh_rel_time);

  if (!matched_point.has_path_point()) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(), *prev_trajectory);

  auto lon_diff = std::fabs(matched_point.path_point().s() - frenet_sd.first);
  auto lat_diff = std::fabs(frenet_sd.second);

  ADEBUG << "Control lateral diff: " << lat_diff
         << ", longitudinal diff: " << lon_diff;

  if (lat_diff > FLAGS_replan_lateral_distance_threshold ||
      lon_diff > FLAGS_replan_longitudinal_distance_threshold) {
    AERROR << "the distance between matched point and actual position is too "
              "large. Replan is triggered. lat_diff = "
           << lat_diff << ", lon_diff = " << lon_diff;
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  double forward_rel_time =
      prev_trajectory->TrajectoryPointAt(matched_index).relative_time() +
      planning_cycle_time;

  std::size_t forward_index =
      prev_trajectory->QueryNearestPoint(forward_rel_time);

  ADEBUG << "matched_index: " << matched_index;
  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->trajectory_points().begin() +
          std::max(0, static_cast<int>(matched_index - 1)),
      prev_trajectory->trajectory_points().begin() + forward_index + 1);

  const double zero_s = matched_point.path_point().s();

  for (auto& tp : stitching_trajectory) {
    if (!tp.has_path_point()) {
      return ComputeReinitStitchingTrajectory(vehicle_state);
    }
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  *is_replan = false;
  return stitching_trajectory;
}

std::pair<double, double> TrajectoryStitcher::ComputePositionProjection(
    const double x, const double y,
    const PublishableTrajectory& prev_trajectory) {
  auto index = prev_trajectory.QueryNearestPoint({x, y});
  auto p = prev_trajectory.TrajectoryPointAt(index);

  common::math::Vec2d v(x - p.path_point().x(), y - p.path_point().y());
  common::math::Vec2d n(std::cos(p.path_point().theta()),
      std::sin(p.path_point().theta()));

  std::pair<double, double> frenet_sd;
  frenet_sd.first = v.InnerProd(n) + p.path_point().s();
  frenet_sd.second = v.CrossProd(n);
  return frenet_sd;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998-2005 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 * -----------------------------------------------------------------
 * File:          $RCSfile$
 * Date modified: $Date$
 * Version:       $Revision$
 * -----------------------------------------------------------------
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <vrj/vrjConfig.h>

#include <boost/filesystem/path.hpp>

#include <vpr/vpr.h>
#include <vpr/System.h>
#include <vpr/DynLoad/LibraryLoader.h>
#include <vpr/IO/Socket/InetAddr.h>

#include <jccl/Config/ConfigElement.h>
#include <jccl/Config/ElementFactory.h>
#include <jccl/RTRC/ConfigManager.h>
#include <jccl/RTRC/DependencyManager.h>
#include <jccl/RTRC/ConfigElementHandler.h>
#include <jccl/Util/Debug.h>

#include <vrj/Performance/PerformanceMediator.h>
#include <vrj/Performance/PerfPlugin.h>

namespace fs = boost::filesystem;

namespace vrj
{

   PerformanceMediator::PerformanceMediator()
   : mPerfIf(NULL)
   {
      loadPerfPlugin();
   }

   PerformanceMediator::~PerformanceMediator()
   {
      if ( NULL != mPerfIf && mPerfIf->isEnabled() )
      {
         mPerfIf->disable();

         delete mPerfIf;
         mPerfIf = NULL;
      }
   }

/**
 * This struct implements a callable object (a functor, basically).  An
 * instance can be passed in where a boost::function1<bool, void*> is expected.
 * In vrj:PerformanceMediator::loadPerfPlugin(), instances are used to handle
 * dynamic loading of plug-ins via vpr::LibraryLoader.
 */
   struct Callable
   {
      Callable(vrj::PerformanceMediator* mgr) : med(mgr)
      {
      }

      /**
       * This will be invoked as a callback by methods of vpr::LibraryLoader.
       *
       * @param func A function pointer for the entry point in a dynamically
       *             loaded plug-in.  This must be cast to the correct signature
       *             before being invoked.
       */
      bool operator()(void* func)
      {
         vrj::PerfPlugin* (*init_func)(vrj::PerformanceMediator* mediator, jccl::ConfigManager* configMgr);

         // Cast the entry point function to the correct signature so that we can
         // call it.  All dynamically plug-ins must have an entry point function
         // that takes no argument and returns a pointer to an implementation of
         // the vrj::RemoteReconfig interface.
         init_func = (vrj::PerfPlugin* (*)(vrj::PerformanceMediator*, jccl::ConfigManager*)) func;

         jccl::ConfigManager* mgr = jccl::ConfigManager::instance();

         // Call the entry point function.
         vrj::PerfPlugin* plugin = (*init_func)(med, mgr);

         if ( NULL != plugin )
         {
            med->setPerfPlugin(plugin);
            return true;
         }
         else
         {
            return false;
         }
      }

      vrj::PerformanceMediator* med;
   };

   void PerformanceMediator::loadPerfPlugin()
   {
      vprASSERT(NULL == mPerfIf && "PerformanceMediator interface object already instantiated.");

      const std::string vj_base_dir("VJ_BASE_DIR");
      std::string base_dir;

      if ( ! vpr::System::getenv(vj_base_dir, base_dir).success() )
      {
         return;
      }

      const std::string no_perf_plugin("NO_PERF_PLUGIN");
      std::string junk;

      // If the user has the environment variable NO_PERF_PLUGIN set (to any
      // value), do not attempt to load the plug-in.
      if ( vpr::System::getenv(no_perf_plugin, junk).success() )
      {
         vprDEBUG(vprDBG_ALL, vprDBG_STATE_LVL)
            << "Remote performance visualization plug-in loading disabled "
            << "via NO_PERF_PLUGIN." << std::endl << vprDEBUG_FLUSH;
         return;
      }

#if defined(VPR_OS_IRIX) && defined(_ABIN32)
      const std::string bit_suffix("32");
#elif defined(VPR_OS_IRIX) && defined(_ABI64) || \
      defined(VPR_OS_Linux) && defined(__x86_64__)
      const std::string bit_suffix("64");
#else
      const std::string bit_suffix("");
#endif

      std::vector<fs::path> search_path(1);
      search_path[0] = fs::path(base_dir, fs::native) /
                       (std::string("lib") + bit_suffix) /
                       std::string("vrjuggler") / std::string("plugins");

      // In the long run, we may not want to hard-code the base name of the
      // plug-in we load.  If we ever reach a point where we have multiple ways
      // of implementing remote performance monitoring, we could have options
      // for which plug-in to load.
      const std::string perf_mon_dso("corba_perf_mon");
      const std::string init_func("initPlugin");
      Callable functor(this);
      vpr::LibraryLoader::findDSOAndCallEntryPoint(perf_mon_dso, search_path,
                                                   init_func, functor,
                                                   mPluginDSO);
   }

   void PerformanceMediator::setPerfPlugin(vrj::PerfPlugin* plugin)
   {
      // If we already have a remote performance monitoring plug-in, discard it
      // first.
      if ( NULL != mPerfIf )
      {
         vprDEBUG(jcclDBG_RECONFIG, vprDBG_STATE_LVL)
         << "[PerformanceMediator::setPerfPlugin()] "
            << "Removing old remote performance monitoring plug-in\n"
            << vprDEBUG_FLUSH;

         if ( mPerfIf->isEnabled() )
         {
            mPerfIf->disable();
         }

         delete mPerfIf;
      }

      vprDEBUG(jcclDBG_RECONFIG, vprDBG_VERB_LVL)
      << "[PerformanceMediator::setPerfPlugin()] "
         << "Enabling new remote performance monitoring plug-in\n"
         << vprDEBUG_FLUSH;
      mPerfIf = plugin;

      if ( NULL != mPerfIf )
      {
         // Attempt to initialize the remote performance monitoring component.
         if ( mPerfIf->init().success() )
         {
            // Now, attempt to enable remote performance monitoring hooks.
            if ( ! mPerfIf->enable().success() )
            {
               vprDEBUG(jcclDBG_RECONFIG, vprDBG_WARNING_LVL)
               << clrOutBOLD(clrYELLOW, "WARNING:")
               << " Failed to enable remote performance monitoring hooks.\n"
                  << vprDEBUG_FLUSH;
               delete mPerfIf;
               mPerfIf = NULL;
            }
         }
         // Initialization failed.
         else
         {
            vprDEBUG(jcclDBG_RECONFIG, vprDBG_WARNING_LVL)
            << clrOutBOLD(clrYELLOW, "WARNING:")
            << " Failed to initialize remote performance monitoring hooks.\n"
               << vprDEBUG_FLUSH;
            delete mPerfIf;
            mPerfIf = NULL;
         }
      }
   }

} // namespace vrj
<commit_msg>Added some simple error handling to loadPerfPlugin(). This is similar to what was added in Revision 1.7.2.2, but since an exception may be thrown by vpr::LibraryLoader::findDSOAndCallEntryPoint() on this branch, it is much more critical that we handle the exceptional case. This shold fix a problem that Aron ran into recently that prevented application statup if the remote performance monitoring plug-in failed to load.<commit_after>/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998-2005 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 * -----------------------------------------------------------------
 * File:          $RCSfile$
 * Date modified: $Date$
 * Version:       $Revision$
 * -----------------------------------------------------------------
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <vrj/vrjConfig.h>

#include <boost/filesystem/path.hpp>

#include <vpr/vpr.h>
#include <vpr/System.h>
#include <vpr/DynLoad/LibraryLoader.h>
#include <vpr/IO/Socket/InetAddr.h>

#include <jccl/Config/ConfigElement.h>
#include <jccl/Config/ElementFactory.h>
#include <jccl/RTRC/ConfigManager.h>
#include <jccl/RTRC/DependencyManager.h>
#include <jccl/RTRC/ConfigElementHandler.h>
#include <jccl/Util/Debug.h>

#include <vrj/Performance/PerformanceMediator.h>
#include <vrj/Performance/PerfPlugin.h>

namespace fs = boost::filesystem;

namespace vrj
{

   PerformanceMediator::PerformanceMediator()
   : mPerfIf(NULL)
   {
      loadPerfPlugin();
   }

   PerformanceMediator::~PerformanceMediator()
   {
      if ( NULL != mPerfIf && mPerfIf->isEnabled() )
      {
         mPerfIf->disable();

         delete mPerfIf;
         mPerfIf = NULL;
      }
   }

/**
 * This struct implements a callable object (a functor, basically).  An
 * instance can be passed in where a boost::function1<bool, void*> is expected.
 * In vrj:PerformanceMediator::loadPerfPlugin(), instances are used to handle
 * dynamic loading of plug-ins via vpr::LibraryLoader.
 */
   struct Callable
   {
      Callable(vrj::PerformanceMediator* mgr) : med(mgr)
      {
      }

      /**
       * This will be invoked as a callback by methods of vpr::LibraryLoader.
       *
       * @param func A function pointer for the entry point in a dynamically
       *             loaded plug-in.  This must be cast to the correct signature
       *             before being invoked.
       */
      bool operator()(void* func)
      {
         vrj::PerfPlugin* (*init_func)(vrj::PerformanceMediator* mediator, jccl::ConfigManager* configMgr);

         // Cast the entry point function to the correct signature so that we can
         // call it.  All dynamically plug-ins must have an entry point function
         // that takes no argument and returns a pointer to an implementation of
         // the vrj::RemoteReconfig interface.
         init_func = (vrj::PerfPlugin* (*)(vrj::PerformanceMediator*, jccl::ConfigManager*)) func;

         jccl::ConfigManager* mgr = jccl::ConfigManager::instance();

         // Call the entry point function.
         vrj::PerfPlugin* plugin = (*init_func)(med, mgr);

         if ( NULL != plugin )
         {
            med->setPerfPlugin(plugin);
            return true;
         }
         else
         {
            return false;
         }
      }

      vrj::PerformanceMediator* med;
   };

   void PerformanceMediator::loadPerfPlugin()
   {
      vprASSERT(NULL == mPerfIf && "PerformanceMediator interface object already instantiated.");

      const std::string vj_base_dir("VJ_BASE_DIR");
      std::string base_dir;

      if ( ! vpr::System::getenv(vj_base_dir, base_dir).success() )
      {
         return;
      }

      const std::string no_perf_plugin("NO_PERF_PLUGIN");
      std::string junk;

      // If the user has the environment variable NO_PERF_PLUGIN set (to any
      // value), do not attempt to load the plug-in.
      if ( vpr::System::getenv(no_perf_plugin, junk).success() )
      {
         vprDEBUG(vprDBG_ALL, vprDBG_STATE_LVL)
            << "Remote performance visualization plug-in loading disabled "
            << "via NO_PERF_PLUGIN." << std::endl << vprDEBUG_FLUSH;
         return;
      }

#if defined(VPR_OS_IRIX) && defined(_ABIN32)
      const std::string bit_suffix("32");
#elif defined(VPR_OS_IRIX) && defined(_ABI64) || \
      defined(VPR_OS_Linux) && defined(__x86_64__)
      const std::string bit_suffix("64");
#else
      const std::string bit_suffix("");
#endif

      std::vector<fs::path> search_path(1);
      search_path[0] = fs::path(base_dir, fs::native) /
                       (std::string("lib") + bit_suffix) /
                       std::string("vrjuggler") / std::string("plugins");

      try
      {
         // In the long run, we may not want to hard-code the base name of the
         // plug-in we load.  If we ever reach a point where we have multiple
         // ways of implementing remote performance monitoring, we could have
         // options for which plug-in to load.
         const std::string perf_mon_dso("corba_perf_mon");
         const std::string init_func("initPlugin");
         Callable functor(this);
         vpr::LibraryLoader::findDSOAndCallEntryPoint(perf_mon_dso,
                                                      search_path, init_func,
                                                      functor, mPluginDSO);
      }
      catch (vpr::Exception&)
      {
         vprDEBUG(vprDBG_ALL, vprDBG_WARNING_LVL)
            << "Failed to load the remote performance monitoring plug-in."
            << std::endl << vprDEBUG_FLUSH;
         vprDEBUG_NEXT(vprDBG_ALL, vprDBG_WARNING_LVL)
            << "Remote performance monitoring is disabled." << std::endl
            << vprDEBUG_FLUSH;
         vprDEBUG_NEXT(vprDBG_ALL, vprDBG_WARNING_LVL)
            << "(This is not a fatal error.)" << std::endl << vprDEBUG_FLUSH;

         // The plug-in is not usable, so we can unload it.
         if ( mPluginDSO.get() != NULL )
         {
            if ( mPluginDSO->isLoaded() )
            {
               mPluginDSO->unload();
            }

            mPluginDSO.reset();
         }
      }
   }

   void PerformanceMediator::setPerfPlugin(vrj::PerfPlugin* plugin)
   {
      // If we already have a remote performance monitoring plug-in, discard it
      // first.
      if ( NULL != mPerfIf )
      {
         vprDEBUG(jcclDBG_RECONFIG, vprDBG_STATE_LVL)
         << "[PerformanceMediator::setPerfPlugin()] "
            << "Removing old remote performance monitoring plug-in\n"
            << vprDEBUG_FLUSH;

         if ( mPerfIf->isEnabled() )
         {
            mPerfIf->disable();
         }

         delete mPerfIf;
      }

      vprDEBUG(jcclDBG_RECONFIG, vprDBG_VERB_LVL)
      << "[PerformanceMediator::setPerfPlugin()] "
         << "Enabling new remote performance monitoring plug-in\n"
         << vprDEBUG_FLUSH;
      mPerfIf = plugin;

      if ( NULL != mPerfIf )
      {
         // Attempt to initialize the remote performance monitoring component.
         if ( mPerfIf->init().success() )
         {
            // Now, attempt to enable remote performance monitoring hooks.
            if ( ! mPerfIf->enable().success() )
            {
               vprDEBUG(jcclDBG_RECONFIG, vprDBG_WARNING_LVL)
               << clrOutBOLD(clrYELLOW, "WARNING:")
               << " Failed to enable remote performance monitoring hooks.\n"
                  << vprDEBUG_FLUSH;
               delete mPerfIf;
               mPerfIf = NULL;
            }
         }
         // Initialization failed.
         else
         {
            vprDEBUG(jcclDBG_RECONFIG, vprDBG_WARNING_LVL)
            << clrOutBOLD(clrYELLOW, "WARNING:")
            << " Failed to initialize remote performance monitoring hooks.\n"
               << vprDEBUG_FLUSH;
            delete mPerfIf;
            mPerfIf = NULL;
         }
      }
   }

} // namespace vrj
<|endoftext|>