<commit_before>///
/// @file  pi_lmo_parallel.cpp
/// @brief Parallel implementation of the Lagarias-Miller-Odlyzko
///        prime counting algorithm using OpenMP. This implementation
///        uses load balancing to ensure all threads are kept busy
///        till the very end. This implementation also does not use a
///        special tree data structure (a.k.a. Fenwick tree) for
///        counting the number of unsieved elements but instead counts
///        the number of unsieved elements directly from the sieve
///        array using the POPCNT instruction which is much faster.
///
///        Lagarias-Miller-Odlyzko formula:
///        pi(x) = pi(y) + S1(x, a) + S2(x, a) - 1 - P2(x, a)
///        with y = x^(1/3), a = pi(y)
///
///        This implementation is based on the paper:
///        Tomás Oliveira e Silva, Computing pi(x): the combinatorial
///        method, Revista do DETUA, vol. 4, no. 6, March 2006,
///        pp. 759-768.
///
/// Copyright (C) 2021 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <primecount-internal.hpp>
#include <Sieve.hpp>
#include <generate.hpp>
#include <generate_phi.hpp>
#include <LoadBalancer.hpp>
#include <min.hpp>
#include <imath.hpp>
#include <PhiTiny.hpp>
#include <PiTable.hpp>
#include <print.hpp>
#include <S.hpp>

#include <stdint.h>
#include <vector>

using namespace std;
using namespace primecount;

namespace {

/// Compute the S2 contribution of the interval
/// [low, low + segments * segment_size[.
///
int64_t S2_thread(int64_t x,
                  int64_t y,
                  int64_t z,
                  int64_t c,
                  const PiTable& pi,
                  const vector<int32_t>& primes,
                  const vector<int32_t>& lpf,
                  const vector<int32_t>& mu,
                  ThreadSettings& thread)
{
  int64_t sum = 0;
  int64_t low = thread.low;
  int64_t low1 = max(low, 1);
  int64_t segments = thread.segments;
  int64_t segment_size = thread.segment_size;
  int64_t limit = min(low + segments * segment_size, z + 1);
  int64_t max_b = pi[min(isqrt(x / low1), y - 1)];
  int64_t pi_sqrty = pi[isqrt(y)];

  if (c >= max_b)
    return 0;

  Sieve sieve(low, segment_size, max_b);
  auto phi = generate_phi(low, max_b, primes, pi);
  thread.init_finished();

  // segmented sieve of Eratosthenes
  for (; low < limit; low += segment_size)
  {
    // current segment [low, high[
    int64_t high = min(low + segment_size, limit);
    low1 = max(low, 1);

    sieve.pre_sieve(primes, c, low, high);
    int64_t b = c + 1;

    // For c + 1 <= b <= pi_sqrty
    // Find all special leaves in the current segment that are
    // composed of a prime and a square free number:
    // low <= x / (primes[b] * m) < high
    for (; b <= min(pi_sqrty, max_b); b++)
    {
      int64_t prime = primes[b];
      int64_t min_m = max(x / (prime * high), y / prime);
      int64_t max_m = min(x / (prime * low1), y);

      if (prime >= max_m)
        goto next_segment;

      for (int64_t m = max_m; m > min_m; m--)
      {
        if (mu[m] != 0 && prime < lpf[m])
        {
          int64_t xpm = x / (prime * m);
          int64_t stop = xpm - low;
          int64_t phi_xpm = phi[b] + sieve.count(stop);
          sum -= mu[m] * phi_xpm;
        }
      }

      phi[b] += sieve.get_total_count();
      sieve.cross_off_count(prime, b);
    }

    // For pi_sqrty < b < pi_y
    // Find all special leaves in the current segment
    // that are composed of 2 primes:
    // low <= x / (primes[b] * primes[l]) < high
    for (; b <= max_b; b++)
    {
      int64_t prime = primes[b];
      int64_t l = pi[min(x / (prime * low1), y)];
      int64_t min_m = max(x / (prime * high), prime);

      if (prime >= primes[l])
        goto next_segment;

      for (; primes[l] > min_m; l--)
      {
        int64_t xpq = x / (prime * primes[l]);
        int64_t stop = xpq - low;
        int64_t phi_xpq = phi[b] + sieve.count(stop);
        sum += phi_xpq;
      }

      phi[b] += sieve.get_total_count();
      sieve.cross_off_count(prime, b);
    }

    next_segment:;
  }

  return sum;
}

/// Calculate the contribution of thes pecial leaves.
///
/// This is a parallel S2(x, y) implementation with advanced load
/// balancing. As most special leaves tend to be in the first segments
/// we start off with a tiny segment size and one segment per thread.
/// After each iteration we dynamically increase the segment size (until
/// it reaches some limit) or the number of segments.
///
/// S2(x, y) has been parallelized using an idea devised by Xavier
/// Gourdon. The idea is to make the individual threads completely
/// independent from each other so that no thread depends on values
/// calculated by another thread. The benefit of this approach is that
/// the algorithm will scale well up to a very large number of CPU
/// cores. In order to make the threads independent from each other
/// each thread needs to precompute a lookup table of phi(x, a) values
/// (this is done in S2_thread(x, y)) every time the thread starts a
/// new computation.
///
int64_t S2(int64_t x,
           int64_t y,
           int64_t z,
           int64_t c,
           int64_t s2_approx,
           const vector<int32_t>& primes,
           const vector<int32_t>& lpf,
           const vector<int32_t>& mu,
           int threads)
{
  print("");
  print("=== S2(x, y) ===");

  double time = get_time();
  int64_t thread_threshold = 1 << 20;
  threads = ideal_num_threads(threads, z, thread_threshold);
  LoadBalancer loadBalancer(x, z, s2_approx);
  PiTable pi(y, threads);

  #pragma omp parallel num_threads(threads)
  {
    ThreadSettings thread;

    while (loadBalancer.get_work(thread))
    {
      thread.start_time();
      thread.sum = S2_thread(x, y, z, c, pi, primes, lpf, mu, thread);
      thread.stop_time();
    }
  }

  int64_t sum = (int64_t) loadBalancer.get_sum();
  print("S2", sum, time);

  return sum;
}

} // namespace

namespace primecount {

/// Calculate the number of primes below x using the
/// Lagarias-Miller-Odlyzko algorithm.
/// Run time: O(x^(2/3) / log x)
/// Memory usage: O(x^(1/3) * (log x)^2)
///
int64_t pi_lmo_parallel(int64_t x, int threads)
{
  if (x < 2)
    return 0;

  double alpha = get_alpha_lmo(x);
  int64_t x13 = iroot<3>(x);
  int64_t y = (int64_t) (x13 * alpha);
  int64_t z = x / y;
  int64_t c = PhiTiny::get_c(y);

  print("");
  print("=== pi_lmo_parallel(x) ===");
  print("pi(x) = S1 + S2 + pi(y) - 1 - P2");
  print(x, y, z, c, threads);

  int64_t p2 = P2(x, y, threads);
  auto primes = generate_primes<int32_t>(y);
  auto lpf = generate_lpf(y);
  auto mu = generate_moebius(y);

  int64_t pi_y = primes.size() - 1;
  int64_t s1 = S1(x, y, c, threads);
  int64_t s2_approx = S2_approx(x, pi_y, p2, s1);
  int64_t s2 = S2(x, y, z, c, s2_approx, primes, lpf, mu, threads);
  int64_t phi = s1 + s2;
  int64_t sum = phi + pi_y - 1 - p2;

  return sum;
}

} // namespace
<commit_msg>Improve pre-sieving<commit_after>///
/// @file  pi_lmo_parallel.cpp
/// @brief Parallel implementation of the Lagarias-Miller-Odlyzko
///        prime counting algorithm using OpenMP. This implementation
///        uses load balancing to ensure all threads are kept busy
///        till the very end. This implementation also does not use a
///        special tree data structure (a.k.a. Fenwick tree) for
///        counting the number of unsieved elements but instead counts
///        the number of unsieved elements directly from the sieve
///        array using the POPCNT instruction which is much faster.
///
///        Lagarias-Miller-Odlyzko formula:
///        pi(x) = pi(y) + S1(x, a) + S2(x, a) - 1 - P2(x, a)
///        with y = x^(1/3), a = pi(y)
///
///        This implementation is based on the paper:
///        Tomás Oliveira e Silva, Computing pi(x): the combinatorial
///        method, Revista do DETUA, vol. 4, no. 6, March 2006,
///        pp. 759-768.
///
/// Copyright (C) 2021 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <primecount-internal.hpp>
#include <Sieve.hpp>
#include <generate.hpp>
#include <generate_phi.hpp>
#include <LoadBalancer.hpp>
#include <min.hpp>
#include <imath.hpp>
#include <PhiTiny.hpp>
#include <PiTable.hpp>
#include <print.hpp>
#include <S.hpp>

#include <stdint.h>
#include <vector>

using namespace std;
using namespace primecount;

