text:
// Copyright (C) 2018 Thejaka Amila Kanewala, Marcin Zalewski, Andrew Lumsdaine.

// Boost Software License - Version 1.0 - August 17th, 2003

// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:

// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

//  Authors: Thejaka Kanewala
//           Andrew Lumsdaine

#ifndef BOOST_GRAPH_DISTRIBUTED_CONTROL_CHAOTIC
#define BOOST_GRAPH_DISTRIBUTED_CONTROL_CHAOTIC

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <am++/counter_coalesced_message_type.hpp>
#include <am++/transport.hpp>
#include <am++/detail/thread_support.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/parallel/thread_support.hpp> // for compare_and_swap

#include <queue>
#include <algorithm> // for std::min, std::max
#include <iostream>
#include <atomic>
#include <tuple>
#include <climits>

namespace boost {
namespace graph {
namespace distributed {

template<typename Graph,
	 typename DistanceMap,
	 typename EdgeWeightMap,
	 typename WorkStats,
	 typename MessageGenerator = amplusplus::simple_generator<amplusplus::counter_coalesced_message_type_gen> >
class distributed_control_chaotic {
  // Type definitions for Vertex and Distance
  // Vertex information is in graph_traits::vertex_descriptor
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  // The distance is in the property traits of the edge
  typedef typename property_traits<EdgeWeightMap>::value_type Distance;
  // This is going to be our message
  typedef std::pair<Vertex, Distance> vertex_distance_data;

  // Default comparer
  // TODO: MZ: We can probably turn of sorting as discussed.  Sorting will occur in the priority queue.  Once we want to try intra-buffer sorting we can revisit this. - this can be done using the preprocessor macro. I guess once we integrate Jesun's code on command line switch we dont need the preprocessor macro
  struct default_comparer {
    bool operator()(const vertex_distance_data& vd1, const vertex_distance_data& vd2) {
      return vd1.second > vd2.second;
    }
  };

  // Owner map type - To calculate which vertex belong to which node
  typedef typename boost::property_map<Graph, vertex_owner_t>::const_type OwnerMap;

  // The handler, forward decleration
  struct vertex_distance_handler;

  typedef typename MessageGenerator::template call_result<
    vertex_distance_data,
    vertex_distance_handler,
    vertex_distance_owner<OwnerMap, vertex_distance_data>,
    amplusplus::idempotent_combination_t<boost::parallel::minimum<Distance>, Distance> >::type
  RelaxMessage;

public:

  distributed_control_chaotic(Graph& g,
                              DistanceMap distance_map,
                              EdgeWeightMap weight_map,
                              amplusplus::transport &t,
                              int offs,
                              WorkStats& stats,
                              MessageGenerator message_gen =
                              MessageGenerator(amplusplus::counter_coalesced_message_type_gen(1 << 12)),
                              unsigned int flushFreq = 2000)
  : dummy_first_member_for_init_order((amplusplus::register_mpi_datatype<vertex_distance_data>(), 0)),
    graph(g),
    transport(t),
    core_offset(offs),
    work_stats(stats),
    distance_map(distance_map),
    weight_map(weight_map),
    owner_map(get(vertex_owner, g)),
    relax_msg(message_gen,
	      transport,
	      vertex_distance_owner<OwnerMap, vertex_distance_data>(owner_map),
	      amplusplus::idempotent_combination(boost::parallel::minimum<Distance>(), std::numeric_limits<Distance>::max())),
    flushFrequency(flushFreq)
  {
    initialize();
  }

#ifdef AMPLUSPLUS_PRINT_HIT_RATES
  typedef std::pair<unsigned long long, unsigned long long> cache_stats;
  cache_stats
  get_cache_stats() {
    cache_stats stats(0, 0);
    for(size_t i = 0; i < relax_msg.counters.hits.size(); ++i) {
      stats.first += relax_msg.counters.hits[i];
      stats.second += relax_msg.counters.tests[i];
    }
    return stats;
  }
#endif // AMPLUSPLUS_PRINT_HIT_RATES

  void set_source(Vertex s) { source = s; }
  // What is actually getting called is this operator
  void operator() (int tid) { run(source, tid); }
  void run(Vertex s, int tid = 0);
  time_type get_start_time() { return start_time; }
  /*void print_in_edge_map() {
    typename graph_traits<Graph>::vertex_iterator i, end;
    unsigned int k = 0;
    //    std::pair<vertex_iter, vertex_iter> vp;
    //for (vp = vertices(graph); vp.first != vp.second; ++vp.first)
    for(tie(i, end) = vertices(graph); i != end; ++i) {
      ++k;
      unsigned int val = processed_in_edge_count_map[*i];
      std::cout << "The vertex " << k << " is visited " << val << " times." << std::endl;
      if (k == 100000)
	break;
    }
    }*/
private:
  void initialize();
  void relax(const vertex_distance_data&);
  void handle_queue(const int, amplusplus::transport::end_epoch_request&);

  const int dummy_first_member_for_init_order; // Unused
  const Graph& graph;
  amplusplus::transport& transport;
  int core_offset;
  WorkStats& work_stats;
  DistanceMap distance_map;
  EdgeWeightMap weight_map;
  //ProcessedInEdgeCountMap processed_in_edge_count_map;
  OwnerMap owner_map;
  Vertex source;
  unsigned int flushFrequency;
  RelaxMessage relax_msg;
  shared_ptr<amplusplus::detail::barrier> t_bar;
  unsigned int in_edge_factor;
  time_type start_time;
};

#define DISTRIBUTE_CONTROL_CHAOTIC_PARMS					\
  typename Graph, typename DistanceMap, typename EdgeWeightMap, typename WorkStats, typename MessageGenerator

#define DISTRIBUTE_CONTROL_CHAOTIC_TYPE						\
  distributed_control_chaotic<Graph, DistanceMap, EdgeWeightMap, WorkStats, MessageGenerator>

template<DISTRIBUTE_CONTROL_CHAOTIC_PARMS> void
DISTRIBUTE_CONTROL_CHAOTIC_TYPE::initialize() {
  relax_msg.set_handler(vertex_distance_handler(*this));

  // Initialize distance labels
  BGL_FORALL_VERTICES_T(v, graph, Graph) {
    put(distance_map, v, (std::numeric_limits<Distance>::max)());
  }
}

template<DISTRIBUTE_CONTROL_CHAOTIC_PARMS> void
DISTRIBUTE_CONTROL_CHAOTIC_TYPE::run(Vertex s, int tid) {

  AMPLUSPLUS_WITH_THREAD_ID(tid) {
    if (0 == tid) {
      int nthreads = transport.get_nthreads();

      // Set the number of threads to the barrier
      t_bar.reset(new amplusplus::detail::barrier(nthreads));
    }

    // This barrier acts as a temporary barrier until we can be sure t_bar is initialized
    { amplusplus::scoped_epoch epoch(transport); }

    t_bar->wait();
    if (pin(tid+core_offset) != 0) {
      std::cerr << "[ERROR] Unable to pin current thread to "
		<< "core : " << tid << std::endl;
      assert(false);
    }

    // wait till all threads are pinned
    t_bar->wait();
    { amplusplus::scoped_epoch epoch(transport); }

    validate_thread_core_relation();

    start_time = get_time();
    // Start the algorithm
    {
      amplusplus::scoped_epoch epoch(transport);
      // If this is main thread and if node is the owner of the vertex do relax
      // for source with distance 0
      if (get(owner_map, s) == transport.rank() && tid == 0) {
	relax(vertex_distance_data(s, 0));
      }
    }

  }
#ifdef PRINT_DEBUG
  std::cerr<<"Done with run\n";
#endif
}

template<DISTRIBUTE_CONTROL_CHAOTIC_PARMS> void
DISTRIBUTE_CONTROL_CHAOTIC_TYPE::relax(const vertex_distance_data& vd) {

#ifdef PBGL2_PRINT_WORK_STATS
  work_stats.increment_edges(amplusplus::detail::get_thread_id());
#endif

  using boost::parallel::val_compare_and_swap;

  Vertex v = vd.first;
  Distance new_distance = vd.second;

  // we are processing an incoming edge for vertex v
  // increase incoming edge count
  //++processed_in_edge_count_map[v];

  // get existing distance
  // need last_old_dist to atomically update distance map
  Distance old_dist = get(distance_map, v), last_old_dist;

  // check whether we got a better distance if we got better
  // distance update the distance map
  // credit : took from delta_stepping_shortest_paths.hpp
  while (new_distance < old_dist) {
    last_old_dist = old_dist;
    old_dist = val_compare_and_swap(&distance_map[v], old_dist, new_distance);
    if (last_old_dist == old_dist) {
      // we got a better distance - insert it to work queue
#ifdef PBGL2_PRINT_WORK_STATS
      int tid = amplusplus::detail::get_thread_id();
      if(old_dist < std::numeric_limits<Distance>::max()) {
	work_stats.increment_invalidated(tid);
      } else {
	work_stats.increment_useful(tid);
      }
#endif
      BGL_FORALL_OUTEDGES_T(v, e, graph, Graph) {
	Vertex u = target(e, graph);
	Distance we = get(weight_map, e);
	relax_msg.send(vertex_distance_data(u, new_distance + we));
      }

      break;
    }
  }

#ifdef PBGL2_PRINT_WORK_STATS
  work_stats.increment_rejected(amplusplus::detail::get_thread_id());
#endif
}

template<DISTRIBUTE_CONTROL_CHAOTIC_PARMS>
struct DISTRIBUTE_CONTROL_CHAOTIC_TYPE::vertex_distance_handler {
  vertex_distance_handler() : self(NULL) {}
  vertex_distance_handler(distributed_control_chaotic& self) : self(&self) {}

  void operator() (const vertex_distance_data& data) const {
    self->relax(data);
  }

protected:
  distributed_control_chaotic* self;
};

} // end namespace distributed
} // end namespace graph
} // end namespace boost

#endif

max_stars_repo_name: thejkane/AGM

max_issues_repo_name: thejkane/AGM

max_forks_repo_name: thejkane/AGM


text:
#include <basix/e-lagrange.h>
#include <basix/quadrature.h>
#include <boost/program_options.hpp>
#include <cmath>
#include <dolfinx.h>
#include <dolfinx/io/XDMFFile.h>
#include <iostream>

#include <cublas_v2.h>
#include <cuda_profiler_api.h>

// Helper functions
#include "precompute.hpp"
#include <cuda/allocator.hpp>
#include <cuda/array.hpp>
#include <cuda/la.hpp>
#include <cuda/scatter.hpp>
#include <cuda/transform.hpp>
#include <cuda/utils.hpp>

using namespace dolfinx;
namespace po = boost::program_options;

