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

#include "results.hpp"

#include "impl/async_query.hpp"
#include "impl/realm_coordinator.hpp"
#include "object_store.hpp"

#include <stdexcept>

using namespace realm;

#ifdef __has_cpp_attribute
#define REALM_HAS_CCP_ATTRIBUTE(attr) __has_cpp_attribute(attr)
#else
#define REALM_HAS_CCP_ATTRIBUTE(attr) 0
#endif

#if REALM_HAS_CCP_ATTRIBUTE(clang::fallthrough)
#define REALM_FALLTHROUGH [[clang::fallthrough]]
#else
#define REALM_FALLTHROUGH
#endif

Results::Results(SharedRealm r, const ObjectSchema &o, Query q, SortOrder s)
: m_realm(std::move(r))
, m_object_schema(&o)
, m_query(std::move(q))
, m_table(m_query.get_table().get())
, m_sort(std::move(s))
, m_mode(Mode::Query)
{
}

Results::Results(SharedRealm r, const ObjectSchema &o, Table& table)
: m_realm(std::move(r))
, m_object_schema(&o)
, m_table(&table)
, m_mode(Mode::Table)
{
}

Results::~Results()
{
    if (m_background_query) {
        m_background_query->unregister();
    }
}

void Results::validate_read() const
{
    if (m_realm)
        m_realm->verify_thread();
    if (m_table && !m_table->is_attached())
        throw InvalidatedException();
    if (m_mode == Mode::TableView && !m_table_view.is_attached())
        throw InvalidatedException();
}

void Results::validate_write() const
{
    validate_read();
    if (!m_realm || !m_realm->is_in_transaction())
        throw InvalidTransactionException("Must be in a write transaction");
}

void Results::set_live(bool live)
{
    if (!live && m_mode == Mode::Table) {
        m_query = m_table->where();
        m_mode = Mode::Query;
    }

    update_tableview();
    m_live = live;
}

size_t Results::size()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty: return 0;
        case Mode::Table: return m_table->size();
        case Mode::Query: return m_query.count();
        case Mode::TableView:
            update_tableview();
            return m_table_view.size();
    }
    REALM_UNREACHABLE();
}

StringData Results::get_object_type() const noexcept
{
    return get_object_schema().name;
}

RowExpr Results::get(size_t row_ndx)
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty: break;
        case Mode::Table:
            if (row_ndx < m_table->size())
                return m_table->get(row_ndx);
            break;
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            if (row_ndx < m_table_view.size())
                return (!m_live && !m_table_view.is_row_attached(row_ndx)) ? RowExpr() : m_table_view.get(row_ndx);
            break;
    }

    throw OutOfBoundsIndexException{row_ndx, size()};
}

util::Optional<RowExpr> Results::first()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
            return none;
        case Mode::Table:
            return m_table->size() == 0 ? util::none : util::make_optional(m_table->front());
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            return m_table_view.size() == 0 ? util::none : util::make_optional(m_table_view.front());
    }
    REALM_UNREACHABLE();
}

util::Optional<RowExpr> Results::last()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
            return none;
        case Mode::Table:
            return m_table->size() == 0 ? util::none : util::make_optional(m_table->back());
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            return m_table_view.size() == 0 ? util::none : util::make_optional(m_table_view.back());
    }
    REALM_UNREACHABLE();
}

void Results::update_tableview()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
        case Mode::Table:
            return;
        case Mode::Query:
            m_table_view = m_query.find_all();
            if (m_sort) {
                m_table_view.sort(m_sort.columnIndices, m_sort.ascending);
            }
            m_mode = Mode::TableView;
            break;
        case Mode::TableView:
            if (!m_live) {
                return;
            }
            if (!m_background_query && !m_realm->is_in_transaction() && m_realm->can_deliver_notifications()) {
                m_background_query = std::make_shared<_impl::AsyncQuery>(*this);
                _impl::RealmCoordinator::register_query(m_background_query);
            }
            m_has_used_table_view = true;
            m_table_view.sync_if_needed();
            break;
    }
}

size_t Results::index_of(Row const& row)
{
    validate_read();
    if (!row) {
        throw DetatchedAccessorException{};
    }
    if (m_table && row.get_table() != m_table) {
        throw IncorrectTableException(m_object_schema->name,
            ObjectStore::object_type_for_table_name(row.get_table()->get_name()),
            "Attempting to get the index of a Row of the wrong type"
        );
    }
    return index_of(row.get_index());
}

size_t Results::index_of(size_t row_ndx)
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
            return not_found;
        case Mode::Table:
            return row_ndx;
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            return m_table_view.find_by_source_ndx(row_ndx);
    }
    REALM_UNREACHABLE();
}

template<typename Int, typename Float, typename Double, typename DateTime>
util::Optional<Mixed> Results::aggregate(size_t column, bool return_none_for_empty,
                                         Int agg_int, Float agg_float,
                                         Double agg_double, DateTime agg_datetime)
{
    validate_read();
    if (!m_table)
        return none;
    if (column > m_table->get_column_count())
        throw OutOfBoundsIndexException{column, m_table->get_column_count()};

    auto do_agg = [&](auto const& getter) -> util::Optional<Mixed> {
        switch (m_mode) {
            case Mode::Empty:
                return none;
            case Mode::Table:
                if (return_none_for_empty && m_table->size() == 0)
                    return none;
                return util::Optional<Mixed>(getter(*m_table));
            case Mode::Query:
            case Mode::TableView:
                this->update_tableview();
                if (return_none_for_empty && m_table_view.size() == 0)
                    return none;
                return util::Optional<Mixed>(getter(m_table_view));
        }
        REALM_UNREACHABLE();
    };

    switch (m_table->get_column_type(column))
    {
        case type_DateTime: return do_agg(agg_datetime);
        case type_Double: return do_agg(agg_double);
        case type_Float: return do_agg(agg_float);
        case type_Int: return do_agg(agg_int);
        default:
            throw UnsupportedColumnTypeException{column, m_table};
    }
}

util::Optional<Mixed> Results::max(size_t column)
{
    return aggregate(column, true,
                     [=](auto const& table) { return table.maximum_int(column); },
                     [=](auto const& table) { return table.maximum_float(column); },
                     [=](auto const& table) { return table.maximum_double(column); },
                     [=](auto const& table) { return table.maximum_datetime(column); });
}

util::Optional<Mixed> Results::min(size_t column)
{
    return aggregate(column, true,
                     [=](auto const& table) { return table.minimum_int(column); },
                     [=](auto const& table) { return table.minimum_float(column); },
                     [=](auto const& table) { return table.minimum_double(column); },
                     [=](auto const& table) { return table.minimum_datetime(column); });
}

util::Optional<Mixed> Results::sum(size_t column)
{
    return aggregate(column, false,
                     [=](auto const& table) { return table.sum_int(column); },
                     [=](auto const& table) { return table.sum_float(column); },
                     [=](auto const& table) { return table.sum_double(column); },
                     [=](auto const&) -> util::None { throw UnsupportedColumnTypeException{column, m_table}; });
}

util::Optional<Mixed> Results::average(size_t column)
{
    return aggregate(column, true,
                     [=](auto const& table) { return table.average_int(column); },
                     [=](auto const& table) { return table.average_float(column); },
                     [=](auto const& table) { return table.average_double(column); },
                     [=](auto const&) -> util::None { throw UnsupportedColumnTypeException{column, m_table}; });
}

void Results::clear()
{
    switch (m_mode) {
        case Mode::Empty:
            return;
        case Mode::Table:
            validate_write();
            m_table->clear();
            break;
        case Mode::Query:
            // Not using Query:remove() because building the tableview and
            // clearing it is actually significantly faster
        case Mode::TableView:
            validate_write();
            update_tableview();
            m_table_view.clear(RemoveMode::unordered);
            break;
    }
}

Query Results::get_query() const
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
        case Mode::Query:
            return m_query;
        case Mode::TableView:
            return m_table_view.get_query();
        case Mode::Table:
            return m_table->where();
    }
    REALM_UNREACHABLE();
}

TableView Results::get_tableview()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
            return {};
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            return m_table_view;
        case Mode::Table:
            return m_table->where().find_all();
    }
    REALM_UNREACHABLE();
}

Results Results::sort(realm::SortOrder&& sort) const
{
    return Results(m_realm, get_object_schema(), get_query(), std::move(sort));
}

Results Results::filter(Query&& q) const
{
    return Results(m_realm, get_object_schema(), get_query().and_query(std::move(q)), get_sort());
}

AsyncQueryCancelationToken Results::async(std::function<void (std::exception_ptr)> target)
{
    if (m_realm->config().read_only) {
        throw InvalidTransactionException("Cannot create asynchronous query for read-only Realms");
    }
    if (m_realm->is_in_transaction()) {
        throw InvalidTransactionException("Cannot create asynchronous query while in a write transaction");
    }

    if (!m_background_query) {
        m_background_query = std::make_shared<_impl::AsyncQuery>(*this);
        _impl::RealmCoordinator::register_query(m_background_query);
    }
    return {m_background_query, m_background_query->add_callback(std::move(target))};
}

void Results::Internal::set_table_view(Results& results, realm::TableView &&tv)
{
    // If the previous TableView was never actually used, then stop generating
    // new ones until the user actually uses the Results object again
    if (results.m_mode == Mode::TableView) {
        results.m_wants_background_updates = results.m_has_used_table_view;
    }

    results.m_table_view = std::move(tv);
    results.m_mode = Mode::TableView;
    results.m_has_used_table_view = false;
    REALM_ASSERT(results.m_table_view.is_in_sync());
}

Results::UnsupportedColumnTypeException::UnsupportedColumnTypeException(size_t column, const Table* table)
: std::runtime_error((std::string)"Operation not supported on '" + table->get_column_name(column).data() + "' columns")
, column_index(column)
, column_name(table->get_column_name(column))
, column_type(table->get_column_type(column))
{
}

AsyncQueryCancelationToken::AsyncQueryCancelationToken(std::shared_ptr<_impl::AsyncQuery> query, size_t token)
: m_query(std::move(query)), m_token(token)
{
}

AsyncQueryCancelationToken::~AsyncQueryCancelationToken()
{
    // m_query itself (and not just the pointed-to thing) needs to be accessed
    // atomically to ensure that there are no data races when the token is
    // destroyed after being modified on a different thread.
    // This is needed despite the token not being thread-safe in general as
    // users find it very surpringing for obj-c objects to care about what
    // thread they are deallocated on.
    if (auto query = m_query.exchange({})) {
        query->remove_callback(m_token);
    }
}

AsyncQueryCancelationToken::AsyncQueryCancelationToken(AsyncQueryCancelationToken&& rgt) = default;

AsyncQueryCancelationToken& AsyncQueryCancelationToken::operator=(realm::AsyncQueryCancelationToken&& rgt)
{
    if (this != &rgt) {
        if (auto query = m_query.exchange({})) {
            query->remove_callback(m_token);
        }
        m_query = std::move(rgt.m_query);
        m_token = rgt.m_token;
    }
    return *this;
}
<commit_msg>Add validate_read() check to Results::set_live()<commit_after>////////////////////////////////////////////////////////////////////////////
//
// Copyright 2015 Realm Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
////////////////////////////////////////////////////////////////////////////