namespace {

/// Compute the S2 contribution of the interval
/// [low, low + segments * segment_size[.
///
int64_t S2_thread(int64_t x,
                  int64_t y,
                  int64_t z,
                  int64_t c,
                  const PiTable& pi,
                  const vector<int32_t>& primes,
                  const vector<int32_t>& lpf,
                  const vector<int32_t>& mu,
                  ThreadSettings& thread)
{
  int64_t sum = 0;
  int64_t low = thread.low;
  int64_t low1 = max(low, 1);
  int64_t segments = thread.segments;
  int64_t segment_size = thread.segment_size;
  int64_t pi_sqrty = pi[isqrt(y)];
  int64_t limit = min(low + segments * segment_size, z + 1);
  int64_t max_b = pi[min(isqrt(x / low1), y - 1)];
  int64_t min_b = pi[min(z / limit, primes[max_b])];
  min_b = max(c, min_b) + 1;

  if (min_b > max_b)
    return 0;

  Sieve sieve(low, segment_size, max_b);
  auto phi = generate_phi(low, max_b, primes, pi);
  thread.init_finished();

  // segmented sieve of Eratosthenes
  for (; low < limit; low += segment_size)
  {
    // current segment [low, high[
    int64_t high = min(low + segment_size, limit);
    low1 = max(low, 1);

    // For b < min_b there are no special leaves:
    // low <= x / (primes[b] * m) < high
    sieve.pre_sieve(primes, min_b - 1, low, high);
    int64_t b = min_b;

    // For c + 1 <= b <= pi_sqrty
    // Find all special leaves in the current segment that are
    // composed of a prime and a square free number:
    // low <= x / (primes[b] * m) < high
    for (; b <= min(pi_sqrty, max_b); b++)
    {
      int64_t prime = primes[b];
      int64_t min_m = max(x / (prime * high), y / prime);
      int64_t max_m = min(x / (prime * low1), y);

      if (prime >= max_m)
        goto next_segment;

      for (int64_t m = max_m; m > min_m; m--)
      {
        if (mu[m] != 0 && prime < lpf[m])
        {
          int64_t xpm = x / (prime * m);
          int64_t stop = xpm - low;
          int64_t phi_xpm = phi[b] + sieve.count(stop);
          sum -= mu[m] * phi_xpm;
        }
      }

      phi[b] += sieve.get_total_count();
      sieve.cross_off_count(prime, b);
    }

    // For pi_sqrty < b < pi_y
    // Find all special leaves in the current segment
    // that are composed of 2 primes:
    // low <= x / (primes[b] * primes[l]) < high
    for (; b <= max_b; b++)
    {
      int64_t prime = primes[b];
      int64_t l = pi[min(x / (prime * low1), y)];
      int64_t min_m = max(x / (prime * high), prime);

      if (prime >= primes[l])
        goto next_segment;

      for (; primes[l] > min_m; l--)
      {
        int64_t xpq = x / (prime * primes[l]);
        int64_t stop = xpq - low;
        int64_t phi_xpq = phi[b] + sieve.count(stop);
        sum += phi_xpq;
      }

      phi[b] += sieve.get_total_count();
      sieve.cross_off_count(prime, b);
    }

    next_segment:;
  }

  return sum;
}

/// Calculate the contribution of thes pecial leaves.
///
/// This is a parallel S2(x, y) implementation with advanced load
/// balancing. As most special leaves tend to be in the first segments
/// we start off with a tiny segment size and one segment per thread.
/// After each iteration we dynamically increase the segment size (until
/// it reaches some limit) or the number of segments.
///
/// S2(x, y) has been parallelized using an idea devised by Xavier
/// Gourdon. The idea is to make the individual threads completely
/// independent from each other so that no thread depends on values
/// calculated by another thread. The benefit of this approach is that
/// the algorithm will scale well up to a very large number of CPU
/// cores. In order to make the threads independent from each other
/// each thread needs to precompute a lookup table of phi(x, a) values
/// (this is done in S2_thread(x, y)) every time the thread starts a
/// new computation.
///
int64_t S2(int64_t x,
           int64_t y,
           int64_t z,
           int64_t c,
           int64_t s2_approx,
           const vector<int32_t>& primes,
           const vector<int32_t>& lpf,
           const vector<int32_t>& mu,
           int threads)
{
  print("");
  print("=== S2(x, y) ===");

  double time = get_time();
  int64_t thread_threshold = 1 << 20;
  threads = ideal_num_threads(threads, z, thread_threshold);
  LoadBalancer loadBalancer(x, z, s2_approx);
  PiTable pi(y, threads);

  #pragma omp parallel num_threads(threads)
  {
    ThreadSettings thread;

    while (loadBalancer.get_work(thread))
    {
      thread.start_time();
      thread.sum = S2_thread(x, y, z, c, pi, primes, lpf, mu, thread);
      thread.stop_time();
    }
  }

  int64_t sum = (int64_t) loadBalancer.get_sum();
  print("S2", sum, time);

  return sum;
}

} // namespace

namespace primecount {

/// Calculate the number of primes below x using the
/// Lagarias-Miller-Odlyzko algorithm.
/// Run time: O(x^(2/3) / log x)
/// Memory usage: O(x^(1/3) * (log x)^2)
///
int64_t pi_lmo_parallel(int64_t x, int threads)
{
  if (x < 2)
    return 0;

  double alpha = get_alpha_lmo(x);
  int64_t x13 = iroot<3>(x);
  int64_t y = (int64_t) (x13 * alpha);
  int64_t z = x / y;
  int64_t c = PhiTiny::get_c(y);

  print("");
  print("=== pi_lmo_parallel(x) ===");
  print("pi(x) = S1 + S2 + pi(y) - 1 - P2");
  print(x, y, z, c, threads);

  int64_t p2 = P2(x, y, threads);
  auto primes = generate_primes<int32_t>(y);
  auto lpf = generate_lpf(y);
  auto mu = generate_moebius(y);

  int64_t pi_y = primes.size() - 1;
  int64_t s1 = S1(x, y, c, threads);
  int64_t s2_approx = S2_approx(x, pi_y, p2, s1);
  int64_t s2 = S2(x, y, z, c, s2_approx, primes, lpf, mu, threads);
  int64_t phi = s1 + s2;
  int64_t sum = phi + pi_y - 1 - p2;

  return sum;
}

} // namespace
<|endoftext|>


<commit_before>#define __STDC_LIMIT_MACROS
#include "rpc/mailbox/mailbox.hpp"

#include <stdint.h>

#include "errors.hpp"
#include <boost/bind.hpp>
#include <boost/function.hpp>

#include "concurrency/pmap.hpp"
#include "logger.hpp"

/* raw_mailbox_t */

const int raw_mailbox_t::address_t::ANY_THREAD = -1;

raw_mailbox_t::address_t::address_t() :
    peer(peer_id_t()), thread(ANY_THREAD), mailbox_id(0) { }

raw_mailbox_t::address_t::address_t(const address_t &a) :
    peer(a.peer), thread(a.thread), mailbox_id(a.mailbox_id) { }

bool raw_mailbox_t::address_t::is_nil() const {
    return peer.is_nil();
}

peer_id_t raw_mailbox_t::address_t::get_peer() const {
    rassert(!is_nil(), "A nil address has no peer");
    return peer;
}

std::string raw_mailbox_t::address_t::human_readable() const {
    return strprintf("%s:%d:%ld", uuid_to_str(peer.get_uuid()).c_str(), thread, mailbox_id);
}

raw_mailbox_t::raw_mailbox_t(mailbox_manager_t *m, mailbox_thread_mode_t tm, mailbox_read_callback_t *_callback) :
    manager(m),
    thread_mode(tm),
    mailbox_id(thread_mode == mailbox_any_thread ?
                   manager->register_mailbox_all_threads(this) :
                   manager->register_mailbox_one_thread(this)),
    callback(_callback) { }

raw_mailbox_t::~raw_mailbox_t() {
    assert_thread();
    manager->unregister_mailbox(mailbox_id);
}

raw_mailbox_t::address_t raw_mailbox_t::get_address() const {
    address_t a;
    a.peer = manager->get_connectivity_service()->get_me();
    if (thread_mode == mailbox_any_thread) {
        a.thread = address_t::ANY_THREAD;
    } else {
        rassert(thread_mode == mailbox_home_thread);
        a.thread = home_thread();
    }
    a.mailbox_id = mailbox_id;
    return a;
}

void send(mailbox_manager_t *src, raw_mailbox_t::address_t dest, mailbox_write_callback_t *callback) {
    rassert(src);
    rassert(!dest.is_nil());

    src->message_service->send_message(dest.peer,
        boost::bind(
            &mailbox_manager_t::write_mailbox_message,
            _1,
            dest.thread,
            dest.mailbox_id,
            callback
            ));
}

mailbox_manager_t::mailbox_manager_t(message_service_t *ms) :
    message_service(ms)
    { }

mailbox_manager_t::mailbox_table_t::mailbox_table_t() {
    next_local_mailbox_id = (UINT64_MAX / get_num_threads()) * get_thread_id();
    next_global_mailbox_id = next_local_mailbox_id;

    // Make local mailbox ids odd and global mailbox ids even so we can do some extra checks and stuff
    if (next_local_mailbox_id & 1) {
        ++next_global_mailbox_id;
    } else {
        ++next_local_mailbox_id;
    }
}

mailbox_manager_t::mailbox_table_t::~mailbox_table_t() {
    rassert(mailboxes.empty(), "Please destroy all mailboxes before destroying "
        "the cluster");
}

raw_mailbox_t *mailbox_manager_t::mailbox_table_t::find_mailbox(raw_mailbox_t::id_t id) {
    std::map<raw_mailbox_t::id_t, raw_mailbox_t *>::iterator it = mailboxes.find(id);
    if (it == mailboxes.end()) {
        return NULL;
    } else {
        return (*it).second;
    }
}