void assert_cublas(cudaError_t e) {
  if (e != cudaSuccess)
    throw std::runtime_error(" Unable to allocate memoy - cublas error");
}
int main(int argc, char* argv[]) {

  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "print usage message")(
      "size", po::value<std::size_t>()->default_value(32))(
      "degree", po::value<int>()->default_value(1));

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).allow_unregistered().run(),
            vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 0;
  }

  const std::size_t Nx = vm["size"].as<std::size_t>();
  const int degree = vm["degree"].as<int>();

  common::subsystem::init_logging(argc, argv);
  common::subsystem::init_mpi(argc, argv);
  {
    // MPI
    MPI_Comm mpi_comm{MPI_COMM_WORLD};
    int rank = utils::set_device(mpi_comm);

    // Create cublas handle
    cublasHandle_t handle;
    cublasCreate(&handle);

    // Read mesh and mesh tags
    std::array<std::array<double, 3>, 2> p = {{{0.0, 0.0, 0.0}, {1.0, 1.0, 1.0}}};
    std::array<std::size_t, 3> n = {Nx, Nx, Nx};
    auto mesh = std::make_shared<mesh::Mesh>(mesh::create_box(
        mpi_comm, p, n, mesh::CellType::hexahedron, mesh::GhostMode::none));

    // Create a Basix continuous Lagrange element of given degree
    basix::FiniteElement e = basix::element::create_lagrange(
        mesh::cell_type_to_basix_type(mesh::CellType::hexahedron), degree,
        basix::element::lagrange_variant::equispaced, false);

    // Create a scalar function space
    std::shared_ptr<fem::FunctionSpace> V
        = std::make_shared<fem::FunctionSpace>(fem::create_functionspace(mesh, e, 1));
    auto idxmap = V->dofmap()->index_map;

    int ncells = mesh->topology().index_map(3)->size_local();
    int ndofs = e.dim();

    fem::Function<double> u(V);
    // Interpolate sin(2 \pi x[0]) in the scalar Lagrange finite element space
    constexpr double PI = xt::numeric_constants<double>::PI;
    u.interpolate([PI](auto&& x) { return xt::sin(2 * PI * xt::row(x, 0)); });

    CUDA::allocator<double> allocator{};
    la::Vector<double, decltype(allocator)> x(idxmap, 1, allocator);
    la::Vector<double, decltype(allocator)> y(idxmap, 1, allocator);

    auto uarray = u.x()->array();
    std::copy(uarray.begin(), uarray.end(), x.mutable_array().begin());

    linalg::prefetch(0, x);
    linalg::prefetch(0, y);

    // =====================================
    // Tabulate basis functions at quadrature points
    // 1 - Tabulate quadrature points and weights
    int q = 2 * degree; // Quadrature degree
    auto cell = basix::cell::type::hexahedron;
    auto quad = basix::quadrature::type::gauss_jacobi;
    auto [points, weights] = basix::quadrature::make_quadrature(quad, cell, q);

    std::int32_t nquads = weights.size();
    std::int32_t Ne = ncells * ndofs;
    std::int32_t Nq = ncells * nquads;

    // 2 - Tabulate basis functions
    xt::xtensor<double, 4> basis = e.tabulate(0, points);
    xt::xtensor<double, 2> _phi = xt::view(basis, 0, xt::all(), xt::all(), 0);
    cuda::array<double> phi(ndofs * nquads);
    phi.set(_phi);
    _phi = xt::transpose(_phi);
    cuda::array<double> phiT(ndofs * nquads);
    phiT.set(_phi);

    // =====================================
    // Get dofmap for the gather/scatter
    // Copy dofmap data to device
    cuda::array<std::int32_t> dofmap(Ne);
    const std::vector<std::int32_t>& dof_array = V->dofmap()->list().array();
    dofmap.set(dof_array);

    // =====================================
    // Compute determinant of jacobian at quadrature points
    xt::xtensor<double, 4> J = compute_jacobian(mesh, points);
    xt::xtensor<double, 2> _detJ = compute_jacobian_determinant(J);
    for (std::size_t i = 0; i < _detJ.shape(0); i++)
      for (std::size_t j = 0; j < _detJ.shape(1); j++)
        _detJ(i, j) = _detJ(i, j) * weights[j];
    cuda::array<double> detJ(Nq);
    detJ.set(_detJ);

    // Allocate memory for working arrays on device
    cuda::array<double> ue(Ne);
    cuda::array<double> uq(Nq);
    cuda::array<double> xe(Ne);

    double alpha = 1;
    double beta = 0;

    double t = MPI_Wtime();
    // =====================================
    // Apply gather operator Ue = G u
    // Ue <- u[dofmap]
    // From global dof vector to element based dof vector
    gather(ue.size(), dofmap.data(), x.array().data(), ue.data(), 512);

    // =====================================
    // Apply operator B^T D B to Ue
    // Uq^ = B Ue^T
    cublasDgemm(handle, CUBLAS_OP_N, CUBLAS_OP_N, nquads, ncells, ndofs, &alpha,
                phiT.data(), nquads, ue.data(), ndofs, &beta, uq.data(), nquads);
    // Uq = detJ .* Uq
    transform1(Ne, uq.data(), detJ.data(), uq.data(), 512);
    // Xe^T = B^T Uq^T
    cublasDgemm(handle, CUBLAS_OP_N, CUBLAS_OP_N, ndofs, ncells, nquads, &alpha,
                phi.data(), ndofs, uq.data(), nquads, &beta, xe.data(), ndofs);
    // =====================================
    // Apply scatter operator
    // x[dofmap] <- Xe
    // From element based dof vector to global dof vector
    scatter(xe.size(), dofmap.data(), xe.data(), y.mutable_array().data(), 512);
    cudaDeviceSynchronize();
    t = MPI_Wtime() - t;

    std::cout << y.norm() << std::endl;

    std::cout << "Number of cells: " << ncells;
    std::cout << "\nNumber of dofs: " << ndofs;
    std::cout << "\nNumber of quads: " << nquads;
    double ops = 4 * ncells * nquads * ndofs + ncells * nquads;
    std::cout << "\n#Elapsed Time: " << t << std::endl;
    std::cout << "\nDOF/s: " << V->dofmap()->index_map->size_local() / t;
    std::cout << std::endl;
  }

  common::subsystem::finalize_mpi();
  return 0;
}

ext: cpp

max_stars_repo_name: Excalibur-SLE/wave-fenics

max_issues_repo_name: Excalibur-SLE/wave-fenics

max_forks_repo_name: Excalibur-SLE/wave-fenics


text:
///////////////////////////////////////////////////////////////////////////////
//
//  Copyright (C) 2008-2012  Artyom Beilis (Tonkikh) <artyomtnk@yahoo.com>
//
//  See accompanying file COPYING.TXT file for licensing details.
//
///////////////////////////////////////////////////////////////////////////////
#if defined(__sun)
#define _POSIX_PTHREAD_SEMANTICS
#endif
#include "tcp_cache_server.h"
#include "cache_storage.h"
#ifndef CPPCMS_WIN32
#include <signal.h>
#include "daemonize.h"
#endif
#ifdef CPPCMS_WIN_NATIVE
#include "winservice.h"
#endif
#include <iostream>
#include <stdlib.h>
#include <stdexcept>
#include <booster/shared_ptr.h>
#include <booster/intrusive_ptr.h>
#include <booster/shared_object.h>
#include <booster/thread.h>
#include <booster/log.h>
#include "base_cache.h"
#include "logging.h"
#include <cppcms/session_storage.h>
#include <cppcms/json.h>
#include <cppcms/service.h>
#include <cppcms/cppcms_error.h>

#ifdef CPPCMS_WIN_NATIVE
#include "session_win32_file_storage.h"
#include "winservice.h"
#else
#include "session_posix_file_storage.h"
#endif
#include "session_memory_storage.h"

struct settings {
	std::string ip;
	int port;
	int threads;
	int gc;
	cppcms::json::value config;

	booster::shared_ptr<cppcms::sessions::session_storage_factory> sessions;
	booster::shared_object plugin;
	booster::intrusive_ptr<cppcms::impl::base_cache> cache;

	~settings()
	{
		sessions.reset(); // ensure that sessions object is destroyed before shared object
		cache = 0;
	}

	settings(int argc,char **argv)
	{
		try {
			config = cppcms::service::load_settings(argc,argv);
			setup(config);
		}
		catch(cppcms::cppcms_error const &) {
			help();
			throw;
		}
	}
	void help()
	{
		std::cerr <<
		"usage: cppcms_scale [ -c config.js ] [ parameters ]\n"
		"  -c config.js       JSON, Configuration file, also parameters may\n"
		"                     be set via command line rather then the file\n"
		" --ip=IP             IP to bind, default 0.0.0.0\n"
		" --port=N            Port to bind, mandatory\n"
		" --threads=N         Worker threads, default = # HW CPU\n"
		" --cache-limit=N     The size of the cache in items\n"
		" --session-storage=(memory|files|external)\n"
		"                     Session storage module\n"
		" --session-gc=N      The frequency of garbage collection\n"
		" --session-dir=/path The location of files for session storage\n"
		" --session-shared_object=/path\n"
		"                     The shared object/dll that is used as external\n"
		"                     storage"
		" --session-module=name\n"
		"                     The name of the module for example mymod for\n"
		"                     an external storage, renamed to shared object\n"
		"                     according to OS conventions, like libmymod.so\n"
#ifndef CPPCMS_WIN32
		" --daemon-enable=(true|false)\n"
		"                     Run the process as daemon, default false\n"
		" --daemon-lock=/path Location of the lock file that keeps process pid\n"
		" --daemon-user=user  User that the daemon should run under\n"
		" --daemon-group=grp  Group that the daemon should run under\n"
		" --daemon-chroot=/path\n"
		"                     Run the service in chroot jain\n"
		" --daemon-fdlimit=N  The limit of file descriptors for the service\n"
#endif
#ifdef CPPCMS_WIN_NATIVE
		" --winservice-mode=(install|uninstall)\n"
		"                     Install or uninstall windows service\n"
		" --winservice-name=XXX\n"
		"                     Name of windows service\n"
		" --winservice-display_name=XXX\n"
		"                     The service name shown in the UI\n"
		" --winservice-start=(auto|demand)\n"
		"                     Windows service start mode\n"
		" --winservice-user=XXX\n"
		" --winservice-password=XXX\n"
		"                     The user and password the service would run under\n"
#endif
		<< std::endl;
	}
	void setup(cppcms::json::value const &v)
	{
		ip=v.get("ip","0.0.0.0");
		port=v.get<int>("port");
		threads=v.get("threads",booster::thread::hardware_concurrency());
		int items = v.get("cache.limit",-1);
		if(items!=-1){
			cache = cppcms::impl::thread_cache_factory(items);
		}
		gc=v.get("session.gc",10);
		std::string stor = v.get("session.storage","");
		if(!stor.empty()) {
			#ifndef CPPCMS_NO_GZIP
			if(stor == "files") {
				std::string dir = v.get("session.dir","");
				#ifdef CPPCMS_WIN_NATIVE
				sessions.reset(new cppcms::sessions::session_file_storage_factory(dir));
				#else
				sessions.reset(new cppcms::sessions::session_file_storage_factory(dir,threads,1,false));
				#endif
			}
			else
			#endif //CPPCMS_NO_GZIP
            if(stor == "memory") {
				sessions.reset(new cppcms::sessions::session_memory_storage_factory());
			}
			else if(stor == "external") {
				std::string so = v.get<std::string>("session.shared_object","");
				std::string module = v.get<std::string>("session.module","");
				std::string entry_point = v.get<std::string>("session.entry_point","sessions_generator");
				if(so.empty() && module.empty())
					throw cppcms::cppcms_error(
								"session.storage=external "
								"and neither session.shared_object "
								"nor session.module is defined");
				if(!so.empty() && !module.empty())
					throw cppcms::cppcms_error(
								"both session.shared_object "
								"and session.module are defined");

				if(so.empty()) {
					so = booster::shared_object::name(module);
				}
				std::string error;
				if(!plugin.open(so,error)) {
					throw cppcms::cppcms_error("sessions_pool: failed to load shared object " + so + ": " + error);
				}
				cppcms::sessions::cppcms_session_storage_generator_type f=0;
				plugin.symbol(f,entry_point);
				sessions.reset(f(v.find("session.settings")));
			}
			else
				throw cppcms::cppcms_error("Unknown session.storage:"+stor);
		}
		if(!sessions && !cache) {
			throw cppcms::cppcms_error("Neither cache.limit nor session.storage is defined");
		}
	}

};