#include "results.hpp"

#include "impl/async_query.hpp"
#include "impl/realm_coordinator.hpp"
#include "object_store.hpp"

#include <stdexcept>

using namespace realm;

#ifdef __has_cpp_attribute
#define REALM_HAS_CCP_ATTRIBUTE(attr) __has_cpp_attribute(attr)
#else
#define REALM_HAS_CCP_ATTRIBUTE(attr) 0
#endif

#if REALM_HAS_CCP_ATTRIBUTE(clang::fallthrough)
#define REALM_FALLTHROUGH [[clang::fallthrough]]
#else
#define REALM_FALLTHROUGH
#endif

Results::Results(SharedRealm r, const ObjectSchema &o, Query q, SortOrder s)
: m_realm(std::move(r))
, m_object_schema(&o)
, m_query(std::move(q))
, m_table(m_query.get_table().get())
, m_sort(std::move(s))
, m_mode(Mode::Query)
{
}

Results::Results(SharedRealm r, const ObjectSchema &o, Table& table)
: m_realm(std::move(r))
, m_object_schema(&o)
, m_table(&table)
, m_mode(Mode::Table)
{
}

Results::~Results()
{
    if (m_background_query) {
        m_background_query->unregister();
    }
}

void Results::validate_read() const
{
    if (m_realm)
        m_realm->verify_thread();
    if (m_table && !m_table->is_attached())
        throw InvalidatedException();
    if (m_mode == Mode::TableView && !m_table_view.is_attached())
        throw InvalidatedException();
}

void Results::validate_write() const
{
    validate_read();
    if (!m_realm || !m_realm->is_in_transaction())
        throw InvalidTransactionException("Must be in a write transaction");
}

void Results::set_live(bool live)
{
    validate_read();

    if (!live && m_mode == Mode::Table) {
        m_query = m_table->where();
        m_mode = Mode::Query;
    }

    update_tableview();
    m_live = live;
}

size_t Results::size()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty: return 0;
        case Mode::Table: return m_table->size();
        case Mode::Query: return m_query.count();
        case Mode::TableView:
            update_tableview();
            return m_table_view.size();
    }
    REALM_UNREACHABLE();
}

StringData Results::get_object_type() const noexcept
{
    return get_object_schema().name;
}

RowExpr Results::get(size_t row_ndx)
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty: break;
        case Mode::Table:
            if (row_ndx < m_table->size())
                return m_table->get(row_ndx);
            break;
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            if (row_ndx < m_table_view.size())
                return (!m_live && !m_table_view.is_row_attached(row_ndx)) ? RowExpr() : m_table_view.get(row_ndx);
            break;
    }

    throw OutOfBoundsIndexException{row_ndx, size()};
}

util::Optional<RowExpr> Results::first()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
            return none;
        case Mode::Table:
            return m_table->size() == 0 ? util::none : util::make_optional(m_table->front());
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            return m_table_view.size() == 0 ? util::none : util::make_optional(m_table_view.front());
    }
    REALM_UNREACHABLE();
}

util::Optional<RowExpr> Results::last()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
            return none;
        case Mode::Table:
            return m_table->size() == 0 ? util::none : util::make_optional(m_table->back());
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            return m_table_view.size() == 0 ? util::none : util::make_optional(m_table_view.back());
    }
    REALM_UNREACHABLE();
}

void Results::update_tableview()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
        case Mode::Table:
            return;
        case Mode::Query:
            m_table_view = m_query.find_all();
            if (m_sort) {
                m_table_view.sort(m_sort.columnIndices, m_sort.ascending);
            }
            m_mode = Mode::TableView;
            break;
        case Mode::TableView:
            if (!m_live) {
                return;
            }
            if (!m_background_query && !m_realm->is_in_transaction() && m_realm->can_deliver_notifications()) {
                m_background_query = std::make_shared<_impl::AsyncQuery>(*this);
                _impl::RealmCoordinator::register_query(m_background_query);
            }
            m_has_used_table_view = true;
            m_table_view.sync_if_needed();
            break;
    }
}

size_t Results::index_of(Row const& row)
{
    validate_read();
    if (!row) {
        throw DetatchedAccessorException{};
    }
    if (m_table && row.get_table() != m_table) {
        throw IncorrectTableException(m_object_schema->name,
            ObjectStore::object_type_for_table_name(row.get_table()->get_name()),
            "Attempting to get the index of a Row of the wrong type"
        );
    }
    return index_of(row.get_index());
}

size_t Results::index_of(size_t row_ndx)
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
            return not_found;
        case Mode::Table:
            return row_ndx;
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            return m_table_view.find_by_source_ndx(row_ndx);
    }
    REALM_UNREACHABLE();
}

template<typename Int, typename Float, typename Double, typename DateTime>
util::Optional<Mixed> Results::aggregate(size_t column, bool return_none_for_empty,
                                         Int agg_int, Float agg_float,
                                         Double agg_double, DateTime agg_datetime)
{
    validate_read();
    if (!m_table)
        return none;
    if (column > m_table->get_column_count())
        throw OutOfBoundsIndexException{column, m_table->get_column_count()};

    auto do_agg = [&](auto const& getter) -> util::Optional<Mixed> {
        switch (m_mode) {
            case Mode::Empty:
                return none;
            case Mode::Table:
                if (return_none_for_empty && m_table->size() == 0)
                    return none;
                return util::Optional<Mixed>(getter(*m_table));
            case Mode::Query:
            case Mode::TableView:
                this->update_tableview();
                if (return_none_for_empty && m_table_view.size() == 0)
                    return none;
                return util::Optional<Mixed>(getter(m_table_view));
        }
        REALM_UNREACHABLE();
    };

    switch (m_table->get_column_type(column))
    {
        case type_DateTime: return do_agg(agg_datetime);
        case type_Double: return do_agg(agg_double);
        case type_Float: return do_agg(agg_float);
        case type_Int: return do_agg(agg_int);
        default:
            throw UnsupportedColumnTypeException{column, m_table};
    }
}

util::Optional<Mixed> Results::max(size_t column)
{
    return aggregate(column, true,
                     [=](auto const& table) { return table.maximum_int(column); },
                     [=](auto const& table) { return table.maximum_float(column); },
                     [=](auto const& table) { return table.maximum_double(column); },
                     [=](auto const& table) { return table.maximum_datetime(column); });
}

util::Optional<Mixed> Results::min(size_t column)
{
    return aggregate(column, true,
                     [=](auto const& table) { return table.minimum_int(column); },
                     [=](auto const& table) { return table.minimum_float(column); },
                     [=](auto const& table) { return table.minimum_double(column); },
                     [=](auto const& table) { return table.minimum_datetime(column); });
}

util::Optional<Mixed> Results::sum(size_t column)
{
    return aggregate(column, false,
                     [=](auto const& table) { return table.sum_int(column); },
                     [=](auto const& table) { return table.sum_float(column); },
                     [=](auto const& table) { return table.sum_double(column); },
                     [=](auto const&) -> util::None { throw UnsupportedColumnTypeException{column, m_table}; });
}

util::Optional<Mixed> Results::average(size_t column)
{
    return aggregate(column, true,
                     [=](auto const& table) { return table.average_int(column); },
                     [=](auto const& table) { return table.average_float(column); },
                     [=](auto const& table) { return table.average_double(column); },
                     [=](auto const&) -> util::None { throw UnsupportedColumnTypeException{column, m_table}; });
}

void Results::clear()
{
    switch (m_mode) {
        case Mode::Empty:
            return;
        case Mode::Table:
            validate_write();
            m_table->clear();
            break;
        case Mode::Query:
            // Not using Query:remove() because building the tableview and
            // clearing it is actually significantly faster
        case Mode::TableView:
            validate_write();
            update_tableview();
            m_table_view.clear(RemoveMode::unordered);
            break;
    }
}

Query Results::get_query() const
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
        case Mode::Query:
            return m_query;
        case Mode::TableView:
            return m_table_view.get_query();
        case Mode::Table:
            return m_table->where();
    }
    REALM_UNREACHABLE();
}

TableView Results::get_tableview()
{
    validate_read();
    switch (m_mode) {
        case Mode::Empty:
            return {};
        case Mode::Query:
        case Mode::TableView:
            update_tableview();
            return m_table_view;
        case Mode::Table:
            return m_table->where().find_all();
    }
    REALM_UNREACHABLE();
}

Results Results::sort(realm::SortOrder&& sort) const
{
    return Results(m_realm, get_object_schema(), get_query(), std::move(sort));
}

Results Results::filter(Query&& q) const
{
    return Results(m_realm, get_object_schema(), get_query().and_query(std::move(q)), get_sort());
}

AsyncQueryCancelationToken Results::async(std::function<void (std::exception_ptr)> target)
{
    if (m_realm->config().read_only) {
        throw InvalidTransactionException("Cannot create asynchronous query for read-only Realms");
    }
    if (m_realm->is_in_transaction()) {
        throw InvalidTransactionException("Cannot create asynchronous query while in a write transaction");
    }

    if (!m_background_query) {
        m_background_query = std::make_shared<_impl::AsyncQuery>(*this);
        _impl::RealmCoordinator::register_query(m_background_query);
    }
    return {m_background_query, m_background_query->add_callback(std::move(target))};
}

void Results::Internal::set_table_view(Results& results, realm::TableView &&tv)
{
    // If the previous TableView was never actually used, then stop generating
    // new ones until the user actually uses the Results object again
    if (results.m_mode == Mode::TableView) {
        results.m_wants_background_updates = results.m_has_used_table_view;
    }

    results.m_table_view = std::move(tv);
    results.m_mode = Mode::TableView;
    results.m_has_used_table_view = false;
    REALM_ASSERT(results.m_table_view.is_in_sync());
}

Results::UnsupportedColumnTypeException::UnsupportedColumnTypeException(size_t column, const Table* table)
: std::runtime_error((std::string)"Operation not supported on '" + table->get_column_name(column).data() + "' columns")
, column_index(column)
, column_name(table->get_column_name(column))
, column_type(table->get_column_type(column))
{
}

AsyncQueryCancelationToken::AsyncQueryCancelationToken(std::shared_ptr<_impl::AsyncQuery> query, size_t token)
: m_query(std::move(query)), m_token(token)
{
}

AsyncQueryCancelationToken::~AsyncQueryCancelationToken()
{
    // m_query itself (and not just the pointed-to thing) needs to be accessed
    // atomically to ensure that there are no data races when the token is
    // destroyed after being modified on a different thread.
    // This is needed despite the token not being thread-safe in general as
    // users find it very surpringing for obj-c objects to care about what
    // thread they are deallocated on.
    if (auto query = m_query.exchange({})) {
        query->remove_callback(m_token);
    }
}

AsyncQueryCancelationToken::AsyncQueryCancelationToken(AsyncQueryCancelationToken&& rgt) = default;

AsyncQueryCancelationToken& AsyncQueryCancelationToken::operator=(realm::AsyncQueryCancelationToken&& rgt)
{
    if (this != &rgt) {
        if (auto query = m_query.exchange({})) {
            query->remove_callback(m_token);
        }
        m_query = std::move(rgt.m_query);
        m_token = rgt.m_token;
    }
    return *this;
}
<|endoftext|>