void mailbox_manager_t::write_mailbox_message(write_stream_t *stream, int dest_thread, raw_mailbox_t::id_t dest_mailbox_id, mailbox_write_callback_t *callback) {
    write_message_t msg;
    int32_t dest_thread_32_bit = dest_thread;
    msg << dest_thread_32_bit;
    msg << dest_mailbox_id;

    // TODO: Maybe pass write_message_t to writer... eventually.
    int res = send_write_message(stream, &msg);

    if (res) { throw fake_archive_exc_t(); }

    callback->write(stream);
}

void mailbox_manager_t::on_message(UNUSED peer_id_t source_peer, read_stream_t *stream) {
    int dest_thread;
    raw_mailbox_t::id_t dest_mailbox_id;
    {
        archive_result_t res = deserialize(stream, &dest_thread);
        if (res) { throw fake_archive_exc_t(); }
        res = deserialize(stream, &dest_mailbox_id);
        if (res) { throw fake_archive_exc_t(); }
    }

    if (dest_thread == raw_mailbox_t::address_t::ANY_THREAD) {
        // TODO: this will just run the callback on the current thread, maybe do some load balancing, instead
        dest_thread = get_thread_id();
    }

    /* TODO: This is probably horribly inefficient; we switch to another
    thread and back before we parse the next message. */
    on_thread_t thread_switcher(dest_thread);

    raw_mailbox_t *mbox = mailbox_tables.get()->find_mailbox(dest_mailbox_id);
    if (mbox) {
        auto_drainer_t::lock_t lock(&mbox->drainer);
        mbox->callback->read(stream);
    } else {
        /* Ignore it, because it's impossible to write code in such a way that
        messages will never be received for dead mailboxes. */
    }
}

raw_mailbox_t::id_t mailbox_manager_t::generate_local_id() {
    raw_mailbox_t::id_t id = mailbox_tables.get()->next_local_mailbox_id;
    mailbox_tables.get()->next_local_mailbox_id += 2;
    return id;
}

raw_mailbox_t::id_t mailbox_manager_t::generate_global_id() {
    raw_mailbox_t::id_t id = mailbox_tables.get()->next_global_mailbox_id;
    mailbox_tables.get()->next_global_mailbox_id += 2;
    return id;
}

raw_mailbox_t::id_t mailbox_manager_t::register_mailbox_one_thread(raw_mailbox_t *mb) {
    raw_mailbox_t::id_t id = generate_local_id();
    register_mailbox_internal(mb, id);
    return id;
}

raw_mailbox_t::id_t mailbox_manager_t::register_mailbox_all_threads(raw_mailbox_t *mb) {
    raw_mailbox_t::id_t id = generate_global_id();
    pmap(get_num_threads(), boost::bind(&mailbox_manager_t::register_mailbox_wrapper, this, mb, id, _1));
    return id;
}

void mailbox_manager_t::register_mailbox_wrapper(raw_mailbox_t *mb, raw_mailbox_t::id_t id, int thread) {
    on_thread_t rethreader(thread);
    register_mailbox_internal(mb, id);
}

void mailbox_manager_t::register_mailbox_internal(raw_mailbox_t *mb, raw_mailbox_t::id_t id) {
    rassert(mailbox_tables.get()->mailboxes.count(id) == 0);
    mailbox_tables.get()->mailboxes[id] = mb;
}

void mailbox_manager_t::unregister_mailbox(raw_mailbox_t::id_t id) {
    // If the id is local, only remove it from this thread, otherwise only remove it from all threads
    if (id & 1) { // An odd id indicates a local mailbox
        unregister_mailbox_internal(id);
    } else {
        pmap(get_num_threads(), boost::bind(&mailbox_manager_t::unregister_mailbox_wrapper, this, id, _1));
    }
}

void mailbox_manager_t::unregister_mailbox_wrapper(raw_mailbox_t::id_t id, int thread) {
    on_thread_t rethreader(thread);
    unregister_mailbox_internal(id);
}

void mailbox_manager_t::unregister_mailbox_internal(raw_mailbox_t::id_t id) {
    rassert(mailbox_tables.get()->mailboxes.count(id) == 1);
    mailbox_tables.get()->mailboxes.erase(id);
}

<commit_msg>removing unneeded boost/function.hpp include<commit_after>#define __STDC_LIMIT_MACROS
#include "rpc/mailbox/mailbox.hpp"

#include <stdint.h>

#include "errors.hpp"
#include <boost/bind.hpp>

#include "concurrency/pmap.hpp"
#include "logger.hpp"

/* raw_mailbox_t */

const int raw_mailbox_t::address_t::ANY_THREAD = -1;

raw_mailbox_t::address_t::address_t() :
    peer(peer_id_t()), thread(ANY_THREAD), mailbox_id(0) { }

raw_mailbox_t::address_t::address_t(const address_t &a) :
    peer(a.peer), thread(a.thread), mailbox_id(a.mailbox_id) { }

bool raw_mailbox_t::address_t::is_nil() const {
    return peer.is_nil();
}

peer_id_t raw_mailbox_t::address_t::get_peer() const {
    rassert(!is_nil(), "A nil address has no peer");
    return peer;
}

std::string raw_mailbox_t::address_t::human_readable() const {
    return strprintf("%s:%d:%ld", uuid_to_str(peer.get_uuid()).c_str(), thread, mailbox_id);
}

raw_mailbox_t::raw_mailbox_t(mailbox_manager_t *m, mailbox_thread_mode_t tm, mailbox_read_callback_t *_callback) :
    manager(m),
    thread_mode(tm),
    mailbox_id(thread_mode == mailbox_any_thread ?
                   manager->register_mailbox_all_threads(this) :
                   manager->register_mailbox_one_thread(this)),
    callback(_callback) { }

raw_mailbox_t::~raw_mailbox_t() {
    assert_thread();
    manager->unregister_mailbox(mailbox_id);
}

raw_mailbox_t::address_t raw_mailbox_t::get_address() const {
    address_t a;
    a.peer = manager->get_connectivity_service()->get_me();
    if (thread_mode == mailbox_any_thread) {
        a.thread = address_t::ANY_THREAD;
    } else {
        rassert(thread_mode == mailbox_home_thread);
        a.thread = home_thread();
    }
    a.mailbox_id = mailbox_id;
    return a;
}

void send(mailbox_manager_t *src, raw_mailbox_t::address_t dest, mailbox_write_callback_t *callback) {
    rassert(src);
    rassert(!dest.is_nil());

    src->message_service->send_message(dest.peer,
        boost::bind(
            &mailbox_manager_t::write_mailbox_message,
            _1,
            dest.thread,
            dest.mailbox_id,
            callback
            ));
}

mailbox_manager_t::mailbox_manager_t(message_service_t *ms) :
    message_service(ms)
    { }

mailbox_manager_t::mailbox_table_t::mailbox_table_t() {
    next_local_mailbox_id = (UINT64_MAX / get_num_threads()) * get_thread_id();
    next_global_mailbox_id = next_local_mailbox_id;

    // Make local mailbox ids odd and global mailbox ids even so we can do some extra checks and stuff
    if (next_local_mailbox_id & 1) {
        ++next_global_mailbox_id;
    } else {
        ++next_local_mailbox_id;
    }
}

mailbox_manager_t::mailbox_table_t::~mailbox_table_t() {
    rassert(mailboxes.empty(), "Please destroy all mailboxes before destroying "
        "the cluster");
}

raw_mailbox_t *mailbox_manager_t::mailbox_table_t::find_mailbox(raw_mailbox_t::id_t id) {
    std::map<raw_mailbox_t::id_t, raw_mailbox_t *>::iterator it = mailboxes.find(id);
    if (it == mailboxes.end()) {
        return NULL;
    } else {
        return (*it).second;
    }
}

void mailbox_manager_t::write_mailbox_message(write_stream_t *stream, int dest_thread, raw_mailbox_t::id_t dest_mailbox_id, mailbox_write_callback_t *callback) {
    write_message_t msg;
    int32_t dest_thread_32_bit = dest_thread;
    msg << dest_thread_32_bit;
    msg << dest_mailbox_id;

    // TODO: Maybe pass write_message_t to writer... eventually.
    int res = send_write_message(stream, &msg);

    if (res) { throw fake_archive_exc_t(); }

    callback->write(stream);
}

void mailbox_manager_t::on_message(UNUSED peer_id_t source_peer, read_stream_t *stream) {
    int dest_thread;
    raw_mailbox_t::id_t dest_mailbox_id;
    {
        archive_result_t res = deserialize(stream, &dest_thread);
        if (res) { throw fake_archive_exc_t(); }
        res = deserialize(stream, &dest_mailbox_id);
        if (res) { throw fake_archive_exc_t(); }
    }

    if (dest_thread == raw_mailbox_t::address_t::ANY_THREAD) {
        // TODO: this will just run the callback on the current thread, maybe do some load balancing, instead
        dest_thread = get_thread_id();
    }

    /* TODO: This is probably horribly inefficient; we switch to another
    thread and back before we parse the next message. */
    on_thread_t thread_switcher(dest_thread);

    raw_mailbox_t *mbox = mailbox_tables.get()->find_mailbox(dest_mailbox_id);
    if (mbox) {
        auto_drainer_t::lock_t lock(&mbox->drainer);
        mbox->callback->read(stream);
    } else {
        /* Ignore it, because it's impossible to write code in such a way that
        messages will never be received for dead mailboxes. */
    }
}