#if !defined(CPPCMS_WIN32)
void main_posix(settings &par)
{
	cppcms::impl::daemonizer demon(par.config);

	cppcms::impl::tcp_cache_service srv(
		par.cache,
		par.sessions,
		par.threads,
		par.ip,
		par.port,
		par.gc);

	// Wait for signals for exit
	sigset_t wait_mask;
	sigemptyset(&wait_mask);
	sigaddset(&wait_mask, SIGINT);
	sigaddset(&wait_mask, SIGQUIT);
	sigaddset(&wait_mask, SIGTERM);
	pthread_sigmask(SIG_BLOCK, &wait_mask, 0);
	int sig = 0;
	sigwait(&wait_mask, &sig);

	BOOSTER_NOTICE("cppcms_scale")<<"Catch signal: exiting...";

	srv.stop();
}
#elif defined(CPPCMS_WIN_NATIVE)

static booster::shared_ptr<cppcms::impl::tcp_cache_service> the_server;
static settings *the_settings;
static booster::mutex done_lock;
static booster::condition_variable done_cond;
static bool done;

static void win_prepare()
{
	BOOSTER_NOTICE("cppcms_scale")<<"Starting service...";
	settings &par = *the_settings;
	the_server.reset(
		new cppcms::impl::tcp_cache_service(
			par.cache,
			par.sessions,
			par.threads,
			par.ip,
			par.port,
			par.gc)
	);

}

static void win_stop()
{
	booster::unique_lock<booster::mutex> guard(done_lock);
	done = true;
	done_cond.notify_all();
}

static void win_run()
{
	{
		booster::unique_lock<booster::mutex> guard(done_lock);
		while(!done) {
			done_cond.wait(guard);
		}
	}
	BOOSTER_NOTICE("cppcms_scale")<<"Stopping service...";
	the_server->stop();
	the_server.reset();
	the_settings = 0;
}

void main_win(settings &par,int argc,char **argv)
{
	using cppcms::impl::winservice;

	the_settings = ∥
	winservice::instance().prepare(win_prepare);
	winservice::instance().stop(win_stop);
	winservice::instance().exec(win_run);

	winservice::instance().run(par.config,argc,argv);

}

#endif

#ifdef CPPCMS_WIN32

void main_console(settings &par)
{
	cppcms::impl::tcp_cache_service srv(
		par.cache,
		par.sessions,
		par.threads,
		par.ip,
		par.port,
		par.gc);

	std::cout << "Press any key to stop..." << std::flush;
	std::cin.get();

	srv.stop();
}

#endif

int main(int argc,char **argv)
{
	try
	{
		settings par(argc,argv);
		cppcms::impl::setup_logging(par.config);
#ifndef CPPCMS_WIN32
		main_posix(par);
#elif defined CPPCMS_WIN_NATIVE
		if(cppcms::impl::winservice::is_console(par.config))
			main_console(par);
		else
			main_win(par,argc,argv);
#else // cygwin
		main_console(par);
#endif

	}
	catch(std::exception const &e) {
		BOOSTER_ERROR("cppcms_scale")<<e.what()<<booster::trace(e);
		return 1;
	}
	return 0;
}

max_stars_repo_name: gatehouse/cppcms

max_issues_repo_name: gatehouse/cppcms

max_forks_repo_name: gatehouse/cppcms


text:
/* -*- c++ -*- */
/*
 * Copyright 2021 bk.
 *
 * This is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * This software 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 Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include <boost/make_unique.hpp>
#include "dpdk_impl.h"

namespace gr {
  namespace dpdk {

    dpdk::sptr
    dpdk::make(uint16_t udp_rx_port)
    {
      return gnuradio::get_initial_sptr(new dpdk_impl(udp_rx_port));
    }

    /*
     * The private constructor
     */
    dpdk_impl::dpdk_impl(uint16_t udp_rx_port)
      : gr::sync_block( "dpdk",
                        gr::io_signature::make(0,0,0), //input signature
                        gr::io_signature::make(1, 1, sizeof(gr_complex))), // output signature
                        sample_source(boost::make_unique<DpdkSampleSource>(0, 12)),
                        num_samples_transmitted(0)
    {

    }

    /*
     * Our virtual destructor.
     */
    dpdk_impl::~dpdk_impl()
    {
    }

    int
    dpdk_impl::work(int noutput_items,
        gr_vector_const_void_star &input_items,
        gr_vector_void_star &output_items)
    {
      gr_complex *out = (gr_complex *) output_items[0];
      auto nsamples = sample_source->getSamples(noutput_items, out);
      // Do <+signal processing+>
      // Tell runtime system how many output items we produced.

      //if (num_samples_transmitted)
      //  return WORK_DONE;
      //num_samples_transmitted += noutput_items;
      return nsamples;
    }

  } /* namespace dpdk */
} /* namespace gr */

ext: cc

max_stars_repo_name: bkarl/gnuradio-dpdk

max_issues_repo_name: bkarl/gnuradio-dpdk

max_forks_repo_name: bkarl/gnuradio-dpdk


text:
// Copyright (c) 2015 Delta Prime, Inc.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

// A very simple sample function and appropriate classes to show how an
// asynchronous finite state machine (FSM) may be implemented.

#include <iostream>
#include <thread>

#include <boost/asio.hpp>
#include <boost/bind.hpp>

#include "FiniteStateMachine.hh"

// The namespace where the FiniteStateMachine lives.
//
using namespace Core;

// A very basic audio recorder model which can be stopped, playing or recording.
// It can also skip to the next recording.

namespace AudioRecorder
{
   /**
    * The subject matter information as the name implies holds the domain
    * specific information that the state machine is operating on. This is
    * not strictly necessary, but it can be helpful.
    */
   struct SubjectMatterInformation
   {
      using SongName = std::string;
      using SongDuration = int;

      std::deque< std::tuple< SongName, SongDuration > > songs;

      int current_song;       // the one that is about to play (or is playing)
      int next_song_number;   // for numbering recordings

      SubjectMatterInformation() : current_song( 0 ), next_song_number( 1 ) { }
   };