<commit_before>/* bzflag
 * Copyright (c) 1993 - 2004 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

#include <math.h>
#include "common.h"
#include "global.h"
#include "PyramidBuilding.h"
#include "Intersect.h"
#include "TriWallSceneNode.h"
#include "QuadWallSceneNode.h"
#include "BZDBCache.h"
#include "PyramidSceneNodeGenerator.h"

std::string		PyramidBuilding::typeName("PyramidBuilding");

PyramidBuilding::PyramidBuilding(const float* p, float a,
				float w, float b, float h, bool drive, bool shoot) :
				Obstacle(p, a, w, b, h,drive,shoot)
{
  // do nothing
}

PyramidBuilding::~PyramidBuilding()
{
  // do nothing
}

std::string		PyramidBuilding::getType() const
{
  return typeName;
}

std::string		PyramidBuilding::getClassName() // const
{
  return typeName;
}

float			PyramidBuilding::intersect(const Ray& r) const
{
  return timeRayHitsPyramids(r, getPosition(), getRotation(),
			     getWidth(), getBreadth(), getHeight(),
			     getZFlip());
}

void			PyramidBuilding::getNormal(const float* p,
							float* n) const
{
  // get normal in z = const plane
  const float s = shrinkFactor(p[2]);
  getNormalRect(p, getPosition(), getRotation(),
			s * getWidth(), s * getBreadth(), n);

  // make sure we are not way above or way below it
  // above is good so we can drive on it when it's fliped
  float top =  getPosition()[2]+getHeight();
  float bottom = getPosition()[2];

  if (s ==0){
	  if (this->getZFlip()){
		  if (p[2] >= top){
			  n[0] = n[1] = 0;
			  n[2] = 1;
			  return;
		  }
	  }else{
		  if (p[2] <= bottom){
			  n[0] = n[1] = 0;
			  n[2] = -1;
			  return;
		  }
	  }
  }

  // now angle it due to slope of wall
  // FIXME -- this assumes the pyramid has a square base!
  const float h = 1.0f / hypotf(getHeight(), getWidth());
  n[0] *= h * getHeight();
  n[1] *= h * getHeight();
  n[2] = h * getWidth();

  if (this->getZFlip())
	  n[2] *= -1;
}

void			PyramidBuilding::get3DNormal(const float* p,
						     float* n) const
{
  const float epsilon = ZERO_TOLERANCE;

  // get normal in z = const plane
  const float s = shrinkFactor(p[2]);
  getNormalRect(p, getPosition(), getRotation(),
		s * getWidth(), s * getBreadth(), n);

  // make sure we are not way above or way below it
  // above is good so we can drive on it when it's fliped
  float top =  getPosition()[2]+getHeight();
  float bottom = getPosition()[2];

  if (s == 0) {
    if (getZFlip()) {
      if (p[2] >= top) {
	n[0] = n[1] = 0;
	n[2] = 1;
	return;
      }
    } else {
      if (p[2] <= bottom) {
	n[0] = n[1] = 0;
	n[2] = -1;
	return;
      }
    }
  }

  if (s >= 1.0f - epsilon) {
    n[0] = n[1] = 0;
    if (getZFlip()) {
      n[2] = 1;
    } else {
      n[2] = -1;
    }
    return;
  }

  // now angle it due to slope of wall
  // FIXME -- this assumes the pyramid has a square base!
  const float h = 1.0f / hypotf(getHeight(), getWidth());
  n[0] *= h * getHeight();
  n[1] *= h * getHeight();
  n[2]  = h * getWidth();

  if (this->getZFlip())
    n[2] *= -1;
}

bool			PyramidBuilding::isInside(const float* p,
						float radius) const
{
  // really rough -- doesn't decrease size with height
  return (p[2] <= getHeight())
  &&     ((p[2]+BZDBCache::tankHeight) >= getPosition()[2])
  &&     testRectCircle(getPosition(), getRotation(), getWidth(), getBreadth(), p, radius);
}

bool			PyramidBuilding::isInside(const float* p, float a,
						float dx, float dy) const
{
  // Tank is below pyramid ?
  if (p[2] + BZDBCache::tankHeight < getPosition()[2])
    return false;
  // Tank is above pyramid ?
  if (p[2] > getPosition()[2] + getHeight())
    return false;
  // Could be inside. Then check collision with the rectangle at object height
  // This is a rectangle reduced by shrinking but pass the height that we are
  // not so sure where collision can be
  const float s = shrinkFactor(p[2], BZDBCache::tankHeight);
  return testRectRect(getPosition(), getRotation(),
		      s * getWidth(), s * getBreadth(), p, a, dx, dy);
}

bool			PyramidBuilding::isCrossing(const float* p, float a,
					float dx, float dy, float* plane) const
{
  // if not inside or contained then not crossing
  if (!isInside(p, a, dx, dy) ||
	testRectInRect(getPosition(), getRotation(),
			getWidth(), getBreadth(), p, a, dx, dy))
    return false;
  if (!plane) return true;

  // it's crossing -- choose which wall is being crossed (this
  // is a guestimate, should really do a careful test).  just
  // see which wall the point is closest to.
  const float* p2 = getPosition();
  const float a2 = getRotation();
  const float c = cosf(-a2), s = sinf(-a2);
  const float x = c * (p[0] - p2[0]) - s * (p[1] - p2[1]);
  const float y = c * (p[1] - p2[1]) + s * (p[0] - p2[0]);
  float pw[2];
  if (fabsf(fabsf(x) - getWidth()) < fabsf(fabsf(y) - getBreadth())) {
    plane[0] = ((x < 0.0) ? -cosf(a2) : cosf(a2));
    plane[1] = ((x < 0.0) ? -sinf(a2) : sinf(a2));
    pw[0] = p2[0] + getWidth() * plane[0];
    pw[1] = p2[1] + getWidth() * plane[1];
  }
  else {
    plane[0] = ((y < 0.0) ? sinf(a2) : -sinf(a2));
    plane[1] = ((y < 0.0) ? -cosf(a2) : cosf(a2));
    pw[0] = p2[0] + getBreadth() * plane[0];
    pw[1] = p2[1] + getBreadth() * plane[1];
  }

  // now finish off plane equation (FIXME -- assumes a square base)
  const float h = 1.0f / hypotf(getHeight(), getWidth());
  plane[0] *= h * getHeight();
  plane[1] *= h * getHeight();
  plane[2] = h * getWidth();
  plane[3] = -(plane[0] * pw[0] + plane[1] * pw[1]);
  return true;
}

bool			PyramidBuilding::getHitNormal(
				const float* pos1, float,
				const float* pos2, float,
				float, float, float height,
				float* normal) const
{
  // pyramids height and flipping
  // normalize height sign and report that in flip
  float oHeight = getHeight();
  bool  flip    = getZFlip();
  if (oHeight < 0) {
    flip    = !flip;
    oHeight = -oHeight;
  }

  // get Bottom and Top of building
  float oBottom = getPosition()[2];
  float oTop    = oBottom + oHeight;

  // get higher and lower point of base of colliding object
  float objHigh = pos1[2];
  float objLow  = pos2[2];
  if (objHigh < objLow) {
    float temp = objHigh;
    objHigh    = objLow;
    objLow     = temp;
  }

  normal[0] = normal[1] = 0;
  if (flip && objHigh > oTop) {
    // base of higher object is over the plateau
    normal[2] = 1;
    return true;
  } else if (!flip && objLow + height < oBottom) {
    // top of lower object is below the base
    normal[2] = -1;
    return true;
  }

  // get normal in z = const plane
  const float s = shrinkFactor(pos1[2], height);

  getNormalRect(pos1, getPosition(), getRotation(),
		s * getWidth(), s * getBreadth(), normal);

  // now angle it due to slope of wall
  // FIXME -- this assumes the pyramid has a square base!
  const float h = 1.0f / hypotf(oHeight, getWidth());
  normal[0] *= h * oHeight;
  normal[1] *= h * oHeight;
  normal[2]  = h * getWidth();

  if (flip)
    normal[2] = -normal[2];
  return true;
}

ObstacleSceneNodeGenerator*	PyramidBuilding::newSceneNodeGenerator() const
{
  return new PyramidSceneNodeGenerator(this);
}

void			PyramidBuilding::getCorner(int index,
						float* pos) const
{
  const float* base = getPosition();
  const float c = cosf(getRotation());
  const float s = sinf(getRotation());
  const float w = getWidth();
  const float h = getBreadth();
  const float top  = getHeight() + base[2];
  switch (index) {
    case 0:
      pos[0] = base[0] + c * w - s * h;
      pos[1] = base[1] + s * w + c * h;
	  if (getZFlip())
		 pos[2] = top;
	  else
		 pos[2] = base[2];
      break;
    case 1:
      pos[0] = base[0] - c * w - s * h;
      pos[1] = base[1] - s * w + c * h;
	  if (getZFlip())
		 pos[2] = top;
	  else
		 pos[2] = base[2];
      break;
    case 2:
      pos[0] = base[0] - c * w + s * h;
      pos[1] = base[1] - s * w - c * h;
	  if (getZFlip())
		 pos[2] = top;
	  else
		 pos[2] = base[2];
      break;
    case 3:
      pos[0] = base[0] + c * w + s * h;
      pos[1] = base[1] + s * w - c * h;
	  if (getZFlip())
		 pos[2] = top;
	  else
		 pos[2] = base[2];
      break;
    case 4:
      pos[0] = base[0];
      pos[1] = base[1];
	  if (getZFlip())
		 pos[2] = base[2];
	  else
		 pos[2] = top;
      break;
  }
}

float			PyramidBuilding::shrinkFactor(float z,
						      float height) const
{
  float shrink;

  // Normalize Height and flip to have height > 0
  float oHeight = getHeight();
  bool  flip    = getZFlip();
  if (oHeight < 0) {
    flip    = !flip;
    oHeight = - oHeight;
  }

 // Remove heights bias
  const float *pos = getPosition();
  z -= pos[2];
  if (oHeight <= ZERO_TOLERANCE) {
    shrink = 1.0f;
  } else {
    // Normalize heights
    z /= oHeight;

    // if flipped the bigger intersection is at top of the object
    if (flip) {
      // Normalize the object height, we have not done yet
      z += height / oHeight;
    }

    // shrink is that
    if (flip) {
      shrink = z;
    } else {
      shrink = 1.0f - z;
    }
  }

  // clamp in 0 .. 1
  if (shrink < 0.0)
    shrink = 0.0;
  else if (shrink > 1.0)
    shrink = 1.0;

  return shrink;
}

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8

<commit_msg>Fixing stuck on flipped pyramids<commit_after>/* bzflag
 * Copyright (c) 1993 - 2004 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

#include <math.h>
#include "common.h"
#include "global.h"
#include "PyramidBuilding.h"
#include "Intersect.h"
#include "TriWallSceneNode.h"
#include "QuadWallSceneNode.h"
#include "BZDBCache.h"
#include "PyramidSceneNodeGenerator.h"

std::string		PyramidBuilding::typeName("PyramidBuilding");

PyramidBuilding::PyramidBuilding(const float* p, float a,
				float w, float b, float h, bool drive, bool shoot) :
				Obstacle(p, a, w, b, h,drive,shoot)
{
  // do nothing
}

PyramidBuilding::~PyramidBuilding()
{
  // do nothing
}

std::string		PyramidBuilding::getType() const
{
  return typeName;
}

std::string		PyramidBuilding::getClassName() // const
{
  return typeName;
}

float			PyramidBuilding::intersect(const Ray& r) const
{
  return timeRayHitsPyramids(r, getPosition(), getRotation(),
			     getWidth(), getBreadth(), getHeight(),
			     getZFlip());
}

void			PyramidBuilding::getNormal(const float* p,
							float* n) const
{
  // get normal in z = const plane
  const float s = shrinkFactor(p[2]);
  getNormalRect(p, getPosition(), getRotation(),
			s * getWidth(), s * getBreadth(), n);

  // make sure we are not way above or way below it
  // above is good so we can drive on it when it's fliped
  float top =  getPosition()[2]+getHeight();
  float bottom = getPosition()[2];

  if (s ==0){
	  if (this->getZFlip()){
		  if (p[2] >= top){
			  n[0] = n[1] = 0;
			  n[2] = 1;
			  return;
		  }
	  }else{
		  if (p[2] <= bottom){
			  n[0] = n[1] = 0;
			  n[2] = -1;
			  return;
		  }
	  }
  }

  // now angle it due to slope of wall
  // FIXME -- this assumes the pyramid has a square base!
  const float h = 1.0f / hypotf(getHeight(), getWidth());
  n[0] *= h * getHeight();
  n[1] *= h * getHeight();
  n[2] = h * getWidth();

  if (this->getZFlip())
	  n[2] *= -1;
}

void			PyramidBuilding::get3DNormal(const float* p,
						     float* n) const
{
  const float epsilon = ZERO_TOLERANCE;

  // get normal in z = const plane
  const float s = shrinkFactor(p[2]);
  getNormalRect(p, getPosition(), getRotation(),
		s * getWidth(), s * getBreadth(), n);

  // make sure we are not way above or way below it
  // above is good so we can drive on it when it's fliped
  float top =  getPosition()[2]+getHeight();
  float bottom = getPosition()[2];

  if (s == 0) {
    if (getZFlip()) {
      if (p[2] >= top) {
	n[0] = n[1] = 0;
	n[2] = 1;
	return;
      }
    } else {
      if (p[2] <= bottom) {
	n[0] = n[1] = 0;
	n[2] = -1;
	return;
      }
    }
  }

  if (s >= 1.0f - epsilon) {
    n[0] = n[1] = 0;
    if (getZFlip()) {
      n[2] = 1;
    } else {
      n[2] = -1;
    }
    return;
  }

  // now angle it due to slope of wall
  // FIXME -- this assumes the pyramid has a square base!
  const float h = 1.0f / hypotf(getHeight(), getWidth());
  n[0] *= h * getHeight();
  n[1] *= h * getHeight();
  n[2]  = h * getWidth();

  if (this->getZFlip())
    n[2] *= -1;
}

bool			PyramidBuilding::isInside(const float* p,
						float radius) const
{
  // really rough -- doesn't decrease size with height
  return (p[2] <= getHeight())
  &&     ((p[2]+BZDBCache::tankHeight) >= getPosition()[2])
  &&     testRectCircle(getPosition(), getRotation(), getWidth(), getBreadth(), p, radius);
}

bool			PyramidBuilding::isInside(const float* p, float a,
						float dx, float dy) const
{
  // Tank is below pyramid ?
  if (p[2] + BZDBCache::tankHeight < getPosition()[2])
    return false;
  // Tank is above pyramid ?
  if (p[2] >= getPosition()[2] + getHeight())
    return false;
  // Could be inside. Then check collision with the rectangle at object height
  // This is a rectangle reduced by shrinking but pass the height that we are
  // not so sure where collision can be
  const float s = shrinkFactor(p[2], BZDBCache::tankHeight);
  return testRectRect(getPosition(), getRotation(),
		      s * getWidth(), s * getBreadth(), p, a, dx, dy);
}

bool			PyramidBuilding::isCrossing(const float* p, float a,
					float dx, float dy, float* plane) const
{
  // if not inside or contained then not crossing
  if (!isInside(p, a, dx, dy) ||
	testRectInRect(getPosition(), getRotation(),
			getWidth(), getBreadth(), p, a, dx, dy))
    return false;
  if (!plane) return true;

  // it's crossing -- choose which wall is being crossed (this
  // is a guestimate, should really do a careful test).  just
  // see which wall the point is closest to.
  const float* p2 = getPosition();
  const float a2 = getRotation();
  const float c = cosf(-a2), s = sinf(-a2);
  const float x = c * (p[0] - p2[0]) - s * (p[1] - p2[1]);
  const float y = c * (p[1] - p2[1]) + s * (p[0] - p2[0]);
  float pw[2];
  if (fabsf(fabsf(x) - getWidth()) < fabsf(fabsf(y) - getBreadth())) {
    plane[0] = ((x < 0.0) ? -cosf(a2) : cosf(a2));
    plane[1] = ((x < 0.0) ? -sinf(a2) : sinf(a2));
    pw[0] = p2[0] + getWidth() * plane[0];
    pw[1] = p2[1] + getWidth() * plane[1];
  }
  else {
    plane[0] = ((y < 0.0) ? sinf(a2) : -sinf(a2));
    plane[1] = ((y < 0.0) ? -cosf(a2) : cosf(a2));
    pw[0] = p2[0] + getBreadth() * plane[0];
    pw[1] = p2[1] + getBreadth() * plane[1];
  }

  // now finish off plane equation (FIXME -- assumes a square base)
  const float h = 1.0f / hypotf(getHeight(), getWidth());
  plane[0] *= h * getHeight();
  plane[1] *= h * getHeight();
  plane[2] = h * getWidth();
  plane[3] = -(plane[0] * pw[0] + plane[1] * pw[1]);
  return true;
}

bool			PyramidBuilding::getHitNormal(
				const float* pos1, float,
				const float* pos2, float,
				float, float, float height,
				float* normal) const
{
  // pyramids height and flipping
  // normalize height sign and report that in flip
  float oHeight = getHeight();
  bool  flip    = getZFlip();
  if (oHeight < 0) {
    flip    = !flip;
    oHeight = -oHeight;
  }

  // get Bottom and Top of building
  float oBottom = getPosition()[2];
  float oTop    = oBottom + oHeight;

  // get higher and lower point of base of colliding object
  float objHigh = pos1[2];
  float objLow  = pos2[2];
  if (objHigh < objLow) {
    float temp = objHigh;
    objHigh    = objLow;
    objLow     = temp;
  }

  normal[0] = normal[1] = 0;
  if (flip && objHigh >= oTop) {
    // base of higher object is over the plateau
    normal[2] = 1;
    return true;
  } else if (!flip && objLow + height < oBottom) {
    // top of lower object is below the base
    normal[2] = -1;
    return true;
  }

  // get normal in z = const plane
  const float s = shrinkFactor(pos1[2], height);

  getNormalRect(pos1, getPosition(), getRotation(),
		s * getWidth(), s * getBreadth(), normal);

  // now angle it due to slope of wall
  // FIXME -- this assumes the pyramid has a square base!
  const float h = 1.0f / hypotf(oHeight, getWidth());
  normal[0] *= h * oHeight;
  normal[1] *= h * oHeight;
  normal[2]  = h * getWidth();

  if (flip)
    normal[2] = -normal[2];
  return true;
}

ObstacleSceneNodeGenerator*	PyramidBuilding::newSceneNodeGenerator() const
{
  return new PyramidSceneNodeGenerator(this);
}

void			PyramidBuilding::getCorner(int index,
						float* pos) const
{
  const float* base = getPosition();
  const float c = cosf(getRotation());
  const float s = sinf(getRotation());
  const float w = getWidth();
  const float h = getBreadth();
  const float top  = getHeight() + base[2];
  switch (index) {
    case 0:
      pos[0] = base[0] + c * w - s * h;
      pos[1] = base[1] + s * w + c * h;
	  if (getZFlip())
		 pos[2] = top;
	  else
		 pos[2] = base[2];
      break;
    case 1:
      pos[0] = base[0] - c * w - s * h;
      pos[1] = base[1] - s * w + c * h;
	  if (getZFlip())
		 pos[2] = top;
	  else
		 pos[2] = base[2];
      break;
    case 2:
      pos[0] = base[0] - c * w + s * h;
      pos[1] = base[1] - s * w - c * h;
	  if (getZFlip())
		 pos[2] = top;
	  else
		 pos[2] = base[2];
      break;
    case 3:
      pos[0] = base[0] + c * w + s * h;
      pos[1] = base[1] + s * w - c * h;
	  if (getZFlip())
		 pos[2] = top;
	  else
		 pos[2] = base[2];
      break;
    case 4:
      pos[0] = base[0];
      pos[1] = base[1];
	  if (getZFlip())
		 pos[2] = base[2];
	  else
		 pos[2] = top;
      break;
  }
}

float			PyramidBuilding::shrinkFactor(float z,
						      float height) const
{
  float shrink;

  // Normalize Height and flip to have height > 0
  float oHeight = getHeight();
  bool  flip    = getZFlip();
  if (oHeight < 0) {
    flip    = !flip;
    oHeight = - oHeight;
  }

 // Remove heights bias
  const float *pos = getPosition();
  z -= pos[2];
  if (oHeight <= ZERO_TOLERANCE) {
    shrink = 1.0f;
  } else {
    // Normalize heights
    z /= oHeight;

    // if flipped the bigger intersection is at top of the object
    if (flip) {
      // Normalize the object height, we have not done yet
      z += height / oHeight;
    }

    // shrink is that
    if (flip) {
      shrink = z;
    } else {
      shrink = 1.0f - z;
    }
  }

  // clamp in 0 .. 1
  if (shrink < 0.0)
    shrink = 0.0;
  else if (shrink > 1.0)
    shrink = 1.0;

  return shrink;
}

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8

<|endoftext|>


<commit_before>// Copyright 2014, runtime.js project authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "transport.h"
#include <kernel/isolate.h>
#include <kernel/template-cache.h>
#include <kernel/object-wrapper.h>
#include <kernel/thread.h>

namespace rt {

TransportData::SerializeError TransportData::MoveValue(Thread* exporter,
                                                       Isolate* isolate_recv,
                                                       v8::Local<v8::Value> value) {
    RT_ASSERT(exporter);
    Isolate* isolate { exporter->isolate() };
    RT_ASSERT(isolate);
    RT_ASSERT(isolate_recv);
    Clear();
    isolate_ = isolate;
    allow_ref_ = isolate_recv == isolate;

    SerializeError err { SerializeValue(exporter, value, 1) };
    if (SerializeError::NONE != err) {
        SetUndefined();
    }

    return err;
}

TransportData::SerializeError TransportData::MoveArgs(Thread* exporter,
                                                      Isolate* isolate_recv,
                                                      const v8::FunctionCallbackInfo<v8::Value>& args) {
    RT_ASSERT(exporter);
    Isolate* isolate { exporter->isolate() };
    RT_ASSERT(isolate);
    RT_ASSERT(isolate_recv);
    Clear();
    isolate_ = isolate;
    allow_ref_ = isolate_recv == isolate;

    AppendType(Type::ARRAY);
    uint32_t len = args.Length();
    stream_.AppendValue<uint32_t>(len);

    for (uint32_t i = 0; i < len; ++i) {
        SerializeError err { SerializeValue(exporter, args[i], 1) };
        if (SerializeError::NONE != err) {
            SetUndefined();
            return err;
        }
    }

    return SerializeError::NONE;
}

v8::Local<v8::Value> TransportData::GetRef(v8::Isolate* iv8, uint32_t index) const {
    RT_ASSERT(allow_ref_);
    RT_ASSERT(iv8);
    RT_ASSERT(isolate_);
    RT_ASSERT(isolate_->IsolateV8());
    RT_ASSERT(isolate_->IsolateV8() == iv8);
    RT_ASSERT(index < refs_.size());
    v8::EscapableHandleScope scope(iv8);
    return scope.Escape(v8::Local<v8::Value>::New(iv8, refs_[index]));
}

uint32_t TransportData::AddRef(v8::Local<v8::Value> value) {
    RT_ASSERT(allow_ref_);
    RT_ASSERT(isolate_);
    RT_ASSERT(isolate_->IsolateV8());
    size_t index = refs_.size();
    refs_.push_back(std::move(v8::UniquePersistent<v8::Value>(isolate_->IsolateV8(), value)));
    return static_cast<uint32_t>(index);
}

TransportData::SerializeError TransportData::SerializeValue(Thread* exporter,
                                                            v8::Local<v8::Value> value,
                                                            uint32_t stack_level) {
    RT_ASSERT(exporter);
    RT_ASSERT(!value.IsEmpty());

    if (stack_level > kMaxStackSize) {
        return SerializeError::MAX_STACK;
    }

    if (value->IsUndefined()) {
        AppendType(Type::UNDEFINED);
        return SerializeError::NONE;
    }

    if (value->IsNull()) {
        AppendType(Type::NUL);
        return SerializeError::NONE;
    }

    if (value->IsBoolean()) {
        AppendType(value->BooleanValue() ? Type::BOOL_TRUE : Type::BOOL_FALSE);
        return SerializeError::NONE;
    }

    if (value->IsInt32()) {
        AppendType(Type::INT32);
        stream_.AppendValue<int32_t>(value->Int32Value());
        return SerializeError::NONE;
    }

    if (value->IsUint32()) {
        AppendType(Type::UINT32);
        stream_.AppendValue<uint32_t>(value->Uint32Value());
        return SerializeError::NONE;
    }

    if (value->IsNumber()) {
        AppendType(Type::DOUBLE);
        stream_.AppendValue<double>(value->NumberValue());
        return SerializeError::NONE;
    }

    if (value->IsString()) {
        if (allow_ref_) {
            // Strings are immutable, its safe to pass by reference
            // for same-isolate calls
            AppendType(Type::STRING_REF);
            stream_.AppendValue<uint32_t>(AddRef(value));
        } else {
            AppendType(Type::STRING_16);
            v8::Local<v8::String> s { value->ToString() };
            int len = s->Length();
            RT_ASSERT(len >= 0);
            stream_.AppendValue<uint32_t>(len);
            void* place { stream_.AppendBuffer((len + 1) * sizeof(uint16_t)) };
            s->Write(reinterpret_cast<uint16_t*>(place), 0, len);
        }

        return SerializeError::NONE;
    }

    if (value->IsArray()) {
        AppendType(Type::ARRAY);
        v8::Local<v8::Array> a { v8::Local<v8::Array>::Cast(value) };
        stream_.AppendValue<uint32_t>(a->Length());

        for (uint32_t i = 0; i < a->Length(); ++i) {
            SerializeError err { SerializeValue(exporter, a->Get(i), stack_level + 1) };
            if (SerializeError::NONE != err) {
                return err;
            }
        }

        return SerializeError::NONE;
    }

    if (value->IsArrayBuffer()) {
        // Neuter this array buffer and take its contents
        AppendType(Type::ARRAYBUFFER);
        v8::Local<v8::ArrayBuffer> b { v8::Local<v8::ArrayBuffer>::Cast(value) };
        if (b->IsExternal()) {
            return SerializeError::EXTERNAL_BUFFER;
        }
        v8::ArrayBuffer::Contents c { b->Externalize() };
        stream_.AppendValue<void*>(c.Data());
        stream_.AppendValue<size_t>(c.ByteLength());
        b->Neuter();

        return SerializeError::NONE;
    }

    if (value->IsArrayBufferView()) {
        return SerializeError::TYPEDARRAY_VIEW;
    }

    if (value->IsFunction()) {
        ExternalFunction* efn { exporter->AddExport(value) };
        AppendType(Type::FUNCTION);
        stream_.AppendValue<ExternalFunction*>(efn);
        return SerializeError::NONE;
    }

    // This check should be the last one
    if (value->IsObject()) {
        RT_ASSERT(isolate_);
        RT_ASSERT(isolate_->template_cache());

        v8::Local<v8::Object> obj { value->ToObject() };

        // Check if this is native object (wrapped instance)
        void* ptr { isolate_->template_cache()->GetWrapped(value) };
        if (nullptr != ptr) {
            if (allow_ref_) {
                AppendType(Type::OBJECT_REF);
                stream_.AppendValue<uint32_t>(AddRef(value));
                return SerializeError::NONE;
            } else {
                RT_ASSERT(!"not implemented");
            }
        }

        AppendType(Type::HASHMAP);
        v8::Local<v8::Array> a { obj->GetOwnPropertyNames() };
        stream_.AppendValue<uint32_t>(a->Length());
        for (uint32_t i = 0; i < a->Length(); ++i) {
            v8::Local<v8::Value> k { a->Get(i) };
            {	SerializeError err { SerializeValue(exporter, k, stack_level + 1) };
                if (SerializeError::NONE != err) {
                    return err;
                }
            }

            {	SerializeError err { SerializeValue(exporter, obj->Get(k), stack_level + 1) };
                if (SerializeError::NONE != err) {
                    return err;
                }
            }
        }

        return SerializeError::NONE;
    }

    return SerializeError::INVALID_TYPE;
}

v8::Local<v8::Value> TransportData::Unpack(Isolate* isolate) const {
    RT_ASSERT(isolate);
    v8::Isolate* iv8 { isolate->IsolateV8() };
    RT_ASSERT(iv8);

    ByteStreamReader reader(stream_);
    if (allow_ref_) {
        RT_ASSERT(nullptr != isolate_);
    }
    v8::EscapableHandleScope scope(iv8);
    return scope.Escape(UnpackValue(isolate, reader));
}

v8::Local<v8::Value> TransportData::UnpackValue(Isolate* isolate, ByteStreamReader& reader) const {
    RT_ASSERT(isolate);
    v8::Isolate* iv8 { isolate->IsolateV8() };
    RT_ASSERT(iv8);

    v8::EscapableHandleScope scope(iv8);
    Type t { ReadType(reader) };

    switch (t) {
    case Type::UNDEFINED:
        return scope.Escape<v8::Primitive>(v8::Undefined(iv8));
    case Type::NUL:
        return scope.Escape<v8::Primitive>(v8::Null(iv8));
    case Type::STRING_UTF8: {
        uint32_t len = reader.ReadValue<uint32_t>();
        return scope.Escape(v8::String::NewFromUtf8(iv8,
            reinterpret_cast<const char*>(reader.ReadBuffer(len + 1)),
            v8::String::kNormalString, len));
    }
    case Type::STRING_16: {
        uint32_t len = reader.ReadValue<uint32_t>();
        return scope.Escape(v8::String::NewFromTwoByte(iv8,
            reinterpret_cast<const uint16_t*>(reader.ReadBuffer((len + 1) * sizeof(uint16_t))),
            v8::String::kNormalString, len));
    }
    case Type::STRING_REF:
    case Type::OBJECT_REF:
        return scope.Escape(GetRef(iv8, reader.ReadValue<uint32_t>()));
    case Type::INT32:
        return scope.Escape<v8::Primitive>(v8::Int32::New(iv8,
            reader.ReadValue<int32_t>()));
    case Type::UINT32:
        return scope.Escape<v8::Primitive>(v8::Int32::New(iv8,
            reader.ReadValue<uint32_t>()));
    case Type::DOUBLE:
        return scope.Escape<v8::Primitive>(v8::Int32::New(iv8,
            reader.ReadValue<double>()));
    case Type::BOOL_TRUE:
        return scope.Escape<v8::Primitive>(v8::True(iv8));
    case Type::BOOL_FALSE:
        return scope.Escape<v8::Primitive>(v8::False(iv8));
    case Type::ARRAYBUFFER: {
        void* buf = reader.ReadValue<void*>();
        size_t len = reader.ReadValue<size_t>();
        return scope.Escape(v8::ArrayBuffer::NewNonExternal(iv8, buf, len));
    }
    case Type::ARRAY: {
        uint32_t len = reader.ReadValue<uint32_t>();
        v8::Local<v8::Array> arr { v8::Array::New(iv8, len) };
        for (uint32_t i = 0; i < len; ++i) {
            arr->Set(i, UnpackValue(isolate, reader));
        }
        return scope.Escape(arr);
    }
    case Type::HASHMAP: {
        uint32_t len = reader.ReadValue<uint32_t>();
        v8::Local<v8::Object> obj { v8::Object::New(iv8) };
        for (uint32_t i = 0; i < len; ++i) {
            v8::Local<v8::Value> k { UnpackValue(isolate, reader) };
            v8::Local<v8::Value> v { UnpackValue(isolate, reader) };
            obj->Set(k, v);
        }
        return scope.Escape(obj);
    }
    case Type::FUNCTION: {
        ExternalFunction* efn = reader.ReadValue<ExternalFunction*>();
        RT_ASSERT(isolate->template_cache());
        v8::Local<v8::Value> fnobj { isolate->template_cache()->NewWrappedFunction(efn) };
        return scope.Escape(fnobj);
    }
    default:
        RT_ASSERT(!"unknown data type");
        break;
    }

    RT_ASSERT(!"should not be here");
    return scope.Escape<v8::Primitive>(v8::Undefined(iv8));
}

} // namespace rt
<commit_msg>External function transport/serializer support<commit_after>// Copyright 2014, runtime.js project authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "transport.h"
#include <kernel/isolate.h>
#include <kernel/template-cache.h>
#include <kernel/object-wrapper.h>
#include <kernel/thread.h>

namespace rt {

TransportData::SerializeError TransportData::MoveValue(Thread* exporter,
                                                       Isolate* isolate_recv,
                                                       v8::Local<v8::Value> value) {
    RT_ASSERT(exporter);
    Isolate* isolate { exporter->isolate() };
    RT_ASSERT(isolate);
    RT_ASSERT(isolate_recv);
    Clear();
    isolate_ = isolate;
    allow_ref_ = isolate_recv == isolate;

    SerializeError err { SerializeValue(exporter, value, 1) };
    if (SerializeError::NONE != err) {
        SetUndefined();
    }

    return err;
}

TransportData::SerializeError TransportData::MoveArgs(Thread* exporter,
                                                      Isolate* isolate_recv,
                                                      const v8::FunctionCallbackInfo<v8::Value>& args) {
    RT_ASSERT(exporter);
    Isolate* isolate { exporter->isolate() };
    RT_ASSERT(isolate);
    RT_ASSERT(isolate_recv);
    Clear();
    isolate_ = isolate;
    allow_ref_ = isolate_recv == isolate;

    AppendType(Type::ARRAY);
    uint32_t len = args.Length();
    stream_.AppendValue<uint32_t>(len);

    for (uint32_t i = 0; i < len; ++i) {
        SerializeError err { SerializeValue(exporter, args[i], 1) };
        if (SerializeError::NONE != err) {
            SetUndefined();
            return err;
        }
    }

    return SerializeError::NONE;
}

v8::Local<v8::Value> TransportData::GetRef(v8::Isolate* iv8, uint32_t index) const {
    RT_ASSERT(allow_ref_);
    RT_ASSERT(iv8);
    RT_ASSERT(isolate_);
    RT_ASSERT(isolate_->IsolateV8());
    RT_ASSERT(isolate_->IsolateV8() == iv8);
    RT_ASSERT(index < refs_.size());
    v8::EscapableHandleScope scope(iv8);
    return scope.Escape(v8::Local<v8::Value>::New(iv8, refs_[index]));
}

uint32_t TransportData::AddRef(v8::Local<v8::Value> value) {
    RT_ASSERT(allow_ref_);
    RT_ASSERT(isolate_);
    RT_ASSERT(isolate_->IsolateV8());
    size_t index = refs_.size();
    refs_.push_back(std::move(v8::UniquePersistent<v8::Value>(isolate_->IsolateV8(), value)));
    return static_cast<uint32_t>(index);
}

TransportData::SerializeError TransportData::SerializeValue(Thread* exporter,
                                                            v8::Local<v8::Value> value,
                                                            uint32_t stack_level) {
    RT_ASSERT(exporter);
    RT_ASSERT(!value.IsEmpty());

    if (stack_level > kMaxStackSize) {
        return SerializeError::MAX_STACK;
    }

    if (value->IsUndefined()) {
        AppendType(Type::UNDEFINED);
        return SerializeError::NONE;
    }

    if (value->IsNull()) {
        AppendType(Type::NUL);
        return SerializeError::NONE;
    }

    if (value->IsBoolean()) {
        AppendType(value->BooleanValue() ? Type::BOOL_TRUE : Type::BOOL_FALSE);
        return SerializeError::NONE;
    }

    if (value->IsInt32()) {
        AppendType(Type::INT32);
        stream_.AppendValue<int32_t>(value->Int32Value());
        return SerializeError::NONE;
    }

    if (value->IsUint32()) {
        AppendType(Type::UINT32);
        stream_.AppendValue<uint32_t>(value->Uint32Value());
        return SerializeError::NONE;
    }

    if (value->IsNumber()) {
        AppendType(Type::DOUBLE);
        stream_.AppendValue<double>(value->NumberValue());
        return SerializeError::NONE;
    }

    if (value->IsString()) {
        if (allow_ref_) {
            // Strings are immutable, its safe to pass by reference
            // for same-isolate calls
            AppendType(Type::STRING_REF);
            stream_.AppendValue<uint32_t>(AddRef(value));
        } else {
            AppendType(Type::STRING_16);
            v8::Local<v8::String> s { value->ToString() };
            int len = s->Length();
            RT_ASSERT(len >= 0);
            stream_.AppendValue<uint32_t>(len);
            void* place { stream_.AppendBuffer((len + 1) * sizeof(uint16_t)) };
            s->Write(reinterpret_cast<uint16_t*>(place), 0, len);
        }

        return SerializeError::NONE;
    }

    if (value->IsArray()) {
        AppendType(Type::ARRAY);
        v8::Local<v8::Array> a { v8::Local<v8::Array>::Cast(value) };
        stream_.AppendValue<uint32_t>(a->Length());

        for (uint32_t i = 0; i < a->Length(); ++i) {
            SerializeError err { SerializeValue(exporter, a->Get(i), stack_level + 1) };
            if (SerializeError::NONE != err) {
                return err;
            }
        }

        return SerializeError::NONE;
    }

    if (value->IsArrayBuffer()) {
        // Neuter this array buffer and take its contents
        AppendType(Type::ARRAYBUFFER);
        v8::Local<v8::ArrayBuffer> b { v8::Local<v8::ArrayBuffer>::Cast(value) };
        if (b->IsExternal()) {
            return SerializeError::EXTERNAL_BUFFER;
        }
        v8::ArrayBuffer::Contents c { b->Externalize() };
        stream_.AppendValue<void*>(c.Data());
        stream_.AppendValue<size_t>(c.ByteLength());
        b->Neuter();

        return SerializeError::NONE;
    }

    if (value->IsArrayBufferView()) {
        return SerializeError::TYPEDARRAY_VIEW;
    }

    if (value->IsFunction()) {
        ExternalFunction* efn { exporter->AddExport(value) };
        AppendType(Type::FUNCTION);
        stream_.AppendValue<ExternalFunction*>(efn);
        return SerializeError::NONE;
    }

    // This condition check should be the last one
    if (value->IsObject()) {
        RT_ASSERT(isolate_);
        RT_ASSERT(isolate_->template_cache());

        v8::Local<v8::Object> obj { value->ToObject() };
        NativeObjectWrapper* ptr { isolate_->template_cache()->GetWrapped(value) };

        // If current object is wrapped native
        if (nullptr != ptr) {
            switch (ptr->type_id()) {
            case NativeTypeId::TYPEID_FUNCTION: {
                ExternalFunction* efn { static_cast<ExternalFunction*>(ptr) };
                AppendType(Type::FUNCTION);
                stream_.AppendValue<ExternalFunction*>(efn);
                return SerializeError::NONE;
            }
            default:
                break;
            }

            if (allow_ref_) {
                AppendType(Type::OBJECT_REF);
                stream_.AppendValue<uint32_t>(AddRef(value));
                return SerializeError::NONE;
            } else {
                RT_ASSERT(!"not implemented");
            }
        }

        AppendType(Type::HASHMAP);
        v8::Local<v8::Array> a { obj->GetOwnPropertyNames() };
        stream_.AppendValue<uint32_t>(a->Length());
        for (uint32_t i = 0; i < a->Length(); ++i) {
            v8::Local<v8::Value> k { a->Get(i) };
            {	SerializeError err { SerializeValue(exporter, k, stack_level + 1) };
                if (SerializeError::NONE != err) {
                    return err;
                }
            }

            {	SerializeError err { SerializeValue(exporter, obj->Get(k), stack_level + 1) };
                if (SerializeError::NONE != err) {
                    return err;
                }
            }
        }

        return SerializeError::NONE;
    }

    return SerializeError::INVALID_TYPE;
}

v8::Local<v8::Value> TransportData::Unpack(Isolate* isolate) const {
    RT_ASSERT(isolate);
    v8::Isolate* iv8 { isolate->IsolateV8() };
    RT_ASSERT(iv8);

    ByteStreamReader reader(stream_);
    if (allow_ref_) {
        RT_ASSERT(nullptr != isolate_);
    }
    v8::EscapableHandleScope scope(iv8);
    return scope.Escape(UnpackValue(isolate, reader));
}

v8::Local<v8::Value> TransportData::UnpackValue(Isolate* isolate, ByteStreamReader& reader) const {
    RT_ASSERT(isolate);
    v8::Isolate* iv8 { isolate->IsolateV8() };
    RT_ASSERT(iv8);

    v8::EscapableHandleScope scope(iv8);
    Type t { ReadType(reader) };

    switch (t) {
    case Type::UNDEFINED:
        return scope.Escape<v8::Primitive>(v8::Undefined(iv8));
    case Type::NUL:
        return scope.Escape<v8::Primitive>(v8::Null(iv8));
    case Type::STRING_UTF8: {
        uint32_t len = reader.ReadValue<uint32_t>();
        return scope.Escape(v8::String::NewFromUtf8(iv8,
            reinterpret_cast<const char*>(reader.ReadBuffer(len + 1)),
            v8::String::kNormalString, len));
    }
    case Type::STRING_16: {
        uint32_t len = reader.ReadValue<uint32_t>();
        return scope.Escape(v8::String::NewFromTwoByte(iv8,
            reinterpret_cast<const uint16_t*>(reader.ReadBuffer((len + 1) * sizeof(uint16_t))),
            v8::String::kNormalString, len));
    }
    case Type::STRING_REF:
    case Type::OBJECT_REF:
        return scope.Escape(GetRef(iv8, reader.ReadValue<uint32_t>()));
    case Type::INT32:
        return scope.Escape<v8::Primitive>(v8::Int32::New(iv8,
            reader.ReadValue<int32_t>()));
    case Type::UINT32:
        return scope.Escape<v8::Primitive>(v8::Int32::New(iv8,
            reader.ReadValue<uint32_t>()));
    case Type::DOUBLE:
        return scope.Escape<v8::Primitive>(v8::Int32::New(iv8,
            reader.ReadValue<double>()));
    case Type::BOOL_TRUE:
        return scope.Escape<v8::Primitive>(v8::True(iv8));
    case Type::BOOL_FALSE:
        return scope.Escape<v8::Primitive>(v8::False(iv8));
    case Type::ARRAYBUFFER: {
        void* buf = reader.ReadValue<void*>();
        size_t len = reader.ReadValue<size_t>();
        return scope.Escape(v8::ArrayBuffer::NewNonExternal(iv8, buf, len));
    }
    case Type::ARRAY: {
        uint32_t len = reader.ReadValue<uint32_t>();
        v8::Local<v8::Array> arr { v8::Array::New(iv8, len) };
        for (uint32_t i = 0; i < len; ++i) {
            arr->Set(i, UnpackValue(isolate, reader));
        }
        return scope.Escape(arr);
    }
    case Type::HASHMAP: {
        uint32_t len = reader.ReadValue<uint32_t>();
        v8::Local<v8::Object> obj { v8::Object::New(iv8) };
        for (uint32_t i = 0; i < len; ++i) {
            v8::Local<v8::Value> k { UnpackValue(isolate, reader) };
            v8::Local<v8::Value> v { UnpackValue(isolate, reader) };
            obj->Set(k, v);
        }
        return scope.Escape(obj);
    }
    case Type::FUNCTION: {
        ExternalFunction* efn = reader.ReadValue<ExternalFunction*>();
        RT_ASSERT(isolate->template_cache());
        v8::Local<v8::Value> fnobj { isolate->template_cache()->NewWrappedFunction(efn) };
        return scope.Escape(fnobj);
    }
    default:
        RT_ASSERT(!"unknown data type");
        break;
    }

    RT_ASSERT(!"should not be here");
    return scope.Escape<v8::Primitive>(v8::Undefined(iv8));
}

} // namespace rt
<|endoftext|>


<commit_before>// Copyright (c) 2016, LAAS-CNRS
// Authors: Pierre Fernbach (pierre.fernbach@laas.fr)
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core 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
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/util/debug.hh>
#include <hpp/model/device.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/kinodynamic-path.hh>
#include <hpp/core/projection-error.hh>

namespace hpp {
  namespace core {
    KinodynamicPath::KinodynamicPath (const DevicePtr_t& device,
                                      ConfigurationIn_t init,
                                      ConfigurationIn_t end,
                                      value_type length, ConfigurationIn_t a1,ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim) :
      parent_t (device,init,end,length),
      a1_(a1),t0_(t0),t1_(t1),tv_(tv),t2_(t2),vLim_(vLim)
    {
      assert (device);
      assert (length >= 0);
      assert (!constraints ());
      hppDout(notice,"Create kinodynamic path with values : ");
      hppDout(notice,"a1 = "<<model::displayConfig(a1_));
      hppDout(notice,"t0 = "<<model::displayConfig(t0_));
      hppDout(notice,"t1 = "<<model::displayConfig(t1_));
      hppDout(notice,"tv = "<<model::displayConfig(tv_));
      hppDout(notice,"t2 = "<<model::displayConfig(t2_));
      hppDout(notice,"length = "<<length);
      hppDout(notice,"vLim = "<<model::displayConfig(vLim_));

      // for now, this class only deal with the translation part of a freeflyer :
      assert(a1.size()==3 && t0.size()==3 && t1.size()==3 && tv.size()==3 && t2.size()==3 && vLim.size()==3 && "Inputs vector of kinodynamicPath are not of size 3");
      for(size_t i = 3 ; i < 3 ; i++){
        assert(fabs(length - (t0[i] + t1[i] + tv[i] + t2[i])) < std::numeric_limits <float>::epsilon ()
               && "Kinodynamic path : length is not coherent with switch times");
      }

    }

    KinodynamicPath::KinodynamicPath (const DevicePtr_t& device,
                                      ConfigurationIn_t init,
                                      ConfigurationIn_t end,
                                      value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim,
                                      ConstraintSetPtr_t constraints) :
      parent_t (device,init,end,length,constraints),
      a1_(a1),t0_(t0),t1_(t1),tv_(tv),t2_(t2),vLim_(vLim)
    {
      assert (device);
      assert (length >= 0);
      hppDout(notice,"Create kinodynamic path with constraints, with values : ");
      hppDout(notice,"a1 = "<<model::displayConfig(a1_));
      hppDout(notice,"t0 = "<<model::displayConfig(t0_));
      hppDout(notice,"t1 = "<<model::displayConfig(t1_));
      hppDout(notice,"tv = "<<model::displayConfig(tv_));
      hppDout(notice,"t2 = "<<model::displayConfig(t2_));
      hppDout(notice,"length = "<<length);
      hppDout(notice,"vLim = "<<model::displayConfig(vLim_));

      // for now, this class only deal with the translation part of a freeflyer :
      assert(a1.size()==3 && t0.size()==3 && t1.size()==3 && tv.size()==3 && t2.size()==3 && vLim.size()==3 && "Inputs vector of kinodynamicPath are not of size 3");
      for(size_t i = 3 ; i < 3 ; i++){
        assert(fabs(length - (t0[i] + t1[i] + tv[i] + t2[i])) < std::numeric_limits <float>::epsilon ()
               && "Kinodynamic path : length is not coherent with switch times");
      }

    }

    KinodynamicPath::KinodynamicPath (const KinodynamicPath& path) :
      parent_t (path),a1_(path.a1_),t0_(path.t0_),t1_(path.t1_),tv_(path.tv_),t2_(path.t2_),vLim_(path.vLim_)
    {
    }

    KinodynamicPath::KinodynamicPath (const KinodynamicPath& path,
                                      const ConstraintSetPtr_t& constraints) :
      parent_t (path, constraints),
      a1_(path.a1_),t0_(path.t0_),t1_(path.t1_),tv_(path.tv_),t2_(path.t2_),vLim_(path.vLim_)
    {
      assert (constraints->apply (initial_));
      assert (constraints->apply (end_));
      assert (constraints->isSatisfied (initial_));
      assert (constraints->isSatisfied (end_));
    }

    bool KinodynamicPath::impl_compute (ConfigurationOut_t result,
                                        value_type t) const
    {
      assert (result.size() == device()->configSize());

      if (t == timeRange ().first || timeRange ().second == 0) {
        result = initial_;
        return true;
      }
      if (t == timeRange ().second) {
        result = end_;
        return true;
      }

      size_type configSize = device()->configSize() - device()->extraConfigSpace().dimension ();
      // const JointVector_t& jv (device()->getJointVector ());
      double v2,t2,t1,tv;
      size_type indexVel;
      size_type indexAcc;
      // straight path for all the joints, except the translations of the base :
      value_type u = t/timeRange ().second;
      if (timeRange ().second == 0)
        u = 0;

      model::interpolate (device_, initial_, end_, u, result);
      //hppDout(notice,"path : initial = "<<model::displayConfig(initial_));

      for(int id = 0 ; id < 3 ; id++){ // FIX ME : only work for freeflyer (translation part)
      //for (model::JointVector_t::const_iterator itJoint = jv.begin (); itJoint != jv.end (); itJoint++) {
        // size_type id = (*itJoint)->rankInConfiguration ();
        // size_type indexVel = (*itJoint)->rankInVelocity() + configSize;
        indexVel = id + configSize;
        indexAcc = id + configSize + 3;

       // hppDout(notice," PATH For joint "<<(*itJoint)->name());
       // hppDout(notice,"PATH for joint :"<<device()->getJointAtConfigRank(id)->name());
        if(device()->getJointAtConfigRank(id)->name() != "base_joint_SO3"){

          //if((*itJoint)->configSize() >= 1){
          // 3 case (each segment of the trajectory) :
          if(t <= t0_[id]){ // before first segment
            result[id] = initial_[id] + t*initial_[indexVel];
            result[indexVel] = initial_[indexVel];
            result[indexAcc] = 0;
          }
          else if(t <= (t0_[id] + t1_[id])){
            //  hppDout(info,"on  1° segment");
            t1 = t - t0_[id];
            result[id] = 0.5*t1*t1*a1_[id] + t1*initial_[indexVel] + initial_[id] + t0_[id]*initial_[indexVel];
            result[indexVel] = t1*a1_[id] + initial_[indexVel];
            result[indexAcc] = a1_[id];
          }else if (t <= (t0_[id] + t1_[id] + tv_[id]) ){
            //  hppDout(info,"on  constant velocity segment");
            tv = t - t0_[id] - t1_[id];
            result[id] = 0.5*t1_[id]*t1_[id]*a1_[id] + t1_[id]*initial_[indexVel] + initial_[id] + t0_[id]*initial_[indexVel] + (tv)*vLim_[id];
            result[indexVel] = vLim_[id];
            result[indexAcc] = 0.;

          }else if (t <= (t0_[id] + t1_[id] + tv_[id] +t2_[id]) ){
          //  hppDout(info,"on  3° segment");
            t2 = t - tv_[id] - t1_[id] - t0_[id];
            if(tv_[id] > 0 )
              v2 = vLim_[id];
            else
              v2 = t1_[id]*a1_[id] + initial_[indexVel];
            result[id] = 0.5*t1_[id]*t1_[id]*a1_[id] + t1_[id]*initial_[indexVel] + initial_[id] + t0_[id]*initial_[indexVel] + tv_[id]*vLim_[id] - 0.5*t2*t2*a1_[id] + t2*v2;
            result[indexVel] = v2 - t2 * a1_[id];
            result[indexAcc] = -a1_[id];

          }else{ // after last segment
            result[id] = end_[id];
            result[indexVel] = end_[indexVel];
            result[indexAcc] = 0;
          }
        }// if not quaternion joint

     // }// if joint config size > 1

     }// for all joints

      return true;
    }

    PathPtr_t KinodynamicPath::extract (const interval_t& subInterval) const
    throw (projection_error)
    {
      // Length is assumed to be proportional to interval range
      value_type l = fabs (subInterval.second - subInterval.first);
      hppDout(notice,"%% EXTRACT PATH :");
      bool success;
      Configuration_t q1 ((*this) (subInterval.first, success));
      if (!success) throw projection_error
          ("Failed to apply constraints in KinodynamicPath::extract");
      Configuration_t q2 ((*this) (subInterval.second, success));
      // set acceleration to 0 for initial and end config :
      size_type configSize = device()->configSize() - device()->extraConfigSpace().dimension ();
      q1[configSize+3] = 0.0;
      q1[configSize+4] = 0.0;
      q1[configSize+5] = 0.0;
      q2[configSize+3] = 0.0;
      q2[configSize+4] = 0.0;
      q2[configSize+5] = 0.0;
      hppDout(info,"from : ");
      hppDout(info,"q1 = "<<model::displayConfig(initial_));
      hppDout(info,"q2 = "<<model::displayConfig(end_));
      hppDout(info,"to : ");
      hppDout(info,"q1 = "<<model::displayConfig(q1));
      hppDout(info,"q2 = "<<model::displayConfig(q2));
      if (!success) throw projection_error
          ("Failed to apply constraints in KinodynamicPath::extract");

      if(subInterval.first > subInterval.second){  // reversed path
        hppDout(notice,"%% REVERSE PATH, not implemented yet !");
        std::cout<<"ERROR, you shouldn't call reverse() on a kinodynamic path"<<std::endl;
        return PathPtr_t();
      }
      double ti,tf,oldT0,oldT2,oldT1,oldTv;
      hppDout(notice,"%% subinterval PATH");
      // new timebounds
      Configuration_t t0(t0_);
      Configuration_t t1(t1_);
      Configuration_t t2(t2_);
      Configuration_t tv(tv_);
      Configuration_t a1(a1_);

      for(int i = 0 ; i < a1_.size() ; ++i){ // adjust times bounds
        ti = subInterval.first - timeRange_.first;
        tf = timeRange_.second - subInterval.second;
        t0[i] = t0_[i] - ti;
        if(t0[i] <= 0){
          t0[i] = 0;
          ti = ti - t0_[i];
          t1[i] = t1_[i] - ti;
          if(t1[i] <= 0){
            t1[i] = 0;
            ti = ti - t1_[i];
            tv[i] = tv_[i] - ti;
            if(tv[i] <= 0 ){
              tv[i] = 0;
              ti = ti - tv_[i];
              t2[i] = t2_[i] - ti;
              if(t2[i] <= 0)
                t2[i] = 0;
            }
          }
        }
        oldT0 = t0[i];
        oldT1 = t1[i];
        oldT2 = t2[i];
        oldTv = tv[i];
        t2[i] = oldT2 - tf;
        if(t2[i] <= 0 ){
          t2[i] = 0 ;
          tf = tf - oldT2;
          tv[i] = oldTv - tf;
          if(tv[i] <= 0){
            tv[i] = 0;
            tf = tf - oldTv;
            t1[i] = oldT1 - tf;
            if(t1[i] <= 0 ){
              t1[i] = 0;
              tf = tf - oldT1;
              t0[i] = oldT0 - tf;
            }
          }
        }

      } // for all joints
      PathPtr_t result = KinodynamicPath::create (device_, q1, q2, l,a1,t0,t1,tv,t2,vLim_,
                                                  constraints ());
      return result;
    }

  } //   namespace core
} // namespace hpp

<commit_msg>If kindoynamicPath::extract is called with the same interval, automatically return the same object<commit_after>// Copyright (c) 2016, LAAS-CNRS
// Authors: Pierre Fernbach (pierre.fernbach@laas.fr)
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core 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
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/util/debug.hh>
#include <hpp/model/device.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/kinodynamic-path.hh>
#include <hpp/core/projection-error.hh>

namespace hpp {
  namespace core {
    KinodynamicPath::KinodynamicPath (const DevicePtr_t& device,
                                      ConfigurationIn_t init,
                                      ConfigurationIn_t end,
                                      value_type length, ConfigurationIn_t a1,ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim) :
      parent_t (device,init,end,length),
      a1_(a1),t0_(t0),t1_(t1),tv_(tv),t2_(t2),vLim_(vLim)
    {
      assert (device);
      assert (length >= 0);
      assert (!constraints ());
      hppDout(notice,"Create kinodynamic path with values : ");
      hppDout(notice,"a1 = "<<model::displayConfig(a1_));
      hppDout(notice,"t0 = "<<model::displayConfig(t0_));
      hppDout(notice,"t1 = "<<model::displayConfig(t1_));
      hppDout(notice,"tv = "<<model::displayConfig(tv_));
      hppDout(notice,"t2 = "<<model::displayConfig(t2_));
      hppDout(notice,"length = "<<length);
      hppDout(notice,"vLim = "<<model::displayConfig(vLim_));

      // for now, this class only deal with the translation part of a freeflyer :
      assert(a1.size()==3 && t0.size()==3 && t1.size()==3 && tv.size()==3 && t2.size()==3 && vLim.size()==3 && "Inputs vector of kinodynamicPath are not of size 3");
      for(size_t i = 3 ; i < 3 ; i++){
        assert(fabs(length - (t0[i] + t1[i] + tv[i] + t2[i])) < std::numeric_limits <float>::epsilon ()
               && "Kinodynamic path : length is not coherent with switch times");
      }

    }

    KinodynamicPath::KinodynamicPath (const DevicePtr_t& device,
                                      ConfigurationIn_t init,
                                      ConfigurationIn_t end,
                                      value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim,
                                      ConstraintSetPtr_t constraints) :
      parent_t (device,init,end,length,constraints),
      a1_(a1),t0_(t0),t1_(t1),tv_(tv),t2_(t2),vLim_(vLim)
    {
      assert (device);
      assert (length >= 0);
      hppDout(notice,"Create kinodynamic path with constraints, with values : ");
      hppDout(notice,"a1 = "<<model::displayConfig(a1_));
      hppDout(notice,"t0 = "<<model::displayConfig(t0_));
      hppDout(notice,"t1 = "<<model::displayConfig(t1_));
      hppDout(notice,"tv = "<<model::displayConfig(tv_));
      hppDout(notice,"t2 = "<<model::displayConfig(t2_));
      hppDout(notice,"length = "<<length);
      hppDout(notice,"vLim = "<<model::displayConfig(vLim_));

      // for now, this class only deal with the translation part of a freeflyer :
      assert(a1.size()==3 && t0.size()==3 && t1.size()==3 && tv.size()==3 && t2.size()==3 && vLim.size()==3 && "Inputs vector of kinodynamicPath are not of size 3");
      for(size_t i = 3 ; i < 3 ; i++){
        assert(fabs(length - (t0[i] + t1[i] + tv[i] + t2[i])) < std::numeric_limits <float>::epsilon ()
               && "Kinodynamic path : length is not coherent with switch times");
      }

    }

    KinodynamicPath::KinodynamicPath (const KinodynamicPath& path) :
      parent_t (path),a1_(path.a1_),t0_(path.t0_),t1_(path.t1_),tv_(path.tv_),t2_(path.t2_),vLim_(path.vLim_)
    {
    }

    KinodynamicPath::KinodynamicPath (const KinodynamicPath& path,
                                      const ConstraintSetPtr_t& constraints) :
      parent_t (path, constraints),
      a1_(path.a1_),t0_(path.t0_),t1_(path.t1_),tv_(path.tv_),t2_(path.t2_),vLim_(path.vLim_)
    {
      assert (constraints->apply (initial_));
      assert (constraints->apply (end_));
      assert (constraints->isSatisfied (initial_));
      assert (constraints->isSatisfied (end_));
    }

    bool KinodynamicPath::impl_compute (ConfigurationOut_t result,
                                        value_type t) const
    {
      assert (result.size() == device()->configSize());

      if (t == timeRange ().first || timeRange ().second == 0) {
        result = initial_;
        return true;
      }
      if (t == timeRange ().second) {
        result = end_;
        return true;
      }

      size_type configSize = device()->configSize() - device()->extraConfigSpace().dimension ();
      // const JointVector_t& jv (device()->getJointVector ());
      double v2,t2,t1,tv;
      size_type indexVel;
      size_type indexAcc;
      // straight path for all the joints, except the translations of the base :
      value_type u = t/timeRange ().second;
      if (timeRange ().second == 0)
        u = 0;

      model::interpolate (device_, initial_, end_, u, result);
      //hppDout(notice,"path : initial = "<<model::displayConfig(initial_));

      for(int id = 0 ; id < 3 ; id++){ // FIX ME : only work for freeflyer (translation part)
      //for (model::JointVector_t::const_iterator itJoint = jv.begin (); itJoint != jv.end (); itJoint++) {
        // size_type id = (*itJoint)->rankInConfiguration ();
        // size_type indexVel = (*itJoint)->rankInVelocity() + configSize;
        indexVel = id + configSize;
        indexAcc = id + configSize + 3;

       // hppDout(notice," PATH For joint "<<(*itJoint)->name());
       // hppDout(notice,"PATH for joint :"<<device()->getJointAtConfigRank(id)->name());
        if(device()->getJointAtConfigRank(id)->name() != "base_joint_SO3"){

          //if((*itJoint)->configSize() >= 1){
          // 3 case (each segment of the trajectory) :
          if(t <= t0_[id]){ // before first segment
            result[id] = initial_[id] + t*initial_[indexVel];
            result[indexVel] = initial_[indexVel];
            result[indexAcc] = 0;
          }
          else if(t <= (t0_[id] + t1_[id])){
            //  hppDout(info,"on  1° segment");
            t1 = t - t0_[id];
            result[id] = 0.5*t1*t1*a1_[id] + t1*initial_[indexVel] + initial_[id] + t0_[id]*initial_[indexVel];
            result[indexVel] = t1*a1_[id] + initial_[indexVel];
            result[indexAcc] = a1_[id];
          }else if (t <= (t0_[id] + t1_[id] + tv_[id]) ){
            //  hppDout(info,"on  constant velocity segment");
            tv = t - t0_[id] - t1_[id];
            result[id] = 0.5*t1_[id]*t1_[id]*a1_[id] + t1_[id]*initial_[indexVel] + initial_[id] + t0_[id]*initial_[indexVel] + (tv)*vLim_[id];
            result[indexVel] = vLim_[id];
            result[indexAcc] = 0.;

          }else if (t <= (t0_[id] + t1_[id] + tv_[id] +t2_[id]) ){
          //  hppDout(info,"on  3° segment");
            t2 = t - tv_[id] - t1_[id] - t0_[id];
            if(tv_[id] > 0 )
              v2 = vLim_[id];
            else
              v2 = t1_[id]*a1_[id] + initial_[indexVel];
            result[id] = 0.5*t1_[id]*t1_[id]*a1_[id] + t1_[id]*initial_[indexVel] + initial_[id] + t0_[id]*initial_[indexVel] + tv_[id]*vLim_[id] - 0.5*t2*t2*a1_[id] + t2*v2;
            result[indexVel] = v2 - t2 * a1_[id];
            result[indexAcc] = -a1_[id];

          }else{ // after last segment
            result[id] = end_[id];
            result[indexVel] = end_[indexVel];
            result[indexAcc] = 0;
          }
        }// if not quaternion joint

     // }// if joint config size > 1

     }// for all joints

      return true;
    }

    PathPtr_t KinodynamicPath::extract (const interval_t& subInterval) const
    throw (projection_error)
    {
      // Length is assumed to be proportional to interval range
      if(subInterval.first == timeRange_.first && subInterval.second == timeRange_.second){
        hppDout(notice,"Call extract with same interval");
        return weak_.lock();
      }
      value_type l = fabs (subInterval.second - subInterval.first);
      hppDout(notice,"%% EXTRACT PATH :");
      bool success;
      Configuration_t q1 ((*this) (subInterval.first, success));
      if (!success) throw projection_error
          ("Failed to apply constraints in KinodynamicPath::extract");
      Configuration_t q2 ((*this) (subInterval.second, success));
      // set acceleration to 0 for initial and end config :
      size_type configSize = device()->configSize() - device()->extraConfigSpace().dimension ();
      q1[configSize+3] = 0.0;
      q1[configSize+4] = 0.0;
      q1[configSize+5] = 0.0;
      q2[configSize+3] = 0.0;
      q2[configSize+4] = 0.0;
      q2[configSize+5] = 0.0;
      hppDout(info,"from : ");
      hppDout(info,"q1 = "<<model::displayConfig(initial_));
      hppDout(info,"q2 = "<<model::displayConfig(end_));
      hppDout(info,"to : ");
      hppDout(info,"q1 = "<<model::displayConfig(q1));
      hppDout(info,"q2 = "<<model::displayConfig(q2));
      if (!success) throw projection_error
          ("Failed to apply constraints in KinodynamicPath::extract");

      if(subInterval.first > subInterval.second){  // reversed path
        hppDout(notice,"%% REVERSE PATH, not implemented yet !");
        std::cout<<"ERROR, you shouldn't call reverse() on a kinodynamic path"<<std::endl;
        return PathPtr_t();
      }
      double ti,tf,oldT0,oldT2,oldT1,oldTv;
      hppDout(notice,"%% subinterval PATH");
      // new timebounds
      Configuration_t t0(t0_);
      Configuration_t t1(t1_);
      Configuration_t t2(t2_);
      Configuration_t tv(tv_);
      Configuration_t a1(a1_);

      for(int i = 0 ; i < a1_.size() ; ++i){ // adjust times bounds
        ti = subInterval.first - timeRange_.first;
        tf = timeRange_.second - subInterval.second;
        t0[i] = t0_[i] - ti;
        if(t0[i] <= 0){
          t0[i] = 0;
          ti = ti - t0_[i];
          t1[i] = t1_[i] - ti;
          if(t1[i] <= 0){
            t1[i] = 0;
            ti = ti - t1_[i];
            tv[i] = tv_[i] - ti;
            if(tv[i] <= 0 ){
              tv[i] = 0;
              ti = ti - tv_[i];
              t2[i] = t2_[i] - ti;
              if(t2[i] <= 0)
                t2[i] = 0;
            }
          }
        }
        oldT0 = t0[i];
        oldT1 = t1[i];
        oldT2 = t2[i];
        oldTv = tv[i];
        t2[i] = oldT2 - tf;
        if(t2[i] <= 0 ){
          t2[i] = 0 ;
          tf = tf - oldT2;
          tv[i] = oldTv - tf;
          if(tv[i] <= 0){
            tv[i] = 0;
            tf = tf - oldTv;
            t1[i] = oldT1 - tf;
            if(t1[i] <= 0 ){
              t1[i] = 0;
              tf = tf - oldT1;
              t0[i] = oldT0 - tf;
            }
          }
        }

      } // for all joints
      PathPtr_t result = KinodynamicPath::create (device_, q1, q2, l,a1,t0,t1,tv,t2,vLim_,
                                                  constraints ());
      return result;
    }

  } //   namespace core
} // namespace hpp

<|endoftext|>