raw_mailbox_t::id_t mailbox_manager_t::generate_local_id() {
    raw_mailbox_t::id_t id = mailbox_tables.get()->next_local_mailbox_id;
    mailbox_tables.get()->next_local_mailbox_id += 2;
    return id;
}

raw_mailbox_t::id_t mailbox_manager_t::generate_global_id() {
    raw_mailbox_t::id_t id = mailbox_tables.get()->next_global_mailbox_id;
    mailbox_tables.get()->next_global_mailbox_id += 2;
    return id;
}

raw_mailbox_t::id_t mailbox_manager_t::register_mailbox_one_thread(raw_mailbox_t *mb) {
    raw_mailbox_t::id_t id = generate_local_id();
    register_mailbox_internal(mb, id);
    return id;
}

raw_mailbox_t::id_t mailbox_manager_t::register_mailbox_all_threads(raw_mailbox_t *mb) {
    raw_mailbox_t::id_t id = generate_global_id();
    pmap(get_num_threads(), boost::bind(&mailbox_manager_t::register_mailbox_wrapper, this, mb, id, _1));
    return id;
}

void mailbox_manager_t::register_mailbox_wrapper(raw_mailbox_t *mb, raw_mailbox_t::id_t id, int thread) {
    on_thread_t rethreader(thread);
    register_mailbox_internal(mb, id);
}

void mailbox_manager_t::register_mailbox_internal(raw_mailbox_t *mb, raw_mailbox_t::id_t id) {
    rassert(mailbox_tables.get()->mailboxes.count(id) == 0);
    mailbox_tables.get()->mailboxes[id] = mb;
}

void mailbox_manager_t::unregister_mailbox(raw_mailbox_t::id_t id) {
    // If the id is local, only remove it from this thread, otherwise only remove it from all threads
    if (id & 1) { // An odd id indicates a local mailbox
        unregister_mailbox_internal(id);
    } else {
        pmap(get_num_threads(), boost::bind(&mailbox_manager_t::unregister_mailbox_wrapper, this, id, _1));
    }
}

void mailbox_manager_t::unregister_mailbox_wrapper(raw_mailbox_t::id_t id, int thread) {
    on_thread_t rethreader(thread);
    unregister_mailbox_internal(id);
}

void mailbox_manager_t::unregister_mailbox_internal(raw_mailbox_t::id_t id) {
    rassert(mailbox_tables.get()->mailboxes.count(id) == 1);
    mailbox_tables.get()->mailboxes.erase(id);
}

<|endoftext|>


<commit_before>#include "AutoTileDockWidget.h"
#include "devices\Microscope.h"
#include "MainWindow.h"
#include "AgentController.h"

namespace fetch{
namespace ui{
  AutoTileDockWidget::AutoTileDockWidget(device::Microscope *dc, MainWindow *parent)
    : QDockWidget("Auto Tile",parent)
  { QWidget *w = new QWidget(this);
    QFormLayout *form = new QFormLayout;
    w->setLayout(form);
    setWidget(w);

    parent->_autotile_zoffum_control->createLineEditAndAddToLayout(form);
    parent->_autotile_zmaxmm_control->createLineEditAndAddToLayout(form);
    parent->_autotile_timeoutms_control->createLineEditAndAddToLayout(form);
    parent->_autotile_chan_control->createLineEditAndAddToLayout(form);
    parent->_autotile_intensity_thresh_control->createLineEditAndAddToLayout(form);
    parent->_autotile_area_thresh_control->createLineEditAndAddToLayout(form);

    AgentControllerButtonPanel *btns = new AgentControllerButtonPanel(&parent->_scope_state_controller,&dc->auto_tile_task);
    form->addRow(btns);
  }
}}//fetch::ui<commit_msg>fix/AutotileAtFixedZ: Added checkbox to select whether or not to use current Z.<commit_after>#include "AutoTileDockWidget.h"
#include "devices\Microscope.h"
#include "MainWindow.h"
#include "AgentController.h"

namespace fetch{
namespace ui{
  AutoTileDockWidget::AutoTileDockWidget(device::Microscope *dc, MainWindow *parent)
    : QDockWidget("Auto Tile",parent)
  { QWidget *w = new QWidget(this);
    QFormLayout *form = new QFormLayout;
    w->setLayout(form);
    setWidget(w);

	///// Use Current Z Checkbox Control
	QCheckBox *b = new QCheckBox();
	QStateMachine *lockmachine = new QStateMachine(this);
	QState *doUseCurrentZ = new QState(),
		*doNotUseCurrentZ = new QState();
	doUseCurrentZ->addTransition(b, SIGNAL(stateChanged(int)), doNotUseCurrentZ);
	doNotUseCurrentZ->addTransition(b, SIGNAL(stateChanged(int)), doUseCurrentZ);
	doUseCurrentZ->assignProperty(b, "text", "Using Current Z");
	doNotUseCurrentZ->assignProperty(b, "text", "Not Using Current Z");

	lockmachine->addState(doUseCurrentZ);
	lockmachine->addState(doNotUseCurrentZ);
	b->setCheckState(Qt::Checked);
	lockmachine->setInitialState(doUseCurrentZ);
	lockmachine->start();

    parent->_autotile_zoffum_control->createLineEditAndAddToLayout(form);
    parent->_autotile_zmaxmm_control->createLineEditAndAddToLayout(form);
    parent->_autotile_timeoutms_control->createLineEditAndAddToLayout(form);
    parent->_autotile_chan_control->createLineEditAndAddToLayout(form);
    parent->_autotile_intensity_thresh_control->createLineEditAndAddToLayout(form);
    parent->_autotile_area_thresh_control->createLineEditAndAddToLayout(form);
	form->addRow("Use Current Z: ", b);

    AgentControllerButtonPanel *btns = new AgentControllerButtonPanel(&parent->_scope_state_controller,&dc->auto_tile_task);
    form->addRow(btns);
  }
}}//fetch::ui<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: XTDataObject.hxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 16:56:12 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    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
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser 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
 *
 ************************************************************************/

#ifndef _XTDATAOBJECT_HXX_
#define _XTDATAOBJECT_HXX_

//------------------------------------------------------------------------
// includes
//------------------------------------------------------------------------

/*
#ifndef _COM_SUN_STAR_DATATRANSFER_XTRANSFERABLE_HPP_
#include <com/sun/star/datatransfer/XTransferable.hpp>
#endif

#ifndef _COM_SUN_STAR_DATATRANSFER_CLIPBOARD_XCLIPBOARDOWNER_HPP_
#include <com/sun/star/datatransfer/clipboard/XClipboardOwner.hpp>
#endif

#ifndef _WINCLIPBOARD_HXX_
#include "WinClipboard.hxx"
#endif
*/

#include <windows.h>
#include <ole2.h>
#include <objidl.h>

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

class EnumFormatEtc;

class CXTDataObject : public IDataObject
{
public:
    CXTDataObject( LONG nRefCntInitVal = 0);
    ~CXTDataObject( );

    //-----------------------------------------------------------------
    // ole interface implementation
    //-----------------------------------------------------------------

    //IUnknown
    STDMETHODIMP           QueryInterface(REFIID iid, LPVOID* ppvObject);
    STDMETHODIMP_( ULONG ) AddRef( );
    STDMETHODIMP_( ULONG ) Release( );

    //IDataObject
    STDMETHODIMP GetData( LPFORMATETC pFormatetc, LPSTGMEDIUM pmedium );
    STDMETHODIMP GetDataHere( LPFORMATETC pFormatetc, LPSTGMEDIUM pmedium );
    STDMETHODIMP QueryGetData( LPFORMATETC pFormatetc );
    STDMETHODIMP GetCanonicalFormatEtc( LPFORMATETC pFormatectIn, LPFORMATETC pFormatetcOut );
    STDMETHODIMP SetData( LPFORMATETC pFormatetc, LPSTGMEDIUM pmedium, BOOL fRelease );
    STDMETHODIMP EnumFormatEtc( DWORD dwDirection, IEnumFORMATETC** ppenumFormatetc );
    STDMETHODIMP DAdvise( LPFORMATETC pFormatetc, DWORD advf, LPADVISESINK pAdvSink, DWORD* pdwConnection );
    STDMETHODIMP DUnadvise( DWORD dwConnection );
    STDMETHODIMP EnumDAdvise( LPENUMSTATDATA* ppenumAdvise );

    operator IDataObject*( );

    // notification handler
    //void SAL_CALL LostOwnership( );

    //sal_Int64 SAL_CALL QueryDataSize( );

    // retrieve the data from the source
    // necessary so that
    //void SAL_CALL GetAllDataFromSource( );

private:
    LONG m_nRefCnt;
    //CWinClipboard& m_rCWinClipboard;
    //const const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardOwner >&  m_rXClipboardOwner;
    //const const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::XTransferable >&               m_rXTDataSource;

    //friend class CWinClipboard;
    friend class CEnumFormatEtc;
};

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

class CEnumFormatEtc : public IEnumFORMATETC
{
public:
    CEnumFormatEtc( LPUNKNOWN pUnkDataObj );
    ~CEnumFormatEtc( );