   /**
    * The state machine of the AudioPlayer object. This is the class in which
    * activity actually occurs.
    */
   class Machine
      :  public FiniteStateMachine<>
   {
   public :

      Machine();
      virtual ~Machine();

      /**
       * Starts the thread on which this machine is executing.
       */
      void StartThread();

      /**
       * Stops the thread.
       */
      void StopThread();

      /**
       * Override to allow this machine to post the event to the thread in a
       * safe manner. This is what makes the machine async capable.
       */
      void EventDeliveryMethod( const Event & event ) override;

      // The calls that the FSM receives from the outside world. They must be
      // converted to appropriate events and delivered to the current state.

      /**
       * Called by the external system when it needs to activate the player.
       */
      void PushPlay() { PostEvent( START_PLAYING_EVENT ); }

      /**
       * Called by the external system when it needs to deactivate the player.
       */
      void PushStop() { PostEvent( STOP_PLAYING_EVENT ); }

      /**
       * Called by the external system to start recording a song.
       */
      void PushRecord() { PostEvent( START_RECORDING_EVENT ); }

      /**
       * Called by the external system to skip to the next song.
       */
      void PushSkipForward() { PostEvent( ADVANCE_TO_NEXT_EVENT ); }

   private :

      // The state machine's event set.
      //
      enum
      {
         START_PLAYING_EVENT,
         STOP_PLAYING_EVENT,
         START_RECORDING_EVENT,
         ADVANCE_TO_NEXT_EVENT,
         TIMEOUT_EVENT
      };

   private :
      // The domain specific information block.
      //
      SubjectMatterInformation info;

      // These store the state numbers associated with the state objects that
      // are to be installed.
      //
      StateID STOPPED_STATE;
      StateID PLAYING_STATE;
      StateID RECORDING_STATE;

      // The states supported by this machine.
      //
      State stopped;
      State playing;
      State recording;

      // These two form the basis of the Boost asynch I/O framework, which is
      // good for event driven work.
      //
      boost::asio::io_service       io;
      boost::asio::io_service::work work;

      // The thread in which the recorder is operating.
      //
      std::thread thread;

      // Use a timer to pretend that songs are playing for some variable amount
      // of time.
      //
      boost::asio::deadline_timer song_timer;

   private :

      // These methods set the entry/exit function objects and install the
      // event handlers and transitions.
      //
      void ConfigureStoppedState();
      void ConfigurePlayingState();
      void ConfigureRecordingState();

      // The methods that actually perform the functions requested by the
      // external system, but in the context of the thread.
      //
      // The work is being done in the FSM not in the individual states.
      // These methods implement algorithms that the states' event handlers can
      // call on event arrival.
      //
      void PerformPlay();
      void PerformStop();
      void PerformRecord();
      void PerformSkip();

      // A utility method to go to the next song.
      //
      void AdvanceToNextSong();

      // Accomodate the operating convention of the boost deadline timer.
      //
      void PerformTimeout( const boost::system::error_code & error )
      {
         if ( error != boost::asio::error::operation_aborted ) PostEvent( TIMEOUT_EVENT );
      }
   };
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
AudioRecorder::Machine::Machine()
   :  io()
   ,  work( io )
   ,  thread()
   ,  song_timer( io )
{
   // The songs to play.
   //
   info.songs.push_back( { "Heartbeat Song", 2 } );
   info.songs.push_back( { "One Hot Mess", 3 } );
   info.songs.push_back( { "Cool", 2 } );

   // The supported states must be registered.
   //
   STOPPED_STATE = RegisterState( & stopped );
   PLAYING_STATE = RegisterState( & playing );
   RECORDING_STATE = RegisterState( & recording );

   // Establish the starting state, this is important.
   //
   SetInitialState( STOPPED_STATE );

   // Configure the states' event handlers and transitions.
   //
   ConfigureStoppedState();
   ConfigurePlayingState();
   ConfigureRecordingState();
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
AudioRecorder::Machine::~Machine()
{
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::StartThread()
{
   try
   {
      this->thread = std::thread( [this](){ this->io.run(); } );
   }
   catch ( const std::system_error & e )
   {
      std::cout << "ERROR: Failed to spawn thread. [" << e.code() << "] " << e.what();
      throw;
   }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::StopThread()
{
   io.stop();
   if ( thread.joinable() )
   {
      thread.join();
   }
}

//------------------------------------------------------------------------------
// The event here is posted to the thread for execution.
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::EventDeliveryMethod( const Event & event )
{
   io.post(
      [this, event]()
         {
            FiniteStateMachine<>::EventDeliveryMethod( event );
         }
      );
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::ConfigureStoppedState()
{
   stopped.SetEntryFunction(
      [this]( const Event & )
         {
            std::cout << "=== Special entry code executed for state: 'STOPPED' ===" << std::endl;
         }
      );

   stopped.SetExitFunction(
      [this]( const Event & )
         {
            std::cout << "=== Special exit code executed for state: 'STOPPED' ===" << std::endl;
         }
      );

   stopped
      .SetEventHandler(
         STOP_PLAYING_EVENT,
         [this]( const Event & )
            {
               std::cout << "Already stopped, nothing to do." << std::endl;
            }
         )
      .SetEventHandler(
         ADVANCE_TO_NEXT_EVENT,
         [this]( const Event & )
            {
               std::cout << "Stopped, can't skip." << std::endl;
            }
         )
      .SetTransition( START_PLAYING_EVENT, PLAYING_STATE )
      .SetTransition( START_RECORDING_EVENT, RECORDING_STATE );
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::ConfigurePlayingState()
{
   playing.SetEntryFunction(
      [this]( const Event & )
         {
            PerformPlay();
         }
      );

   playing.SetExitFunction(
      [this]( const Event & )
         {
            song_timer.cancel();
         }
      );

   playing
      .SetEventHandler(
         START_PLAYING_EVENT,
         [this]( const Event & )
            {
               std::cout << "Already playing, nothing to do." << std::endl;
            }
         )
      .SetEventHandler(
         START_RECORDING_EVENT,
         [this]( const Event & )
            {
               std::cout << "Playing, can't record." << std::endl;
            }
         )
      .SetEventHandler(
         ADVANCE_TO_NEXT_EVENT,
         [this]( const Event & )
            {
               std::cout << "Skipping..." << std::endl;
               PerformSkip();
               PerformPlay();
            }
         )
      .SetEventHandler(
         TIMEOUT_EVENT,
         [this]( const Event & )
            {
               AdvanceToNextSong();
               PerformPlay();
            }
         )
      .SetTransition( STOP_PLAYING_EVENT, STOPPED_STATE );
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::ConfigureRecordingState()
{
   recording.SetEntryFunction(
      [this]( const Event & )
         {
            PerformRecord();
         }
      );

   recording.SetExitFunction(
      [this]( const Event & )
         {
            song_timer.cancel();
         }
      );

   recording
      .SetEventHandler(
            START_PLAYING_EVENT,
            [this]( const Event & event )
               {
                  std::cout << "Recording, can't play." << std::endl;
               }
            )
      .SetEventHandler(
         START_RECORDING_EVENT,
         [this]( const Event & event )
            {
               std::cout << "Already recording, nothing to do." << std::endl;
            }
         )
      .SetEventHandler(
         ADVANCE_TO_NEXT_EVENT,
         [this]( const Event & )
            {
               std::cout << "Recording, can't skip." << std::endl;
            }
         )
      .SetEventHandler(
         TIMEOUT_EVENT,
         [this]( const Event & event )
            {
               PerformRecord();
            }
         )
      .SetTransition( STOP_PLAYING_EVENT, STOPPED_STATE );
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::PerformPlay()
{
   std::cout << "Playing '" << std::get<0>(info.songs[info.current_song]) << "'." << std::endl;

   const int duration = std::get<1>(info.songs[info.current_song]);

   song_timer.expires_from_now( boost::posix_time::seconds(duration) );
   song_timer.async_wait(
      boost::bind(
         & AudioRecorder::Machine::PerformTimeout,
         this,
         boost::asio::placeholders::error
         )
      );
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::PerformStop()
{
   std::cout << "Stopped playing." << std::endl;
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::PerformRecord()
{
   std::ostringstream ostr;
   ostr << "Recorded Song " << info.next_song_number++;
   const int duration = 2 + (info.next_song_number % 2);
   info.songs.push_back( { ostr.str(), duration } );

   std::cout << "Recording '" << ostr.str() << "'." << std::endl;

   song_timer.expires_from_now( boost::posix_time::seconds(duration) );
   song_timer.async_wait(
      boost::bind(
         & AudioRecorder::Machine::PerformTimeout,
         this,
         boost::asio::placeholders::error
         )
      );
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::PerformSkip()
{
   song_timer.cancel();
   AdvanceToNextSong();
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
AudioRecorder::Machine::AdvanceToNextSong()
{
   info.current_song++;
   if ( info.current_song > (info.songs.size() - 1) )
   {
      info.current_song = 0;
   }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void
RunAsynchronousFSM()
{
   std::cout << "<<< Asynchronous FSM Sample Started >>>" << std::endl << std::endl;

   AudioRecorder::Machine audio_recorder;

   audio_recorder.StartThread();

   sleep( 2 );
   audio_recorder.PushStop();
   sleep( 2 );
   audio_recorder.PushPlay();
   sleep( 10 );
   audio_recorder.PushSkipForward();
   sleep( 5 );
   audio_recorder.PushRecord();
   sleep( 1 );
   audio_recorder.PushStop();
   audio_recorder.PushRecord();
   sleep( 5 );
   audio_recorder.PushPlay();
   sleep( 2 );
   audio_recorder.PushStop();
   sleep( 2 );

   audio_recorder.PushPlay();
   sleep( 15 );
   audio_recorder.PushStop();
   sleep( 1 );

   audio_recorder.StopThread();

   std::cout << std::endl << "<<< Asynchronous FSM Sample Finished >>>" << std::endl << std::endl;
}

max_stars_repo_name: laszlo-kiss/FiniteStateMachine

max_issues_repo_name: laszlo-kiss/FiniteStateMachine

max_forks_repo_name: laszlo-kiss/FiniteStateMachine


text:
#ifndef BOOST_ARCHIVE_ARCHIVE_POINTER_OSERIALIZER_POINTER_HPP
#define BOOST_ARCHIVE_ARCHIVE_POINTER_OSERIALIZER_POINTER_HPP

// MS compatible compilers support #pragma once
#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8
// archive_pointer_oserializer.hpp: extenstion of type_info required for
// serialization.

// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com .
// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org for updates, documentation, and revision history.

#include <boost/config.hpp>
#include <boost/archive/detail/basic_serializer.hpp>
#include <boost/archive/detail/basic_pointer_oserializer.hpp>

#include <boost/archive/detail/abi_prefix.hpp> // must be the last header

namespace boost {

namespace serialization {
    class extended_type_info;
} // namespace serialization

namespace archive {
namespace detail {

template<class Archive>
class archive_pointer_oserializer :
    public basic_pointer_oserializer {
protected:
    explicit BOOST_ARCHIVE_OR_WARCHIVE_DECL(BOOST_PP_EMPTY())
    archive_pointer_oserializer(
        const boost::serialization::extended_type_info & eti
    );
    BOOST_ARCHIVE_OR_WARCHIVE_DECL(BOOST_PP_EMPTY())
    // account for bogus gcc warning
    #if defined(__GNUC__)
    virtual
    #endif
    ~archive_pointer_oserializer();
public:
    // return the type_extended save pointer corresponding to a give
    // type_info.  returns NULL, if there is no such instance. This
    // would indicate that the no object of the specified type was saved
    // any where in the code.
    static BOOST_ARCHIVE_OR_WARCHIVE_DECL(const basic_pointer_oserializer *)
    find(
        const boost::serialization::extended_type_info & eti
    );
};

} // namespace detail
} // namespace archive
} // namespace boost

#include <boost/archive/detail/abi_suffix.hpp> // pops abi_suffix.hpp pragmas

#endif // BOOST_ARCHIVE_ARCHIVE_POINTER_OSERIALIZER_POINTER_HPP

ext: hpp

max_stars_repo_name: jaredhoberock/gotham

max_issues_repo_name: jaredhoberock/gotham

max_forks_repo_name: jaredhoberock/gotham


text:
 /*
landmarks in images read from a webcam and points that drew by the program.
 */
#include <dlib/opencv.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <stdio.h>
#include <opencv2/video/tracking.hpp>

using namespace cv;
using namespace dlib;

void print_usage() {
    std::cout << "Usage:" << std::endl;
    std::cout << "./face_landmark_detection [path/to/shape_predictor_68_face_landmarks.dat]" << std::endl;
}

// calculate the variance between the current points and last points
double cal_dist_diff(std::vector<cv::Point2f> curPoints, std::vector<cv::Point2f> lastPoints) {
    double variance = 0.0;
    double sum = 0.0;
    std::vector<double> diffs;
    if (curPoints.size() == lastPoints.size()) {
        for (int i = 0; i < curPoints.size(); i++) {
            double diff = std::sqrt(std::pow(curPoints[i].x - lastPoints[i].x, 2.0) + std::pow(curPoints[i].y - lastPoints[i].y, 2.0));
            sum += diff;
            diffs.push_back(diff);
        }
        double mean = sum / diffs.size();
        for (int i = 0; i < curPoints.size(); i++) {
            variance += std::pow(diffs[i] - mean, 2);
        }
        return variance / diffs.size();
    }
    return variance;
}

// Shows the results that optimized with Optical Flow method only
int main(int argc, char** argv) {
    try {
        if (argc > 2) {
            print_usage();
            return 0;
        }

        std::cout << "Press q to exit." << std::endl;

        cv::VideoCapture cap(0);

        if (!cap.isOpened()) {
            std::cerr << "Unable to connect to camera" << std::endl;
            return 1;
        }

        // Load face detection and pose estimation models.
        frontal_face_detector detector = get_frontal_face_detector();
        shape_predictor pose_model;

        if (argc == 2) {
            deserialize(argv[1]) >> pose_model;
        } else {
            deserialize("../../data/shape_predictor_68_face_landmarks.dat") >> pose_model;
        }

        // Initialize the points of last frame
        std::vector<cv::Point2f> last_object;
        for (int i = 0; i < 68; ++i) {
            last_object.push_back(cv::Point2f(0.0, 0.0));
        }

        double scaling = 0.5;
        int flag = -1;
        int count = 0;

        // Initialize Optical Flow
        cv::Mat prevgray, gray;
        std::vector<cv::Point2f> prevTrackPts;
        std::vector<cv::Point2f> nextTrackPts;
        for (int i = 0; i < 68; i++) {
            prevTrackPts.push_back(cv::Point2f(0, 0));
        }

        // Grab and process frames until the main window is closed by the user.
        while(true) {
            // Grab a frame
            cv::Mat raw;
            cap >> raw;

            // Resize
            cv::Mat tmp;
            cv::resize(raw, tmp, cv::Size(), scaling, scaling);

            //Flip
            cv::Mat temp;
            cv::flip(tmp, temp, 1);

            // Turn OpenCV's Mat into something dlib can deal with.  Note that this just
            // wraps the Mat object, it doesn't copy anything.  So cimg is only valid as
            // long as temp is valid.  Also don't do anything to temp that would cause it
            // to reallocate the memory which stores the image as that will make cimg
            // contain dangling pointers.  This basically means you shouldn't modify temp
            // while using cimg.
            cv_image<bgr_pixel> cimg(temp);

            // Detect faces, load the vertexes as vector
            std::vector<dlib::rectangle> faces = detector(cimg);

            // Find the pose of each face.
            std::vector<full_object_detection> shapes;
            for (unsigned long i = 0; i < faces.size(); ++i) {
                shapes.push_back(pose_model(cimg, faces[i]));
            }

            // We cannot modify temp so we clone a new one
            cv::Mat face = temp.clone();
            cv::Mat face_2 = temp.clone();
            cv::Mat face_3 = temp.clone();
            cv::Mat frame = temp.clone();

            if (flag == -1) {
                cvtColor(frame, prevgray, CV_BGR2GRAY);
                const full_object_detection& d = shapes[0];
                for (int i = 0; i < d.num_parts(); i++) {
                    prevTrackPts[i].x = d.part(i).x();
                    prevTrackPts[i].y = d.part(i).y();
                }
                flag = 1;
            }

            if (shapes.size() == 1) {
                cvtColor(frame, gray, CV_BGR2GRAY);
                if (prevgray.data) {
                    std::vector<uchar> status;
                    std::vector<float> err;
                    calcOpticalFlowPyrLK(prevgray, gray, prevTrackPts, nextTrackPts, status, err);
                    std::cout << "variance:" << cal_dist_diff(prevTrackPts, nextTrackPts) << std::endl;
                    if (cal_dist_diff(prevTrackPts, nextTrackPts) > 1.0) {
                        const full_object_detection& d = shapes[0];
                        for (int i = 0; i < d.num_parts(); i++) {
                            cv::circle(frame, cv::Point2f(d.part(i).x(), d.part(i).y()), 2, cv::Scalar(0, 255, 255), -1);
                            nextTrackPts[i].x = d.part(i).x();
                            nextTrackPts[i].y = d.part(i).y();
                        }
                    } else {
                        for (int i = 0; i < nextTrackPts.size(); i++) {
                            cv::circle(frame, nextTrackPts[i], 2, cv::Scalar(0, 0, 255), -1);
                        }
                    }
                    std::swap(prevTrackPts, nextTrackPts);
                    //prevTrackPts = nextTrackPts;
                    std::swap(prevgray, gray);
                    }
                }

            cv::imshow("OpticalFlow", frame);

            char key = cv::waitKey(1);
            if (key == 'q') {
                break;
            }
        }
    } catch(serialization_error& e) {
        print_usage();
    } catch(std::exception& e) {
        std::cout << e.what() << std::endl;
    }
    return 0;
}

max_issues_repo_name: zhucebuliaolongchuan/KalmanFilter


text:
/**
 * \file Utilities.cpp
 */

#include <ATK/Core/Utilities.h>

#include <cstdint>
#include <vector>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int16_t_int16_t )
{
  std::vector<std::int16_t> input(3);
  std::vector<std::int16_t> output(3);
  input[0] = std::numeric_limits<std::int16_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int16_t>::max();

  ATK::ConversionUtilities<std::int16_t, std::int16_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(input[0], output[0]);
  BOOST_CHECK_EQUAL(input[1], output[1]);
  BOOST_CHECK_EQUAL(input[2], output[2]);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int16_t_int32_t )
{
  std::vector<std::int16_t> input(3);
  std::vector<std::int32_t> output(3);
  input[0] = std::numeric_limits<std::int16_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int16_t>::max();

  ATK::ConversionUtilities<std::int16_t, std::int32_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<std::int32_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<std::int32_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int16_t_int64_t )
{
  std::vector<std::int16_t> input(3);
  std::vector<int64_t> output(3);
  input[0] = std::numeric_limits<std::int16_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int16_t>::max();

  ATK::ConversionUtilities<std::int16_t, int64_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<int64_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<int64_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int16_t_float )
{
  std::vector<std::int16_t> input(3);
  std::vector<float> output(3);
  input[0] = std::numeric_limits<std::int16_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int16_t>::max();

  ATK::ConversionUtilities<std::int16_t, float>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_CLOSE(-1, output[0], 0.01);
  BOOST_CHECK_CLOSE(0, output[1], 0.01);
  BOOST_CHECK_CLOSE(1, output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int16_t_double )
{
  std::vector<std::int16_t> input(3);
  std::vector<double> output(3);
  input[0] = std::numeric_limits<std::int16_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int16_t>::max();

  ATK::ConversionUtilities<std::int16_t, double>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_CLOSE(-1, output[0], 0.01);
  BOOST_CHECK_CLOSE(0, output[1], 0.01);
  BOOST_CHECK_CLOSE(1, output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int32_t_int16_t )
{
  std::vector<std::int32_t> input(3);
  std::vector<std::int16_t> output(3);
  input[0] = std::numeric_limits<std::int32_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int32_t>::max();

  ATK::ConversionUtilities<std::int32_t, std::int16_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<std::int16_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_EQUAL(std::numeric_limits<std::int16_t>::max(), output[2]);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int32_t_int32_t )
{
  std::vector<std::int32_t> input(3);
  std::vector<std::int32_t> output(3);
  input[0] = std::numeric_limits<std::int32_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int32_t>::max();

  ATK::ConversionUtilities<std::int32_t, std::int32_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(input[0], output[0]);
  BOOST_CHECK_EQUAL(input[1], output[1]);
  BOOST_CHECK_EQUAL(input[2], output[2]);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int32_t_int64_t )
{
  std::vector<std::int32_t> input(3);
  std::vector<int64_t> output(3);
  input[0] = std::numeric_limits<std::int32_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int32_t>::max();

  ATK::ConversionUtilities<std::int32_t, int64_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<int64_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<int64_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int32_t_float )
{
  std::vector<std::int32_t> input(3);
  std::vector<float> output(3);
  input[0] = std::numeric_limits<std::int32_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int32_t>::max();

  ATK::ConversionUtilities<std::int32_t, float>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(-1, output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(1, output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int32_t_double )
{
  std::vector<std::int32_t> input(3);
  std::vector<double> output(3);
  input[0] = std::numeric_limits<std::int32_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<std::int32_t>::max();

  ATK::ConversionUtilities<std::int32_t, double>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(-1, output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(1, output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int64_t_int16_t )
{
  std::vector<int64_t> input(3);
  std::vector<std::int16_t> output(3);
  input[0] = std::numeric_limits<int64_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<int64_t>::max() - (static_cast<int64_t>(1) << 48);

  ATK::ConversionUtilities<int64_t, std::int16_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<std::int16_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<std::int16_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int64_t_int32_t )
{
  std::vector<int64_t> input(3);
  std::vector<std::int32_t> output(3);
  input[0] = std::numeric_limits<int64_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<int64_t>::max() - (1L << 16);

  ATK::ConversionUtilities<int64_t, std::int32_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<std::int32_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<std::int32_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int64_t_int64_t )
{
  std::vector<int64_t> input(3);
  std::vector<int64_t> output(3);
  input[0] = std::numeric_limits<int64_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<int64_t>::max();

  ATK::ConversionUtilities<int64_t, int64_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(input[0], output[0]);
  BOOST_CHECK_EQUAL(input[1], output[1]);
  BOOST_CHECK_EQUAL(input[2], output[2]);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int64_t_float )
{
  std::vector<int64_t> input(3);
  std::vector<float> output(3);
  input[0] = std::numeric_limits<int64_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<int64_t>::max();

  ATK::ConversionUtilities<int64_t, float>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(-1, output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(1, output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_int64_t_double )
{
  std::vector<int64_t> input(3);
  std::vector<double> output(3);
  input[0] = std::numeric_limits<int64_t>::min();
  input[1] = 0;
  input[2] = std::numeric_limits<int64_t>::max();

  ATK::ConversionUtilities<int64_t, double>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(-1, output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(1, output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_float_int16_t )
{
  std::vector<float> input(3);
  std::vector<std::int16_t> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = .9999;

  ATK::ConversionUtilities<float, std::int16_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<std::int16_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<std::int16_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_float_int32_t )
{
  std::vector<float> input(3);
  std::vector<std::int32_t> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = .999999;

  ATK::ConversionUtilities<float, std::int32_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<std::int32_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<std::int32_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_float_int64_t )
{
  std::vector<float> input(3);
  std::vector<int64_t> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = .9999999;

  ATK::ConversionUtilities<float, int64_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<int64_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<int64_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_float_float )
{
  std::vector<float> input(3);
  std::vector<float> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = 1;

  ATK::ConversionUtilities<float, float>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(input[0], output[0]);
  BOOST_CHECK_EQUAL(input[1], output[1]);
  BOOST_CHECK_EQUAL(input[2], output[2]);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_float_double )
{
  std::vector<float> input(3);
  std::vector<double> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = 1;

  ATK::ConversionUtilities<float, double>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(input[0], output[0]);
  BOOST_CHECK_EQUAL(input[1], output[1]);
  BOOST_CHECK_EQUAL(input[2], output[2]);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_double_int16_t )
{
  std::vector<double> input(3);
  std::vector<std::int16_t> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = .9999;

  ATK::ConversionUtilities<double, std::int16_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<std::int16_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<std::int16_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_double_int32_t )
{
  std::vector<double> input(3);
  std::vector<std::int32_t> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = .999999;

  ATK::ConversionUtilities<double, std::int32_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<std::int32_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<std::int32_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_double_int64_t )
{
  std::vector<double> input(3);
  std::vector<int64_t> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = .9999999;

  ATK::ConversionUtilities<double, int64_t>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(std::numeric_limits<int64_t>::min(), output[0]);
  BOOST_CHECK_EQUAL(0, output[1]);
  BOOST_CHECK_CLOSE(static_cast<double>(std::numeric_limits<int64_t>::max()), output[2], 0.01);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_double_float )
{
  std::vector<double> input(3);
  std::vector<float> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = 1;

  ATK::ConversionUtilities<double, float>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(input[0], output[0]);
  BOOST_CHECK_EQUAL(input[1], output[1]);
  BOOST_CHECK_EQUAL(input[2], output[2]);
}

BOOST_AUTO_TEST_CASE( Utilities_test_convert_array_double_double )
{
  std::vector<double> input(3);
  std::vector<double> output(3);
  input[0] = -1;
  input[1] = 0;
  input[2] = 1;

  ATK::ConversionUtilities<double, double>::convert_array(&input[0], &output[0], 3);

  BOOST_CHECK_EQUAL(input[0], output[0]);
  BOOST_CHECK_EQUAL(input[1], output[1]);
  BOOST_CHECK_EQUAL(input[2], output[2]);
}

max_stars_repo_name: D-J-Roberts/AudioTK

max_forks_repo_name: D-J-Roberts/AudioTK


text:
//   GAMBIT: Global and Modular BSM Inference Tool
//   *********************************************
///  \file
///
///  A simple C++ wrapper for the MPI C bindings.
///  It is by no means comprehensive, and is just
///  intended to simplify some MPI commands within
///  Gambit.
///
///  Extend as needed.
///
///  Note: Some C++ bindings already exist, but
///  it appears that they are deprecated in really
///  new versions of the MPI standard, and don't
///  offer functionality over the C bindings
///  anyway. I have nevertheless mirrored the names
///  of some of the deprecated C++ bindings, in
///  case it helps some people who were used to
///  them.
///
///  There is also boost/mpi, but it is a compiled
///  library and we have been avoiding those.
///
///  You can remove the error handlers if you want
///  these wrappers to be independent of GAMBIT.
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Ben Farmer
///          (benjamin.farmer@fysik.su.se)
///  \date 2015 Apr
///
///  \author Pat Scott
///          (p.scott@imperial.ac.uk)
///  \date 2015 Jun
///
///  *********************************************

// The WITH_MPI macro determines whether MPI is used or not.
// It is defined at compile time by cmake with -DWITH_MPI.
// The contents of this file are ignored if MPI is disabled.

#ifdef WITH_MPI
#ifndef __mpiwrapper_hpp__
#define __mpiwrapper_hpp__

#include <sstream>
#include <iostream>
#include <type_traits>
#include <chrono>
#include <mpi.h>
#include <boost/utility/enable_if.hpp>

#include "gambit/Utils/standalone_error_handlers.hpp"
#include "gambit/Utils/local_info.hpp"
#include "gambit/Utils/util_macros.hpp"

// I wanted to keep the GAMBIT logger separate from this code so that it
// would be more streamlined for using elsewhere. But the logger is very
// useful for debugging, so this preprocessor flag can be used to turn
// it on and off
// Though it might already be included via the error handlers anyway.
#define USE_GAMBIT_LOGGER

#ifdef USE_GAMBIT_LOGGER
  #include "gambit/Logs/logger.hpp"
  #define LOGGER logger() << LogTags::utils << LogTags::info
#else
  #define LOGGER std::cerr
  #define EOM std::endl
#endif

/// Provide template specialisation of get_mpi_data_type only if the requested type hasn't been used to define one already.
#define SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(TYPEDEFD_TYPE, RETURN_MPI_TYPE)                                                   \
      template<typename T>                                                                                \
      struct get_mpi_data_type<T, typename boost::enable_if_c< std::is_same<T, TYPEDEFD_TYPE>::value                      && \
                                                               !std::is_same<char, TYPEDEFD_TYPE>::value                  && \
                                                               !std::is_same<short, TYPEDEFD_TYPE>::value                 && \
                                                               !std::is_same<int, TYPEDEFD_TYPE>::value                   && \
                                                               !std::is_same<long, TYPEDEFD_TYPE>::value                  && \
                                                               !std::is_same<long long, TYPEDEFD_TYPE>::value             && \
                                                               !std::is_same<unsigned char, TYPEDEFD_TYPE>::value         && \
                                                               !std::is_same<unsigned short, TYPEDEFD_TYPE>::value        && \
                                                               !std::is_same<unsigned int, TYPEDEFD_TYPE>::value          && \
                                                               !std::is_same<unsigned long, TYPEDEFD_TYPE>::value         && \
                                                               !std::is_same<unsigned long long, TYPEDEFD_TYPE>::value    && \
                                                               !std::is_same<float, TYPEDEFD_TYPE>::value                 && \
                                                               !std::is_same<double, TYPEDEFD_TYPE>::value                && \
                                                               !std::is_same<long double, TYPEDEFD_TYPE>::value           && \
                                                               !std::is_same<bool, TYPEDEFD_TYPE>::value>::type >            \
      {                                                                                \
         static MPI_Datatype type() { return RETURN_MPI_TYPE; }                                                              \
      };                                                                                \

/// Trigger debugging output when messages sent/recvd
//#define MPI_MSG_DEBUG

namespace Gambit
{

   namespace GMPI
   {

      /// Mapping from (basic) C++ types to MPI datatypes
      /// Based on of "get_hdf5_data_type" in hdf5tools.hpp
      /// Base template is left undefined in order to raise
      /// a compile error if specialisation doesn't exist.
      template<typename T, typename Enable=void>
      struct get_mpi_data_type;

      /// Overload to work with arrays
      template<typename T, size_t SIZE>
      struct get_mpi_data_type<T[SIZE]> { static MPI_Datatype type() { return get_mpi_data_type<T>::type(); } };

      /// True types
      /// @{
      template<> struct get_mpi_data_type<char>              { static MPI_Datatype type() { return MPI_CHAR;               } };
      template<> struct get_mpi_data_type<short>             { static MPI_Datatype type() { return MPI_SHORT;              } };
      template<> struct get_mpi_data_type<int>               { static MPI_Datatype type() { return MPI_INT;                } };
      template<> struct get_mpi_data_type<long>              { static MPI_Datatype type() { return MPI_LONG;               } };
      template<> struct get_mpi_data_type<long long>         { static MPI_Datatype type() { return MPI_LONG_LONG;          } };
      template<> struct get_mpi_data_type<unsigned char>     { static MPI_Datatype type() { return MPI_UNSIGNED_CHAR;      } };
      template<> struct get_mpi_data_type<unsigned short>    { static MPI_Datatype type() { return MPI_UNSIGNED_SHORT;     } };
      template<> struct get_mpi_data_type<unsigned int>      { static MPI_Datatype type() { return MPI_UNSIGNED;           } };
      template<> struct get_mpi_data_type<unsigned long>     { static MPI_Datatype type() { return MPI_UNSIGNED_LONG;      } };
      template<> struct get_mpi_data_type<unsigned long long>{ static MPI_Datatype type() { return MPI_UNSIGNED_LONG_LONG; } };
      template<> struct get_mpi_data_type<float>             { static MPI_Datatype type() { return MPI_FLOAT;              } };
      template<> struct get_mpi_data_type<double>            { static MPI_Datatype type() { return MPI_DOUBLE;             } };
      template<> struct get_mpi_data_type<long double>       { static MPI_Datatype type() { return MPI_LONG_DOUBLE;        } };
      /// @}

      /// Typedef'd types; enabled only where they differ from the true types, and where the relevant constants have been
      /// defined in the linked MPI implementation.
      /// @{
      #ifdef MPI_INT8_T
        SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(int8_t,   MPI_INT8_T  )
      #endif
      #ifdef MPI_UINT8_T
        SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(uint8_t,  MPI_UINT8_T )
      #endif
      #ifdef MPI_INT16_T
        SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(int16_t,  MPI_INT16_T )
      #endif
      #ifdef MPI_UINT16_T
        SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(uint16_t, MPI_UINT16_T)
      #endif
      #ifdef MPI_INT32_T
        SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(int32_t,  MPI_INT32_T )
      #endif
      #ifdef MPI_UINT32_T
        SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(uint32_t, MPI_UINT32_T)
      #endif
      #ifdef MPI_INT64_T
        SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(int64_t,  MPI_INT64_T )
      #endif
      #ifdef MPI_UINT64_T
        SPECIALISE_MPI_DATA_TYPE_IF_NEEDED(uint64_t, MPI_UINT64_T)
      #endif
      /// @}

      /// Main "Communicator" class
      class EXPORT_SYMBOLS Comm
      {
         public:
            /// Default Constructor; attaches to MPI_COMM_WORLD
            Comm();

            /// Constructor which copies existing communicator into boundcomm
            Comm(const MPI_Comm& comm, const std::string& name);

            /// Destructor
            ~Comm();

            /// As name
            void check_for_undelivered_messages();

            /// Duplicate existing communicator
            /// (NOTE, this is a collective operation on all procceses)
            void dup(const MPI_Comm& comm, const std::string& newname);

            /// Get total number of MPI tasks in this communicator group
            int Get_size() const;

            /// Get "rank" (ID number) of current task in this communicator group
            int Get_rank() const;

            /// Get name of communicator group (for error messages)
            std::string Get_name() const;

            /// Prevent further executation until all members of the bound communicator group enter the call
            void Barrier()
            {
              #ifdef MPI_MSG_DEBUG
              std::cout<<"rank "<<Get_rank()<<": Barrier() called"<<std::endl;
              #endif

              int errflag;
              errflag = MPI_Barrier(boundcomm);
              if(errflag!=0) {
                 std::ostringstream errmsg;
                 errmsg << "Error performing MPI_Barrier! Received error flag: "<<errflag;
                 utils_error().raise(LOCAL_INFO, errmsg.str());
              }

              #ifdef MPI_MSG_DEBUG
              std::cout<<"rank "<<Get_rank()<<": Barrier() passed"<<std::endl;
              #endif
           }

            /// Blocking receive
            ///  void*        buf      - memory address in which to store received message
            ///  int          count    - number of elements in message
            ///  MPI_Datatype datatype - datatype of each message element
            ///  int          source   - rank of sending (receiving?) process
            ///  int          tag      - message tag
            ///  MPI_status*  status   - struct containing data about the received message
            /// Returns:
            ///  MPI_status - struct containing data about the received message
            void Recv(void *buf /*out*/, int count, MPI_Datatype datatype,
                                  int source, int tag,
                                  MPI_Status *in_status=NULL /*out*/)
            {
              #ifdef MPI_MSG_DEBUG
              std::cout<<"rank "<<Get_rank()<<": Recv() called (count="<<count<<", source="<<source<<", tag="<<tag<<")"<<std::endl;
              #endif
              int errflag;
              errflag = MPI_Recv(buf, count, datatype, source, tag, boundcomm, in_status == NULL ? MPI_STATUS_IGNORE : in_status);
              if(errflag!=0)
              {
                std::ostringstream errmsg;
                errmsg << "Error performing MPI_Recv! Received error flag: "<<errflag;
                utils_error().raise(LOCAL_INFO, errmsg.str());
              }
              #ifdef MPI_MSG_DEBUG
              std::cout<<"rank "<<Get_rank()<<": Recv() finished "<<std::endl;
              #endif
            }

            /// Templated blocking receive to automatically determine types
            template<class T>
            void Recv(T *buf /*out*/, int count,
                      int source, int tag,
                      MPI_Status *status=NULL /*out*/)
            {
               static const MPI_Datatype datatype = get_mpi_data_type<T>::type();
               Recv(buf, count, datatype, source, tag, status);
            }

            /// Blocking send
            void Send(void *buf, int count, MPI_Datatype datatype,
                                  int destination, int tag)
            {
               #ifdef MPI_MSG_DEBUG
               std::cout<<"rank "<<Get_rank()<<": Send() called (count="<<count<<", destination="<<destination<<", tag="<<tag<<")"<<std::endl;
               #endif
               int errflag;
               errflag = MPI_Send(buf, count, datatype, destination, tag, boundcomm);
               if(errflag!=0) {
                 std::ostringstream errmsg;
                 errmsg << "Error performing MPI_Send! Received error flag: "<<errflag;
                 utils_error().raise(LOCAL_INFO, errmsg.str());
               }
               #ifdef MPI_MSG_DEBUG
               std::cout<<"rank "<<Get_rank()<<": Send() finished"<<std::endl;
               #endif
            }

            /// Templated blocking send
            template<class T>
            void Send(T *buf, int count,
                      int destination, int tag)
            {
               static const MPI_Datatype datatype = get_mpi_data_type<T>::type();
               Send(buf, count, datatype, destination, tag);
            }

            /// Non-blocking send
            void Isend(void *buf, int count, MPI_Datatype datatype,
                                  int destination, int tag,
                                  MPI_Request *request /*out*/)
            {
              #ifdef MPI_MSG_DEBUG
              std::cerr<<"rank "<<Get_rank()<<": Isend() called (count="<<count<<", destination="<<destination<<", tag="<<tag<<")"<<std::endl;
              #endif
              int errflag;
               errflag = MPI_Isend(buf, count, datatype, destination, tag, boundcomm, request);
               if(errflag!=0) {
                 std::ostringstream errmsg;
                 errmsg << "Error performing MPI_Isend! Received error flag: "<<errflag;
                 utils_error().raise(LOCAL_INFO, errmsg.str());
               }
            }

            /// Templated Non-blocking send
            template<class T>
            void Isend(T *buf, int count,
                      int destination, int tag,
                      MPI_Request *request /*out*/)
            {
               static const MPI_Datatype datatype = get_mpi_data_type<T>::type();
               Isend(buf, count, datatype, destination, tag, request);
            }

            /// Blocking wait for e.g. Isend to complete
            //void Wait(MPI_Request *request, MPI_Status *status)
            //{
            //   MPI_Wait(MPI_Request *request, MPI_Status *status)
            // }

            // Probe for messages waiting to be delivered
            bool Iprobe(int source, int tag, MPI_Status* in_status=NULL /*out*/)
            {
              //#ifdef MPI_MSG_DEBUG
              //std::cout<<"rank "<<Get_rank()<<": Iprobe() called (source="<<source<<", tag="<<tag<<")"<<std::endl;
              //#endif
               int errflag;
               int you_have_mail; // C does not have a bool type...
               MPI_Status def_status;
               MPI_Status* status;
               if(in_status!=NULL) {
                 status = in_status;
               } else {
                 status = &def_status;
               }
               MPI_Iprobe(source, 1, boundcomm, &you_have_mail, status);
               errflag = MPI_Iprobe(source, tag, boundcomm, &you_have_mail, status);
               if(errflag!=0) {
                 std::ostringstream errmsg;
                 errmsg << "Error performing MPI_Iprobe! Received error flag: "<<errflag;
                 utils_error().raise(LOCAL_INFO, errmsg.str());
               }
               #ifdef MPI_MSG_DEBUG
               if(you_have_mail!=0) {
                     std::cerr<<"rank "<<Get_rank()<<": Iprobe: Message waiting from process "<<status->MPI_SOURCE<<std::endl;
               }
               #endif
               return (you_have_mail != 0);
            }

            // Perform an Isend to all other processes
            // (using templated non-blocking send repeatedly)
            template<class T>
            void IsendToAll(T *buf, int count, int tag,
                      MPI_Request *in_req=NULL /*out*/)
            {
               MPI_Request def_req;
               MPI_Request* req;
               if(in_req!=NULL) {
                 req = in_req;
               } else {
                 req = &def_req;
               }
               int rank = Get_rank();
               int size = Get_size();
               for(int i=0; i<size; i++)
               {
                  if(i!=rank) Isend(buf, count, i, tag, req);
               }
            }

            template <typename T>
            void Bcast (T &buffer, int count, int root)
            {
                static const MPI_Datatype datatype = get_mpi_data_type<T>::type();

                MPI_Bcast (&buffer, count, datatype, root, boundcomm);
            }

            template<typename T>
            void Scatter (std::vector<T> &sendbuf, T &recvbuf, int root)
            {
                static const MPI_Datatype datatype = get_mpi_data_type<T>::type();

                MPI_Scatter (&sendbuf[0], 1, datatype, &recvbuf, 1, datatype, root, boundcomm);
            }

            template<typename T>
            void Allreduce (T &sendbuf, T &recvbuf, MPI_Op op)
            {
                static const MPI_Datatype datatype = get_mpi_data_type<T>::type();

                MPI_Allreduce (&sendbuf, &recvbuf, 1, datatype, op, boundcomm);
            }

            // Force all processes in this group (possibly all processes in
            // the "WORLD"; implementation dependent) to stop executing.
            // Useful for abnormal termination (since if one processes throws
            // an exception then the others can easily get stuck waiting
            // for messages that will never arrive).
            void Abort()
            {
              std::cerr << "rank "<<Get_rank()<<": Issuing MPI_Abort command, attempting to terminate all processes..." << std::endl;
              MPI_Abort(boundcomm, 1);
            }

            /// Tells master to wait until all other processes pass this function, with the specified MPI tag
            void masterWaitForAll(int tag);

            /// Inverse of the above. Everyone waits for master to pass this (but not for anyone else)
            void allWaitForMaster(int tag);

            /// Everyone waits for master to pass this, and runs "func" periodically while waiting
            void allWaitForMasterWithFunc(int tag, void (*func)());

            /// An implementation of Barrier that will fall through if synchronisation takes too long
            /// Could modify to take a function pointer to run while waiting.
            /// Supply MPI tag to identify each particular barrier.
            /// Returns 'false' if barrier succeeds, 'true' if barrier times out (i.e. answers the question "did the barrier time out?")
            bool BarrierWithTimeout(const std::chrono::duration<double> timeout, const int tag);

            /// This is a fancy barrier that waits a certain amount of time after the FIRST process
            /// enters before unlocking (so that other action can be taken). This means that all the
            /// processes that enter the barrier *do* get synchronised, even if the barrier unlocks.
            /// This helps the synchronisation to be achieved next time.
            bool BarrierWithCommonTimeout(std::chrono::duration<double> timeout,
                                          const int tag_entered,
                                          const int tag_timeleft);

            /// Receive any waiting messages with a given tag from a given source (possibly MPI_ANY_SOURCE)
            /// Need to know what the messages are in order to provide an appropriate Recv buffer (and size)
            /// The last message received will remain in the buffer and may be used (useful if several messages
            /// about the same thing are expected to pile up)
            template<class T>
            void Recv_all(T* buffer, int size, int source, int tag, int max_loops)
            {
              int loop = 0;

              MPI_Status status;
              while(loop<max_loops and Iprobe(source, tag, &status))
              {
                #ifdef SIGNAL_DEBUG
                LOGGER << "Detected message from process "<<status.MPI_SOURCE<<" with tag "<<status.MPI_TAG<<"; doing Recv" << EOM;
                #endif
                MPI_Status recv_status;
                Recv(buffer, size, status.MPI_SOURCE, status.MPI_TAG, &recv_status);
                #ifdef SIGNAL_DEBUG
                LOGGER << "Received message from process "<<status.MPI_SOURCE<<" with tag "<<status.MPI_TAG<<". Discarding any existing message in the output buffer as obsolete..." << EOM;
                #endif
                ++loop;
              }

              if(loop==max_loops)
              {
                std::ostringstream errmsg;
                errmsg << "Error while attempting to clean out unreceived messages from other processes! Received maximum allowed number of messages ("<<loop<<", note that MPI size is "<<Get_size()<<")";
                utils_error().raise(LOCAL_INFO, errmsg.str());
              }

              if(loop>0) LOGGER << "Communicator '"<<myname<<"' received "<<loop<<" messages with tag "<<tag<<". Only the last of these will be readable from the output buffer, the rest were discarded."<<EOM;
            }

            /// A generic place to store a tag commonly used by this communicator
            int mytag = 1;

            /// Get pointer to raw bound communicator
            MPI_Comm* get_boundcomm() { return &boundcomm; }

         private:

            // The MPI communicator to which the current object "talks".
            MPI_Comm boundcomm;

            // A name to identify the communicator group to which this object is bound
            std::string myname;
      };

      /// Check if MPI_Init has been called (it is an error to call it twice)
      EXPORT_SYMBOLS bool Is_initialized();

      /// Initialise MPI
      EXPORT_SYMBOLS void Init();

      /// Check if MPI_Finalize has been called (it is an error to do anything else after this)
      EXPORT_SYMBOLS bool Is_finalized();

      /// Finalize MPI
      EXPORT_SYMBOLS void Finalize();

      /// Prepare for calling MPI_Finalize, but call MPI_abort and exit function if timeout is exceeded.
      EXPORT_SYMBOLS bool PrepareForFinalizeWithTimeout(bool use_mpi_abort);

      /// Nice wrapper for getting the message size from an MPI_status struct.
      /// Provide the type whose MPI_Datatype you want to retrieve as the
      /// template argument.
      template<class T>
      int Get_count(MPI_Status *status)
      {
         static const MPI_Datatype datatype = get_mpi_data_type<T>::type();
         int msgsize;
         MPI_Get_count(status, datatype, &msgsize);
         if(msgsize<0)
         {
            std::ostringstream errmsg;
            errmsg << "Error performing MPI_Get_count! Message size returned negative (value was "<<msgsize<<")! This can happen if the number of bytes received is not a multiple of the size of the specified MPI_Datatype. In other words you may have specified a type that doesn't match the type of the sent message; please double-check this.";
            utils_error().raise(LOCAL_INFO, errmsg.str());
         }
         return msgsize;
      }

      /// @{ Helpers for registration of compound datatypes

      /// Structure to hold an MPI startup function plus metadata
      class MpiIniFunc
      {
        private:
          std::string location;
          std::string name;
          void (*func)();
        public:
          MpiIniFunc(const std::string& l, const std::string& n, void(*f)())
            : location(l)
            , name(n)
            , func(f)
          {}
          void runme()
          {
            (*func)();
          }
          std::string mylocation(){return location;}
          std::string myname    (){return name;}
      };

      /// Struct for adding functions to the 'mpi_ini_functions' map
      ///
      /// This will add functions to the map when it is constructed. Works
      /// on the same idea as the "ini_code" struct, except it doesn't
      /// cause the functions to be run, just "queues them up" so to speak.
      // struct AddMpiIniFunc {
      //   AddMpiIniFunc(const std::string& local_info, const std::string& name, void(*func)());
      // };

      /// @}

   }
}

#endif // include guard
#endif // MPI


text:
#include "math_helper.h"

#include <iostream>
#include <Eigen/LU>
#include <Eigen/Dense>

namespace recon
{

namespace MathHelper
{

void getTopViewHomography(const double* cparams, int src_w, int src_h, int dst_w, int dst_h,
                          const Eigen::Vector4d& plane, Eigen::Matrix3d& H)
{
  double fx = cparams[0];
  double fy = cparams[1];
  double cx = cparams[2];
  double cy = cparams[3];

  Eigen::Vector3d n;
  n << plane[0], plane[1], plane[2];
  double d = plane[3];
  if(d < 0.0) {
    throw "Error!\n";
  }
  std::cout << "n = \n" << n << "\n\n";
  std::cout << "d = " << d << "\n\n";

  Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity();
  K1(0,0) = fx;
  K1(1,1) = fy;
  K1(0,2) = cx;
  K1(1,2) = cy;
  Eigen::Matrix3d K1_inv = K1.inverse();

  Eigen::Vector3d q1a;
  Eigen::Vector3d q1b;
  q1a << (double) 0.0, (double) src_h - 1.0, 1.0;
  q1b << (double) src_w - 1.0, (double) src_h - 1.0, 1.0;
  q1a = K1_inv * q1a;
  q1b = K1_inv * q1b;

  Eigen::Vector3d Q1a, Q1b;
  Q1a = (-d * q1a) / n.dot(q1a);
  Q1b = (-d * q1b) / n.dot(q1b);
  std::cout << "Q1a:\n" << Q1a << "\nQ1b:\n" << Q1b << "\n\n";

  // determine R matrix
  Eigen::Matrix3d R;
  //Eigen::Vector3d Q1diff = Q1a - Q1b;
  Eigen::Vector3d Q1diff = Q1b - Q1a;
  std::cout << Q1diff << "\n";
  R.row(0) = Q1diff / Q1diff.norm();
  R.row(2) = -n;
  R.row(1) = -(R.row(0).cross(R.row(2)));

  std::cout << "0 == " << Q1diff.dot(n) << "\n";

  //double Ty = -10.0 - (R.row(1) * Q1a);
  double Ty = 10.0 - (R.row(1) * Q1a);
  std::cout << "Ty = " << Ty << "\n";
  Eigen::Vector3d t;
  t << 0.0, Ty, 0.0;

  Eigen::Vector3d Q2a = R * Q1a + t;
  Eigen::Vector3d Q2b = R * Q1b + t;
  std::cout << "Q2a:\n" << Q2a << "\nQ2b:\n" << Q2b << "\n\n";;
  double dst_cx = dst_w / 2.0;
  double dst_cy = dst_h / 2.0;
  double f2 = (dst_cy - 1.0) * (Q2a[2] / Q2a[1]);

  Eigen::Matrix3d K2;
  K2 << f2,   0.0,  dst_cx,
        0.0,  f2,   dst_cy,
        0.0,  0.0,  1.0;
  std::cout << "K2:\n" << K2 << "\n";
  std::cout << "q2a:\n" << (K2 * Q2a) / Q2a[2] << "\nq2b:\n" << (K2 * Q2b) / Q2b[2] << "\n\n";;

  //std::cout << "d = " << d << "\n";
  //std::cout << "<n,Q1a> = " << n.dot(Q1a) << "\n\n";
  Eigen::Matrix3d H_n = R - ((t * n.transpose()) / d);
  std::cout << "Q2a_H:\n" << H_n * Q1a << "\n\n";
  H = K2 * H_n * K1_inv;
  Eigen::Vector3d q2a_H = H * q1a;
  q2a_H = q2a_H / q2a_H[2];
  std::cout << "q2a_H:\n" << q2a_H << "\n\n";
}

void debugTopViewHomography(const double* cparams, int src_w, int src_h, int dst_w, int dst_h,
                            const Eigen::Vector4d& plane, pcl::visualization::PCLVisualizer& viewer)
{
  double fx = cparams[0];
  double fy = cparams[1];
  double cx = cparams[2];
  double cy = cparams[3];

  Eigen::Vector3d n;
  n << plane[0], plane[1], plane[2];
  double d = plane[3];
  if(d < 0.0) {
    throw "Error!\n";
  }
  std::cout << "n = \n" << n << "\n\n";
  std::cout << "d = " << d << "\n\n";

  Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity();
  K1(0,0) = fx;
  K1(1,1) = fy;
  K1(0,2) = cx;
  K1(1,2) = cy;
  Eigen::Matrix3d K1_inv = K1.inverse();

  Eigen::Vector3d q1a;
  Eigen::Vector3d q1b;
  q1a << (double) 0.0, (double) src_h - 1.0, 1.0;
  q1b << (double) src_w - 1.0, (double) src_h - 1.0, 1.0;
  q1a = K1_inv * q1a;
  q1b = K1_inv * q1b;

  Eigen::Vector3d Q1a, Q1b;
  Q1a = (-d * q1a) / n.dot(q1a);
  Q1b = (-d * q1b) / n.dot(q1b);
  std::cout << "Q1a:\n" << Q1a << "\nQ1b:\n" << Q1b << "\n\n";

  pcl::PointXYZ pt1, pt2;
  for(int i = 0; i < 3; i++) {
    pt1.data[i] = Q1a[i];
    pt2.data[i] = Q1b[i];
  }
  viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2, 0, 200, 0, "line1");
}

void projectPoint(const double* cam_params, Eigen::Vector3d& point, Eigen::Vector2d& proj)
{
  double fx = cam_params[0];
  double fy = cam_params[1];
  double cx = cam_params[2];
  double cy = cam_params[3];

  proj[0] = point[0] / point[2];
  proj[1] = point[1] / point[2];
  proj[0] = fx * proj[0] + cx;
  proj[1] = fy * proj[1] + cy;
}

void setTransform2DEM(Eigen::Vector4d& plane_model, Eigen::Matrix4d& transform)
{
  Eigen::Vector3d n1, n2;
  for(int i = 0; i < 3; i++) {
    n1[i] = plane_model[i];
    n2[i] = 0.0;
  }
  n2[1] = 1.0;
  // find axis with cross product

  // find angle with dot product

  // move Y with d after rotation
}

void createPolyFromPlane(const Eigen::Vector4d& plane, pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud)
{
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];

  double x[4] = {-20, 20, 20, -20};
  double z[4] = {  0,  0, 100, 100};
  for(int i = 0; i < 4; i++) {
    double y = (a*x[i] + c*z[i] + d) / (-b);
    pcl::PointXYZ pt(x[i], y, z[i]);
    poly_cloud->points.push_back(pt);
  }
}

void triangulate(const double (&cam_params)[5], double x, double y, double disp, cv::Mat& pt3d)
{
  double f = cam_params[0];
  double cx = cam_params[2];
  double cy = cam_params[3];
  double b = cam_params[4];

  pt3d.at<double>(0) = (x - cx) * b / disp;
  pt3d.at<double>(1) = (y - cy) * b / disp;
  pt3d.at<double>(2) = f * b / disp;
  pt3d.at<double>(3) = 1.0;
}

void getSignedDistancesToModel(const Eigen::Vector4d& plane, const pcl::PointCloud<pcl::PointXYZ>::Ptr pc,
                                           std::vector<double>& distances)
{
  distances.assign(pc->points.size(), 0.0);
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];
  double norm = std::sqrt(a*a + b*b + c*c);
  for(size_t i = 0; i < pc->points.size(); i++) {
    pcl::PointXYZ pt = pc->points[i];
    double p = a*pt.x + b*pt.y + c*pt.z + d;
    distances[i] = p / norm;
  }
}

void getDistancesToModel(const Eigen::Vector4d& plane, const pcl::PointCloud<pcl::PointXYZ>::Ptr pc,
                         std::vector<double>& distances)
{
  distances.assign(pc->points.size(), 0.0);
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];
  double norm = std::sqrt(a*a + b*b + c*c);
  for(size_t i = 0; i < pc->points.size(); i++) {
    pcl::PointXYZ pt = pc->points[i];
    double p = a*pt.x + b*pt.y + c*pt.z + d;
    distances[i] = std::abs(p / norm);
  }
}

}

}


text:
/*!
  @file
  @author Shizuko Hattori
*/

#include "ModelLoader_impl.h"
#include "BodyInfo_impl.h"
#include "SceneInfo_impl.h"
#include <boost/filesystem.hpp>
#include <iostream>

using namespace std;
using namespace cnoid;
using namespace OpenHRP;
namespace filesystem = boost::filesystem;

namespace {

//Copy from VRMLParser.cpp
string removeURLScheme(string url)
{
    static const string fileProtocolHeader1("file://");
    static const string fileProtocolHeader2("file:");

    size_t pos = url.find(fileProtocolHeader1);
    if(0 == pos){
        url.erase(0, fileProtocolHeader1.size());
    } else {
        size_t pos = url.find(fileProtocolHeader2);
        if(0 == pos) {
            url.erase(0, fileProtocolHeader2.size());
        }
    }

    // Remove the first slash if the successive part is a Windows drive letter
    if(url.find(":") == 2 && url.find("/") ==0){
        url.erase(0, 1);
    }

    return url;
}

}

ModelLoader_impl::ModelLoader_impl(CORBA::ORB_ptr orb, PortableServer::POA_ptr poa)
    : orb(CORBA::ORB::_duplicate(orb)),
      poa(PortableServer::POA::_duplicate(poa))
{
    os = &cout;
}

ModelLoader_impl::~ModelLoader_impl()
{
    clearData();
}

POA_OpenHRP::BodyInfo* ModelLoader_impl::loadBodyInfoFromModelFile(const string& filename)
{
    BodyInfo_impl* bodyInfoImpl = new BodyInfo_impl(poa);
    bodyInfoImpl->setMessageSink(*os);
    try{
        bodyInfoImpl->loadModelFile(filename);
    }catch(OpenHRP::ModelLoader::ModelLoaderException& ex){
        *os << ex.description << endl;
        throw;
    }

    urlToBodyInfoMap[filename] = bodyInfoImpl;
    const filesystem::path path(filename);
    const time_t last_update = filesystem::last_write_time(path);
    bodyFileUpdateTimeMap[bodyInfoImpl] = last_update;

    return bodyInfoImpl;
}

BodyInfo_ptr ModelLoader_impl::getBodyInfo(const char* url)
{
    const string filename = removeURLScheme(string(url));
    UrlToBodyInfoMap::iterator p = urlToBodyInfoMap.find(filename);
    if(p != urlToBodyInfoMap.end() ){
        const filesystem::path path(filename);
        const time_t last_update = filesystem::last_write_time(path);
        if(last_update == bodyFileUpdateTimeMap[p->second]){
            return p->second->_this();
        }
    }

    return loadBodyInfo(url);
}

BodyInfo_ptr ModelLoader_impl::getBodyInfoEx(const char* url, const OpenHRP::ModelLoader::ModelLoadOption& option)
{
    return getBodyInfo(url);
}

PortableServer::POA_ptr ModelLoader_impl::_default_POA()
{
    return PortableServer::POA::_duplicate(poa);
}

void ModelLoader_impl::clearData()
{
    urlToBodyInfoMap.clear();
    bodyFileUpdateTimeMap.clear();
}

BodyInfo_ptr ModelLoader_impl::loadBodyInfo(const char* url)
{
    const string filename = removeURLScheme(string(url));
    POA_OpenHRP::BodyInfo* bodyInfo = loadBodyInfoFromModelFile(filename);
    return bodyInfo->_this();
}

BodyInfo_ptr ModelLoader_impl::loadBodyInfoEx(const char* url, const OpenHRP::ModelLoader::ModelLoadOption& option)
{
    return loadBodyInfo(url);
}

SceneInfo_ptr ModelLoader_impl::loadSceneInfo(const char* url)
{
    const string filename = removeURLScheme(string(url));
    SceneInfo_impl* sceneInfoImpl = new SceneInfo_impl(poa);
    sceneInfoImpl->setMessageSink(*os);
    try {
            sceneInfoImpl->load(filename);
    }
    catch(OpenHRP::ModelLoader::ModelLoaderException& ex){
        *os << ex.description << endl;
        throw;
    }
    return sceneInfoImpl->_this();
}

void ModelLoader_impl::shutdown()
{
    clearData();
    orb->shutdown(false);
}

max_stars_repo_name: gergondet/choreonoid

max_issues_repo_name: gergondet/choreonoid

max_forks_repo_name: gergondet/choreonoid