    // IUnknown
    STDMETHODIMP           QueryInterface( REFIID iid, LPVOID* ppvObject );
    STDMETHODIMP_( ULONG ) AddRef( );
    STDMETHODIMP_( ULONG ) Release( );

    //IEnumFORMATETC
    STDMETHODIMP Next( ULONG celt, LPFORMATETC rgelt, ULONG* pceltFetched );
    STDMETHODIMP Skip( ULONG celt );
    STDMETHODIMP Reset( );
    STDMETHODIMP Clone( IEnumFORMATETC** ppenum );

private:
    LONG        m_nRefCnt;
    LPUNKNOWN   m_pUnkDataObj;
    ULONG       m_nCurrentPos;
    CLIPFORMAT  m_cfFormats[2];
};

typedef CEnumFormatEtc *PCEnumFormatEtc;

#endif
<commit_msg>INTEGRATION: CWS changefileheader (1.4.378); FILE MERGED 2008/04/01 15:25:10 thb 1.4.378.3: #i85898# Stripping all external header guards 2008/04/01 12:34:24 thb 1.4.378.2: #i85898# Stripping all external header guards 2008/03/31 13:24:41 rt 1.4.378.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: XTDataObject.hxx,v $
 * $Revision: 1.5 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 Lesser General Public License version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#ifndef _XTDATAOBJECT_HXX_
#define _XTDATAOBJECT_HXX_

//------------------------------------------------------------------------
// includes
//------------------------------------------------------------------------

/*
#include <com/sun/star/datatransfer/XTransferable.hpp>
#include <com/sun/star/datatransfer/clipboard/XClipboardOwner.hpp>
#include "WinClipboard.hxx"
*/

#include <windows.h>
#include <ole2.h>
#include <objidl.h>

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

class EnumFormatEtc;

class CXTDataObject : public IDataObject
{
public:
    CXTDataObject( LONG nRefCntInitVal = 0);
    ~CXTDataObject( );

    //-----------------------------------------------------------------
    // ole interface implementation
    //-----------------------------------------------------------------

    //IUnknown
    STDMETHODIMP           QueryInterface(REFIID iid, LPVOID* ppvObject);
    STDMETHODIMP_( ULONG ) AddRef( );
    STDMETHODIMP_( ULONG ) Release( );

    //IDataObject
    STDMETHODIMP GetData( LPFORMATETC pFormatetc, LPSTGMEDIUM pmedium );
    STDMETHODIMP GetDataHere( LPFORMATETC pFormatetc, LPSTGMEDIUM pmedium );
    STDMETHODIMP QueryGetData( LPFORMATETC pFormatetc );
    STDMETHODIMP GetCanonicalFormatEtc( LPFORMATETC pFormatectIn, LPFORMATETC pFormatetcOut );
    STDMETHODIMP SetData( LPFORMATETC pFormatetc, LPSTGMEDIUM pmedium, BOOL fRelease );
    STDMETHODIMP EnumFormatEtc( DWORD dwDirection, IEnumFORMATETC** ppenumFormatetc );
    STDMETHODIMP DAdvise( LPFORMATETC pFormatetc, DWORD advf, LPADVISESINK pAdvSink, DWORD* pdwConnection );
    STDMETHODIMP DUnadvise( DWORD dwConnection );
    STDMETHODIMP EnumDAdvise( LPENUMSTATDATA* ppenumAdvise );

    operator IDataObject*( );

    // notification handler
    //void SAL_CALL LostOwnership( );

    //sal_Int64 SAL_CALL QueryDataSize( );

    // retrieve the data from the source
    // necessary so that
    //void SAL_CALL GetAllDataFromSource( );

private:
    LONG m_nRefCnt;
    //CWinClipboard& m_rCWinClipboard;
    //const const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardOwner >&  m_rXClipboardOwner;
    //const const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::XTransferable >&               m_rXTDataSource;

    //friend class CWinClipboard;
    friend class CEnumFormatEtc;
};

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

class CEnumFormatEtc : public IEnumFORMATETC
{
public:
    CEnumFormatEtc( LPUNKNOWN pUnkDataObj );
    ~CEnumFormatEtc( );

    // IUnknown
    STDMETHODIMP           QueryInterface( REFIID iid, LPVOID* ppvObject );
    STDMETHODIMP_( ULONG ) AddRef( );
    STDMETHODIMP_( ULONG ) Release( );

    //IEnumFORMATETC
    STDMETHODIMP Next( ULONG celt, LPFORMATETC rgelt, ULONG* pceltFetched );
    STDMETHODIMP Skip( ULONG celt );
    STDMETHODIMP Reset( );
    STDMETHODIMP Clone( IEnumFORMATETC** ppenum );

private:
    LONG        m_nRefCnt;
    LPUNKNOWN   m_pUnkDataObj;
    ULONG       m_nCurrentPos;
    CLIPFORMAT  m_cfFormats[2];
};

typedef CEnumFormatEtc *PCEnumFormatEtc;

#endif
<|endoftext|>


<commit_before>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * 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
 * OpenSceneGraph Public License for more details.
 */

#include "CameraController.h"
#include <osg/Quat>
#include <osg/Notify>
#include <osg/io_utils>
#include <osgUtil/LineSegmentIntersector>
#include "MathUtils.h"

namespace ews {
    namespace app {
        namespace drawable {
            using namespace osg;
            const Vec3 DEFAULT_OFFSET(200, 300, 200);
            const Vec3 UP(0, 1, 0);

            CameraController::CameraController()
            : TerrainManipulator()
            {
                setMinimumDistance(10);
                computeHomePosition();
            }

            CameraController::~CameraController() {
            }

            /** Compute the home position.*/
            void CameraController::computeHomePosition() {
                if(getNode()) {
                    const BoundingSphere& boundingSphere = getNode()->getBound();
                    const Vec3& center = boundingSphere.center();

                    setHomePosition(center+DEFAULT_OFFSET, center, UP,
                                    getAutoComputeHomePosition());
                }
                else {
                    notify(NOTICE) << "Can't compute home position. No node assigned to camera" << std::endl;
                }
            }

            bool CameraController::handle(const GUIEventAdapter& ea, GUIActionAdapter& us) {
                if (ea.getHandled()) return false;

                if (ea.getEventType() == GUIEventAdapter::FRAME) return false;

                switch(ea.getEventType())
                {
                    case(GUIEventAdapter::PUSH):
                    case(GUIEventAdapter::RELEASE):
                    {
                        flushMouseEventStack();
                        addMouseEvent(ea);
                        if (calcMovement()) us.requestRedraw();
                        us.requestContinuousUpdate(false);
                        return true;
                    }

                    case(GUIEventAdapter::DRAG):
                    {
                        addMouseEvent(ea);
                        if (calcMovement()) us.requestRedraw();
                        us.requestContinuousUpdate(false);
                        return true;
                    }

                    case(GUIEventAdapter::MOVE):
                    {
                        return false;
                    }

                    case(GUIEventAdapter::KEYDOWN):
                        if (ea.getKey()== GUIEventAdapter::KEY_Space) {
                            flushMouseEventStack();
                            home(ea,us);
                            us.requestRedraw();
                            us.requestContinuousUpdate(false);
                            return true;
                        }
                        return false;
                    default:
                        return false;
                }
            }

            void CameraController::computePosition(const osg::Vec3d& eye,const osg::Vec3d& center,const osg::Vec3d& up) {
                if (!_node) return;

                // compute rotation matrix
                osg::Vec3d lv(center - eye);
                _distance = lv.length();
                _center = center;

                osg::notify(osg::INFO) << "In compute"<< std::endl;

                if (_node.valid()) {
                    bool hitFound = false;

                    const double distance = lv.length();
                    const double maxDistance = distance + 2 * (eye - _node->getBound().center()).length();
                    const osg::Vec3d farPosition = eye + lv * (maxDistance / distance);
                    osg::Vec3d endPoint = center;
                    for (int i = 0; !hitFound && i < 2; ++i, endPoint = farPosition) {
                        // compute the intersection with the scene.
                        osg::Vec3d ip;
                        if (intersect(eye, endPoint, ip)) {
                            _center = ip;
                            _distance = (ip - eye).length();
                            hitFound = true;
                        }
                    }
                }

                // note LookAt = inv(CF)*inv(RM)*inv(T) which is equivalent to:
                // inv(R) = CF*LookAt.

                osg::Matrixd rotation_matrix = osg::Matrixd::lookAt(eye, center, up);

                _rotation = rotation_matrix.getRotate().inverse();

                CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
                _previousUp = getUpVector(coordinateFrame);
                clampOrientation();
            }

            void CameraController::doRotate(double dx, double dy) {
                // rotate camera.
                osg::Vec3 axis;
                double angle;

                double px0 = _ga_t0->getXnormalized();
                double py0 = _ga_t0->getYnormalized();

                double px1 = _ga_t1->getXnormalized();
                double py1 = _ga_t1->getYnormalized();

                trackball(axis, angle, px1, py1, px0, py0);

                osg::Quat new_rotate;
                new_rotate.makeRotate(angle, axis);

                _rotation = _rotation * new_rotate;
                clampOrientation();
            }

            void CameraController::doZoom(double dx, double dy) {
                using namespace ews::util;
                // zoom model.
                const double scale = 1.0f + dy;
                _distance = imposeBounds(MIN_ZOOM_DISTANCE, _distance * scale, MAX_ZOOM_DISTANCE);
            }

            void CameraController::doPan(double dx, double dy) {
                // pan model.
                double scale = -0.3f*_distance;

                osg::Matrixd rotation_matrix;
                rotation_matrix.makeRotate(_rotation);

                // compute look vector.
                osg::Vec3d lookVector = -getUpVector(rotation_matrix);
                osg::Vec3d sideVector = getSideVector(rotation_matrix);
                osg::Vec3d upVector = getFrontVector(rotation_matrix);

                // CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
                // osg::Vec3d localUp = getUpVector(coordinateFrame);
                osg::Vec3d localUp = _previousUp;

                osg::Vec3d forwardVector =localUp^sideVector;
                sideVector = forwardVector^localUp;

                forwardVector.normalize();
                sideVector.normalize();

                osg::Vec3d dv = forwardVector * (dy*scale) + sideVector * (dx*scale);

                _center += dv;

                // need to recompute the intersection point along the look vector.

                bool hitFound = false;

                if (_node.valid()) {

                    // now reorientate the coordinate frame to the frame coords.
                    CoordinateFrame coordinateFrame =  getCoordinateFrame(_center);

                    // need to reintersect with the terrain
                    double distance = _node->getBound().radius()*0.25f;

                    osg::Vec3d ip1;
                    osg::Vec3d ip2;
                    bool hit_ip1 = intersect(_center, _center + getUpVector(coordinateFrame) * distance, ip1);
                    bool hit_ip2 = intersect(_center, _center - getUpVector(coordinateFrame) * distance, ip2);
                    if (hit_ip1)  {
                        if (hit_ip2) {
                            _center = (_center-ip1).length2() < (_center-ip2).length2() ?
                            ip1 :
                            ip2;

                            hitFound = true;
                        }
                        else {
                            _center = ip1;
                            hitFound = true;
                        }
                    }
                    else if (hit_ip2) {
                        _center = ip2;
                        hitFound = true;
                    }

                    if (!hitFound) {
                        // ??
                        osg::notify(INFO)<<"CameraController unable to intersect"<<std::endl;
                    }

                    coordinateFrame = getCoordinateFrame(_center);
                    osg::Vec3d new_localUp = getUpVector(coordinateFrame);

                    osg::Quat pan_rotation;
                    pan_rotation.makeRotate(localUp,new_localUp);

                    if (!pan_rotation.zeroRotation()) {
                        _rotation = _rotation * pan_rotation;
                        _previousUp = new_localUp;
//                        osg::notify(osg::NOTICE)<<"Rotating from "<<localUp<<" to "<<new_localUp<<"  angle = "<<acos(localUp*new_localUp/(localUp.length()*new_localUp.length()))<<std::endl;
//                        clampOrientation();
                    }
                    else {
                        osg::notify(osg::INFO)<<"New up orientation nearly inline - no need to rotate"<<std::endl;
                    }
                }
            }

            bool CameraController::calcMovement() {
                // return if less then two events have been added.
                if (_ga_t0.get() == NULL || _ga_t1.get() == NULL) return false;

                double dx = _ga_t0->getXnormalized() - _ga_t1->getXnormalized();
                double dy = _ga_t0->getYnormalized() - _ga_t1->getYnormalized();

                // return if there is no movement.
                if (dx==0 && dy==0) return false;

                unsigned int buttonMask = _ga_t1->getButtonMask();
                if (buttonMask==GUIEventAdapter::LEFT_MOUSE_BUTTON) {
                    doRotate(dx, dy);
                    return true;
                }
                else if (buttonMask==GUIEventAdapter::MIDDLE_MOUSE_BUTTON ||
                         buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON|GUIEventAdapter::RIGHT_MOUSE_BUTTON)) {
                    doPan(dx, dy);
                    return true;
                }
                else if (buttonMask==GUIEventAdapter::RIGHT_MOUSE_BUTTON) {
                    doZoom(dx, dy);
                    return true;
                }

                return false;
            }

            void CameraController::clampOrientation() {
                osg::Matrixd rotation_matrix;
                rotation_matrix.makeRotate(_rotation);

                osg::Vec3d lookVector = -getUpVector(rotation_matrix);
                osg::Vec3d upVector = getFrontVector(rotation_matrix);

                // Don't let it rotate past the X-Y plane
                if (lookVector.z() > 0) {
                    Vec3d newLookVector(lookVector.x(), lookVector.y(), 0);
                    newLookVector.normalize();
                    osg::Quat keepNegative;
                    keepNegative.makeRotate(lookVector, newLookVector);
                    _rotation = _rotation * keepNegative;
                    rotation_matrix.makeRotate(_rotation);
                    lookVector = -getUpVector(rotation_matrix);
                    upVector = getFrontVector(rotation_matrix);
                }
                // Don't let it rotate beyond the Z-axis
                if (upVector.z() < 0) {
                    Vec3d newUpVector(upVector.x(), upVector.y(), 0);
                    newUpVector.normalize();
                    osg::Quat keepPositive;
                    keepPositive.makeRotate(upVector, newUpVector);
                    _rotation = _rotation * keepPositive;
                    rotation_matrix.makeRotate(_rotation);
                    lookVector = -getUpVector(rotation_matrix);
                    upVector = getFrontVector(rotation_matrix);
                }

                CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
                osg::Vec3d localUp = getUpVector(coordinateFrame);
                //osg::Vec3d localUp = _previousUp;

                osg::Vec3d sideVector = lookVector ^ localUp;

                if (sideVector.length()<0.1) {
                    osg::notify(osg::INFO)<<"Side vector short "<<sideVector.length()<<std::endl;

                    sideVector = upVector ^ localUp;
                    sideVector.normalize();

                }

                Vec3d newUpVector = sideVector ^ lookVector;
                newUpVector.normalize();
                osg::Quat rotate_roll;
                rotate_roll.makeRotate(upVector, newUpVector);
                if (!rotate_roll.zeroRotation()) {
                    _rotation = _rotation * rotate_roll;
                }
            }
        }
    }
}
<commit_msg>Minor changes to CameraController.cpp<commit_after>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * 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
 * OpenSceneGraph Public License for more details.
 */

#include "CameraController.h"
#include <osg/Quat>
#include <osg/Notify>
#include <osg/io_utils>
#include <osgUtil/LineSegmentIntersector>
#include "MathUtils.h"

namespace ews {
    namespace app {
        namespace drawable {
            using namespace osg;
            const Vec3 DEFAULT_OFFSET(200, 300, 200);
            const Vec3 UP(0, 1, 0);

            CameraController::CameraController()
            : TerrainManipulator()
            {
                setMinimumDistance(10);
                computeHomePosition();
            }

            CameraController::~CameraController() {
            }

            /** Compute the home position.*/
            void CameraController::computeHomePosition() {
                if(getNode()) {
                    const BoundingSphere& boundingSphere = getNode()->getBound();
                    const Vec3& center = boundingSphere.center();

                    setHomePosition(center+DEFAULT_OFFSET, center, UP,
                                    getAutoComputeHomePosition());
                }
                else {
                    notify(NOTICE) << "Can't compute home position. No node assigned to camera" << std::endl;
                }
            }

            bool CameraController::handle(const GUIEventAdapter& ea, GUIActionAdapter& us) {
                if (ea.getHandled()) return false;

                if (ea.getEventType() == GUIEventAdapter::FRAME) return false;

                switch(ea.getEventType())
                {
                    case(GUIEventAdapter::PUSH):
                    case(GUIEventAdapter::RELEASE):
                    {
                        flushMouseEventStack();
                        addMouseEvent(ea);
                        if (calcMovement()) us.requestRedraw();
                        us.requestContinuousUpdate(false);
                        return true;
                    }

                    case(GUIEventAdapter::DRAG):
                    {
                        addMouseEvent(ea);
                        if (calcMovement()) us.requestRedraw();
                        us.requestContinuousUpdate(false);
                        return true;
                    }

                    case(GUIEventAdapter::MOVE):
                    {
                        return false;
                    }

                    case(GUIEventAdapter::KEYDOWN):
                        if (ea.getKey()== GUIEventAdapter::KEY_Space) {
                            flushMouseEventStack();
                            home(ea,us);
                            us.requestRedraw();
                            us.requestContinuousUpdate(false);
                            return true;
                        }
                        return false;
                    default:
                        return false;
                }
            }

            void CameraController::computePosition(const osg::Vec3d& eye,const osg::Vec3d& center,const osg::Vec3d& up) {
                if (!_node) return;

                // compute rotation matrix
                osg::Vec3d lv(center - eye);
                _distance = lv.length();
                _center = center;

                osg::notify(osg::INFO) << "In compute"<< std::endl;

                if (_node.valid()) {
                    bool hitFound = false;

                    const double distance = lv.length();
                    const double maxDistance = distance + 2 * (eye - _node->getBound().center()).length();
                    const osg::Vec3d farPosition = eye + lv * (maxDistance / distance);
                    osg::Vec3d endPoint = center;
                    for (int i = 0; !hitFound && i < 2; ++i, endPoint = farPosition) {
                        // compute the intersection with the scene.
                        osg::Vec3d ip;
                        if (intersect(eye, endPoint, ip)) {
                            _center = ip;
                            _distance = (ip - eye).length();
                            hitFound = true;
                        }
                    }
                }

                // note LookAt = inv(CF)*inv(RM)*inv(T) which is equivalent to:
                // inv(R) = CF*LookAt.

                osg::Matrixd rotation_matrix = osg::Matrixd::lookAt(eye, center, up);

                _rotation = rotation_matrix.getRotate().inverse();

                CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
                _previousUp = getUpVector(coordinateFrame);
                clampOrientation();
            }

            void CameraController::doRotate(double dx, double dy) {
                // rotate camera.
                osg::Vec3 axis;
                double angle;

                double px0 = _ga_t0->getXnormalized();
                double py0 = _ga_t0->getYnormalized();

                double px1 = _ga_t1->getXnormalized();
                double py1 = _ga_t1->getYnormalized();

                trackball(axis, angle, px1, py1, px0, py0);

                osg::Quat new_rotate;
                new_rotate.makeRotate(angle, axis);

                _rotation = _rotation * new_rotate;
                clampOrientation();
            }

            void CameraController::doZoom(double dx, double dy) {
                using namespace ews::util;
                // zoom model.
                const double scale = 1.0f + dy;
                _distance = imposeBounds(MIN_ZOOM_DISTANCE, _distance * scale, MAX_ZOOM_DISTANCE);
            }

            void CameraController::doPan(double dx, double dy) {
                // pan model.
                double scale = -0.3f*_distance;

                osg::Matrixd rotation_matrix;
                rotation_matrix.makeRotate(_rotation);

                // compute look vector.
                osg::Vec3d lookVector = -getUpVector(rotation_matrix);
                osg::Vec3d sideVector = getSideVector(rotation_matrix);
                osg::Vec3d upVector = getFrontVector(rotation_matrix);

                // CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
                // osg::Vec3d localUp = getUpVector(coordinateFrame);
                osg::Vec3d localUp = _previousUp;

                osg::Vec3d forwardVector =localUp^sideVector;
                sideVector = forwardVector^localUp;

                forwardVector.normalize();
                sideVector.normalize();

                osg::Vec3d dv = forwardVector * (dy*scale) + sideVector * (dx*scale);

                _center += dv;

                // need to recompute the intersection point along the look vector.

                bool hitFound = false;

                if (_node.valid()) {

                    // now reorientate the coordinate frame to the frame coords.
                    CoordinateFrame coordinateFrame =  getCoordinateFrame(_center);

                    // need to reintersect with the terrain
                    double distance = _node->getBound().radius()*0.25f;

                    osg::Vec3d ip1;
                    osg::Vec3d ip2;
                    bool hit_ip1 = intersect(_center, _center + getUpVector(coordinateFrame) * distance, ip1);
                    bool hit_ip2 = intersect(_center, _center - getUpVector(coordinateFrame) * distance, ip2);
                    if (hit_ip1)  {
                        if (hit_ip2) {
                            _center = (_center-ip1).length2() < (_center-ip2).length2() ?
                            ip1 :
                            ip2;

                            hitFound = true;
                        }
                        else {
                            _center = ip1;
                            hitFound = true;
                        }
                    }
                    else if (hit_ip2) {
                        _center = ip2;
                        hitFound = true;
                    }

                    if (!hitFound) {
                        // ??
                        osg::notify(INFO)<<"CameraController unable to intersect"<<std::endl;
                    }

                    coordinateFrame = getCoordinateFrame(_center);
                    osg::Vec3d new_localUp = getUpVector(coordinateFrame);

                    osg::Quat pan_rotation;
                    pan_rotation.makeRotate(localUp, new_localUp);

                    if (!pan_rotation.zeroRotation()) {
                        _rotation = _rotation * pan_rotation;
                        _previousUp = new_localUp;
                        osg::notify(osg::NOTICE)<<"Rotating from "<<localUp<<" to "<<new_localUp<<"  angle = "<<acos(localUp*new_localUp/(localUp.length()*new_localUp.length()))<<std::endl;
//                        clampOrientation();
                    }
                    else {
                        osg::notify(osg::INFO)<<"New up orientation nearly inline - no need to rotate"<<std::endl;
                    }
                }
            }

            bool CameraController::calcMovement() {
                // return if less then two events have been added.
                if (_ga_t0.get() == NULL || _ga_t1.get() == NULL) return false;

                double dx = _ga_t0->getXnormalized() - _ga_t1->getXnormalized();
                double dy = _ga_t0->getYnormalized() - _ga_t1->getYnormalized();

                // return if there is no movement.
                if (dx==0 && dy==0) return false;

                unsigned int buttonMask = _ga_t1->getButtonMask();
                if (buttonMask==GUIEventAdapter::LEFT_MOUSE_BUTTON) {
                    doRotate(dx, dy);
                    return true;
                }
                else if (buttonMask==GUIEventAdapter::MIDDLE_MOUSE_BUTTON ||
                         buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON|GUIEventAdapter::RIGHT_MOUSE_BUTTON)) {
                    doPan(dx, dy);
                    return true;
                }
                else if (buttonMask==GUIEventAdapter::RIGHT_MOUSE_BUTTON) {
                    doZoom(dx, dy);
                    return true;
                }

                return false;
            }

            void CameraController::clampOrientation() {
                osg::Matrixd rotation_matrix;
                rotation_matrix.makeRotate(_rotation);

                osg::Vec3d lookVector = -getUpVector(rotation_matrix);
                osg::Vec3d upVector = getFrontVector(rotation_matrix);

                // Don't let it rotate past the X-Y plane
                if (lookVector.z() > 0) {
                    Vec3d newLookVector(lookVector.x(), lookVector.y(), 0);
                    newLookVector.normalize();
                    osg::Quat keepNegative;
                    keepNegative.makeRotate(lookVector, newLookVector);
                    _rotation = _rotation * keepNegative;
                    rotation_matrix.makeRotate(_rotation);
                    lookVector = -getUpVector(rotation_matrix);
                    upVector = getFrontVector(rotation_matrix);
                }
                // Don't let it rotate beyond the Z-axis
                if (upVector.z() < 0) {
                    Vec3d newUpVector(upVector.x(), upVector.y(), 0);
                    newUpVector.normalize();
                    osg::Quat keepPositive;
                    keepPositive.makeRotate(upVector, newUpVector);
                    _rotation = _rotation * keepPositive;
                    rotation_matrix.makeRotate(_rotation);
                    lookVector = -getUpVector(rotation_matrix);
                    upVector = getFrontVector(rotation_matrix);
                }

                CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
                osg::Vec3d localUp = getUpVector(coordinateFrame);
                //osg::Vec3d localUp = _previousUp;

                osg::Vec3d sideVector = lookVector ^ localUp;

                if (sideVector.length()<0.1) {
                    osg::notify(osg::INFO) << "Side vector short " << sideVector.length() << std::endl;
                    sideVector = upVector ^ localUp;
                    sideVector.normalize();
                }

                Vec3d newUpVector = sideVector ^ lookVector;
                newUpVector.normalize();
                osg::Quat rotate_roll;
                rotate_roll.makeRotate(upVector, newUpVector);
                if (!rotate_roll.zeroRotation()) {
                    _rotation = _rotation * rotate_roll;
                }
            }
        }
    }
}
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 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/lattice/trajectory1d/piecewise_jerk_trajectory1d.h"

#include "modules/common/log.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

PiecewiseJerkTrajectory1d::PiecewiseJerkTrajectory1d(
    const double p, const double v, const double a) {
  last_p_ = p;
  last_v_ = v;
  last_a_ = a;
  param_.push_back(0.0);
}

void PiecewiseJerkTrajectory1d::AppendSegment(
    const double jerk, const double param) {
  CHECK_GT(param, FLAGS_lattice_epsilon);

  param_.push_back(param_.back() + param);

  segments_.emplace_back(last_p_, last_v_, last_a_, jerk, param);

  last_p_ = segments_.back().end_position();

  last_v_ = segments_.back().end_velocity();

  last_a_ = segments_.back().end_acceleration();
}

double PiecewiseJerkTrajectory1d::Evaluate(const std::uint32_t order,
    const double param) const {
  CHECK(param >= 0.0 && param <= param_.back());

  auto it_lower = std::lower_bound(param_.begin(), param_.end(), param);

  if (it_lower == param_.begin()) {
    return segments_.front().Evaluate(order, param);
  } else {
    auto index = std::distance(param_.begin(), it_lower) - 1;
    return segments_[index].Evaluate(order, param - param_[index]);
  }
}

double PiecewiseJerkTrajectory1d::ParamLength() const {
  return param_.back();
}

std::string PiecewiseJerkTrajectory1d::ToString() const {
  return "";
}

/**
void PiecewiseJerkTrajectory1d::AppendSegment(
    const double p1, const double v1, const double a1,
    const double param) {
  CHECK_GT(param, FLAGS_lattice_epsilon);
  const auto& last_segment = segments_.back();
  double last_p = last_segment.end_position();
  double last_v = last_segment.end_velocity();
  double last_a = last_segment.end_acceleration();
  double jerk = (a1 - last_a) / param;
  segments_.emplace_back(last_p, last_v, last_a, jerk, param);
}
**/

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: modified Evaluate function in PiecewiseJerkTrajectory1d<commit_after>/******************************************************************************
 * Copyright 2018 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/lattice/trajectory1d/piecewise_jerk_trajectory1d.h"

#include "modules/common/log.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

PiecewiseJerkTrajectory1d::PiecewiseJerkTrajectory1d(
    const double p, const double v, const double a) {
  last_p_ = p;
  last_v_ = v;
  last_a_ = a;
  param_.push_back(0.0);
}

void PiecewiseJerkTrajectory1d::AppendSegment(
    const double jerk, const double param) {
  CHECK_GT(param, FLAGS_lattice_epsilon);

  param_.push_back(param_.back() + param);

  segments_.emplace_back(last_p_, last_v_, last_a_, jerk, param);

  last_p_ = segments_.back().end_position();

  last_v_ = segments_.back().end_velocity();

  last_a_ = segments_.back().end_acceleration();
}

double PiecewiseJerkTrajectory1d::Evaluate(const std::uint32_t order,
    const double param) const {
  CHECK(param >= 0.0 && param <= param_.back());

  auto it_lower = std::lower_bound(param_.begin(), param_.end(), param);

  int index = std::max(0,
      static_cast<int>(std::distance(param_.begin(), it_lower)) - 1);

  return segments_[index].Evaluate(order, param - param_[index]);
}

double PiecewiseJerkTrajectory1d::ParamLength() const {
  return param_.back();
}

std::string PiecewiseJerkTrajectory1d::ToString() const {
  return "";
}

/**
void PiecewiseJerkTrajectory1d::AppendSegment(
    const double p1, const double v1, const double a1,
    const double param) {
  CHECK_GT(param, FLAGS_lattice_epsilon);
  const auto& last_segment = segments_.back();
  double last_p = last_segment.end_position();
  double last_v = last_segment.end_velocity();
  double last_a = last_segment.end_acceleration();
  double jerk = (a1 - last_a) / param;
  segments_.emplace_back(last_p, last_v, last_a, jerk, param);
}
**/

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


<commit_before>/**
 * \file RLSFilter.cpp
 */

#include "RLSFilter.h"

#include <cstdint>
#include <iostream>
#include <stdexcept>

#include <Eigen/Core>

namespace ATK
{
  template<typename DataType_>
  class RLSFilter<DataType_>::RLSFilterImpl
  {
  public:
    typedef Eigen::Matrix<DataType_, Eigen::Dynamic, Eigen::Dynamic> PType;
    typedef Eigen::Matrix<DataType_, Eigen::Dynamic, 1> wType;
    typedef Eigen::Map<const wType> xType;

    RLSFilterImpl(std::size_t size)
      :P(PType::Identity(size, size) / size), w(wType::Zero(size, DataType_(1))), memory(.99)
    {
    }

    void learn(const xType& x, DataType_ target, DataType_ actual)
    {
      auto alpha = target - actual;
      auto xreverse = x.reverse();

      wType g = (P * xreverse) / (memory + xreverse.transpose() * P * xreverse);
      PType pupdate = (g * (xreverse.transpose() * P));
      w = w + alpha * g;
      P = (P - (pupdate + pupdate.transpose()) / 2) * memory;
    }

    void set_P(const DataType_* P)
    {
      this->P = Eigen::Map<const PType>(P, this->P.rows(), this->P.cols());
    }

    const DataType_* get_P() const
    {
      return P.data();
    }

    void set_w(const DataType_* w)
    {
      this->w = xType(w, this->w.rows(), 1);
    }

    const DataType_* get_w() const
    {
      return w.data();
    }

    PType P;
    wType w;
    DataType memory;
  };

  template<typename DataType_>
  RLSFilter<DataType_>::RLSFilter(std::size_t size)
  :Parent(1, 1), global_size(size), impl(new RLSFilterImpl(size)), learning(false)
  {
    input_delay = size;
  }

  template<typename DataType_>
  RLSFilter<DataType_>::~RLSFilter()
  {
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_size(std::size_t size)
  {
    if(size == 0)
    {
      throw std::out_of_range("Size must be strictly positive");
    }

    impl->P = RLSFilterImpl::PType::Identity(size, size) / size;
    impl->w = RLSFilterImpl::wType(size, 1);
    input_delay = size+1;
    this->global_size = size;
  }

  template<typename DataType_>
  std::size_t RLSFilter<DataType_>::get_size() const
  {
    return global_size;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_memory(DataType_ memory)
  {
    if(memory >= 1)
    {
      throw std::out_of_range("Memory must be less than 1");
    }
    if(memory <= 0)
    {
      throw std::out_of_range("Memory must be strictly positive");
    }

    impl->memory = memory;
  }

  template<typename DataType_>
  DataType_ RLSFilter<DataType_>::get_memory() const
  {
    return impl->memory;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_learning(bool learning)
  {
    this->learning = learning;
  }

  template<typename DataType_>
  bool RLSFilter<DataType_>::get_learning() const
  {
    return learning;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::process_impl(std::size_t size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];

    for(std::size_t i = 0; i < size; ++i)
    {
      RLSFilterImpl::xType x(input - global_size + i, global_size, 1);

      // compute next sample
      output[i] = impl->w.transpose() * x.reverse();

      if(learning)
      {
        //update w and P
        impl->learn(x, input[i], output[i]);
      }
    }
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_P(const DataType_* P)
  {
     impl->set_P(P);
  }

  template<typename DataType_>
  const DataType_* RLSFilter<DataType_>::get_P() const
  {
    return impl->get_P();
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_w(const DataType_* w)
  {
    impl->set_w(w);
  }

  template<typename DataType_>
  const DataType_* RLSFilter<DataType_>::get_w() const
  {
    return impl->get_w();
  }

  template class RLSFilter<float>;
  template class RLSFilter<double>;
}
<commit_msg>Fix typename compile on Linux/OS X<commit_after>/**
 * \file RLSFilter.cpp
 */

#include "RLSFilter.h"

#include <cstdint>
#include <iostream>
#include <stdexcept>

#include <Eigen/Core>

namespace ATK
{
  template<typename DataType_>
  class RLSFilter<DataType_>::RLSFilterImpl
  {
  public:
    typedef Eigen::Matrix<DataType_, Eigen::Dynamic, Eigen::Dynamic> PType;
    typedef Eigen::Matrix<DataType_, Eigen::Dynamic, 1> wType;
    typedef Eigen::Map<const wType> xType;

    RLSFilterImpl(std::size_t size)
      :P(PType::Identity(size, size) / size), w(wType::Zero(size, DataType_(1))), memory(.99)
    {
    }

    void learn(const xType& x, DataType_ target, DataType_ actual)
    {
      auto alpha = target - actual;
      auto xreverse = x.reverse();

      wType g = (P * xreverse) / (memory + xreverse.transpose() * P * xreverse);
      PType pupdate = (g * (xreverse.transpose() * P));
      w = w + alpha * g;
      P = (P - (pupdate + pupdate.transpose()) / 2) * memory;
    }

    void set_P(const DataType_* P)
    {
      this->P = Eigen::Map<const PType>(P, this->P.rows(), this->P.cols());
    }

    const DataType_* get_P() const
    {
      return P.data();
    }

    void set_w(const DataType_* w)
    {
      this->w = xType(w, this->w.rows(), 1);
    }

    const DataType_* get_w() const
    {
      return w.data();
    }

    PType P;
    wType w;
    DataType memory;
  };

  template<typename DataType_>
  RLSFilter<DataType_>::RLSFilter(std::size_t size)
  :Parent(1, 1), global_size(size), impl(new RLSFilterImpl(size)), learning(false)
  {
    input_delay = size;
  }

  template<typename DataType_>
  RLSFilter<DataType_>::~RLSFilter()
  {
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_size(std::size_t size)
  {
    if(size == 0)
    {
      throw std::out_of_range("Size must be strictly positive");
    }

    impl->P = RLSFilterImpl::PType::Identity(size, size) / size;
    impl->w = typename RLSFilterImpl::wType(size, 1);
    input_delay = size+1;
    this->global_size = size;
  }

  template<typename DataType_>
  std::size_t RLSFilter<DataType_>::get_size() const
  {
    return global_size;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_memory(DataType_ memory)
  {
    if(memory >= 1)
    {
      throw std::out_of_range("Memory must be less than 1");
    }
    if(memory <= 0)
    {
      throw std::out_of_range("Memory must be strictly positive");
    }

    impl->memory = memory;
  }

  template<typename DataType_>
  DataType_ RLSFilter<DataType_>::get_memory() const
  {
    return impl->memory;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_learning(bool learning)
  {
    this->learning = learning;
  }

  template<typename DataType_>
  bool RLSFilter<DataType_>::get_learning() const
  {
    return learning;
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::process_impl(std::size_t size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];

    for(std::size_t i = 0; i < size; ++i)
    {
      typename RLSFilterImpl::xType x(input - global_size + i, global_size, 1);

      // compute next sample
      output[i] = impl->w.transpose() * x.reverse();

      if(learning)
      {
        //update w and P
        impl->learn(x, input[i], output[i]);
      }
    }
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_P(const DataType_* P)
  {
     impl->set_P(P);
  }

  template<typename DataType_>
  const DataType_* RLSFilter<DataType_>::get_P() const
  {
    return impl->get_P();
  }

  template<typename DataType_>
  void RLSFilter<DataType_>::set_w(const DataType_* w)
  {
    impl->set_w(w);
  }

  template<typename DataType_>
  const DataType_* RLSFilter<DataType_>::get_w() const
  {
    return impl->get_w();
  }

  template class RLSFilter<float>;
  template class RLSFilter<double>;
}
<|endoftext|>