text:
// Boost.Function library

//  Copyright Douglas Gregor 2001-2004. 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)

// For more information, see http://www.boost.org

#ifndef BOOST_FUNCTION_BASE_HEADER
#define BOOST_FUNCTION_BASE_HEADER

#include <stdexcept>
#include <string>
#include <memory>
#include <new>
#include <typeinfo>
#include <boost/config.hpp>
#include <boost/assert.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/composite_traits.hpp>
#include <boost/type_traits/is_stateless.hpp>
#include <boost/ref.hpp>
#include <boost/pending/ct_if.hpp>
#include <boost/detail/workaround.hpp>
#ifndef BOOST_NO_SFINAE
#  include "boost/utility/enable_if.hpp"
#else
#  include "boost/mpl/bool.hpp"
#endif
#include <boost/function_equal.hpp>

// Borrowed from Boost.Python library: determines the cases where we
// need to use std::type_info::name to compare instead of operator==.
# if (defined(__GNUC__) && __GNUC__ >= 3) \
 || defined(_AIX) \
 || (   defined(__sgi) && defined(__host_mips))
#  include <cstring>
#  define BOOST_FUNCTION_COMPARE_TYPE_ID(X,Y) \
     (std::strcmp((X).name(),(Y).name()) == 0)
# else
#  define BOOST_FUNCTION_COMPARE_TYPE_ID(X,Y) ((X)==(Y))
#endif

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300 || defined(__ICL) && __ICL <= 600 || defined(__MWERKS__) && __MWERKS__ < 0x2406 && !defined(BOOST_STRICT_CONFIG)
#  define BOOST_FUNCTION_TARGET_FIX(x) x
#else
#  define BOOST_FUNCTION_TARGET_FIX(x)
#endif // not MSVC

#if defined(__sgi) && defined(_COMPILER_VERSION) && _COMPILER_VERSION <= 730 && !defined(BOOST_STRICT_CONFIG)
// Work around a compiler bug.
// boost::python::objects::function has to be seen by the compiler before the
// boost::function class template.
namespace boost { namespace python { namespace objects {
  class function;
}}}
#endif

#if defined (BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)                    \
 || defined(BOOST_BCB_PARTIAL_SPECIALIZATION_BUG)                         \
 || !(BOOST_STRICT_CONFIG || !defined(__SUNPRO_CC) || __SUNPRO_CC > 0x540)
#  define BOOST_FUNCTION_NO_FUNCTION_TYPE_SYNTAX
#endif

#if !BOOST_WORKAROUND(__BORLANDC__, < 0x600)
#  define BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor,Type)              \
      typename ::boost::enable_if_c<(::boost::type_traits::ice_not<          \
                            (::boost::is_integral<Functor>::value)>::value), \
                           Type>::type
#else
// BCC doesn't recognize this depends on a template argument and complains
// about the use of 'typename'
#  define BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor,Type)     \
      ::boost::enable_if_c<(::boost::type_traits::ice_not<          \
                   (::boost::is_integral<Functor>::value)>::value), \
                       Type>::type
#endif

#if !defined(BOOST_FUNCTION_NO_FUNCTION_TYPE_SYNTAX)
namespace boost {

#if defined(__sgi) && defined(_COMPILER_VERSION) && _COMPILER_VERSION <= 730 && !defined(BOOST_STRICT_CONFIG)
// The library shipping with MIPSpro 7.3.1.3m has a broken allocator<void>
class function_base;

template<typename Signature,
         typename Allocator = std::allocator<function_base> >
class function;
#else
template<typename Signature, typename Allocator = std::allocator<void> >
class function;
#endif

template<typename Signature, typename Allocator>
inline void swap(function<Signature, Allocator>& f1,
                 function<Signature, Allocator>& f2)
{
  f1.swap(f2);
}

} // end namespace boost
#endif // have partial specialization

namespace boost {
  namespace detail {
    namespace function {
      /**
       * A union of a function pointer and a void pointer. This is necessary
       * because 5.2.10/6 allows reinterpret_cast<> to safely cast between
       * function pointer types and 5.2.9/10 allows static_cast<> to safely
       * cast between a void pointer and an object pointer. But it is not legal
       * to cast between a function pointer and a void* (in either direction),
       * so function requires a union of the two. */
      union any_pointer
      {
        void* obj_ptr;
        const void* const_obj_ptr;
        void (*func_ptr)();
        char data[1];
      };

      inline any_pointer make_any_pointer(void* o)
      {
        any_pointer p;
        p.obj_ptr = o;
        return p;
      }

      inline any_pointer make_any_pointer(const void* o)
      {
        any_pointer p;
        p.const_obj_ptr = o;
        return p;
      }

      inline any_pointer make_any_pointer(void (*f)())
      {
        any_pointer p;
        p.func_ptr = f;
        return p;
      }

      /**
       * The unusable class is a placeholder for unused function arguments
       * It is also completely unusable except that it constructable from
       * anything. This helps compilers without partial specialization to
       * handle Boost.Function objects returning void.
       */
      struct unusable
      {
        unusable() {}
        template<typename T> unusable(const T&) {}
      };

      /* Determine the return type. This supports compilers that do not support
       * void returns or partial specialization by silently changing the return
       * type to "unusable".
       */
      template<typename T> struct function_return_type { typedef T type; };

      template<>
      struct function_return_type<void>
      {
        typedef unusable type;
      };

      // The operation type to perform on the given functor/function pointer
      enum functor_manager_operation_type {
        clone_functor_tag,
        destroy_functor_tag,
        check_functor_type_tag
      };

      // Tags used to decide between different types of functions
      struct function_ptr_tag {};
      struct function_obj_tag {};
      struct member_ptr_tag {};
      struct function_obj_ref_tag {};
      struct stateless_function_obj_tag {};

      template<typename F>
      class get_function_tag
      {
        typedef typename ct_if<(is_pointer<F>::value),
                            function_ptr_tag,
                            function_obj_tag>::type ptr_or_obj_tag;

        typedef typename ct_if<(is_member_pointer<F>::value),
                            member_ptr_tag,
                            ptr_or_obj_tag>::type ptr_or_obj_or_mem_tag;

        typedef typename ct_if<(is_reference_wrapper<F>::value),
                             function_obj_ref_tag,
                             ptr_or_obj_or_mem_tag>::type or_ref_tag;

      public:
        typedef typename ct_if<(is_stateless<F>::value),
                            stateless_function_obj_tag,
                            or_ref_tag>::type type;
      };

      // The trivial manager does nothing but return the same pointer (if we
      // are cloning) or return the null pointer (if we are deleting).
      template<typename F>
      struct trivial_manager
      {
        static inline any_pointer
        get(any_pointer f, functor_manager_operation_type op)
        {
          switch (op) {
          case clone_functor_tag: return f;

          case destroy_functor_tag:
            return make_any_pointer(reinterpret_cast<void*>(0));

          case check_functor_type_tag:
            {
              std::type_info* t = static_cast<std::type_info*>(f.obj_ptr);
              return BOOST_FUNCTION_COMPARE_TYPE_ID(typeid(F), *t)?
                f
                : make_any_pointer(reinterpret_cast<void*>(0));
            }
          }

          // Clears up a warning with GCC 3.2.3
          return make_any_pointer(reinterpret_cast<void*>(0));
        }
      };

      /**
       * The functor_manager class contains a static function "manage" which
       * can clone or destroy the given function/function object pointer.
       */
      template<typename Functor, typename Allocator>
      struct functor_manager
      {
      private:
        typedef Functor functor_type;

        // For function pointers, the manager is trivial
        static inline any_pointer
        manager(any_pointer function_ptr,
                functor_manager_operation_type op,
                function_ptr_tag)
        {
          if (op == clone_functor_tag)
            return function_ptr;
          else
            return make_any_pointer(static_cast<void (*)()>(0));
        }

        // For function object pointers, we clone the pointer to each
        // function has its own version.
        static inline any_pointer
        manager(any_pointer function_obj_ptr,
                functor_manager_operation_type op,
                function_obj_tag)
        {
#ifndef BOOST_NO_STD_ALLOCATOR
        typedef typename Allocator::template rebind<functor_type>::other
          allocator_type;
        typedef typename allocator_type::pointer pointer_type;
#else
        typedef functor_type* pointer_type;
#endif // BOOST_NO_STD_ALLOCATOR

#  ifndef BOOST_NO_STD_ALLOCATOR
          allocator_type allocator;
#  endif // BOOST_NO_STD_ALLOCATOR

          if (op == clone_functor_tag) {
            functor_type* f =
              static_cast<functor_type*>(function_obj_ptr.obj_ptr);

            // Clone the functor
#  ifndef BOOST_NO_STD_ALLOCATOR
            pointer_type copy = allocator.allocate(1);
            allocator.construct(copy, *f);

            // Get back to the original pointer type
            functor_type* new_f = static_cast<functor_type*>(copy);
#  else
            functor_type* new_f = new functor_type(*f);
#  endif // BOOST_NO_STD_ALLOCATOR
            return make_any_pointer(static_cast<void*>(new_f));
          }
          else {
            /* Cast from the void pointer to the functor pointer type */
            functor_type* f =
              reinterpret_cast<functor_type*>(function_obj_ptr.obj_ptr);

#  ifndef BOOST_NO_STD_ALLOCATOR
            /* Cast from the functor pointer type to the allocator's pointer
               type */
            pointer_type victim = static_cast<pointer_type>(f);

            // Destroy and deallocate the functor
            allocator.destroy(victim);
            allocator.deallocate(victim, 1);
#  else
            delete f;
#  endif // BOOST_NO_STD_ALLOCATOR

            return make_any_pointer(static_cast<void*>(0));
          }
        }
      public:
        /* Dispatch to an appropriate manager based on whether we have a
           function pointer or a function object pointer. */
        static any_pointer
        manage(any_pointer functor_ptr, functor_manager_operation_type op)
        {
          if (op == check_functor_type_tag) {
            std::type_info* type =
              static_cast<std::type_info*>(functor_ptr.obj_ptr);
            return (BOOST_FUNCTION_COMPARE_TYPE_ID(typeid(Functor), *type)?
                    functor_ptr
                    : make_any_pointer(reinterpret_cast<void*>(0)));
          }
          else {
            typedef typename get_function_tag<functor_type>::type tag_type;
            return manager(functor_ptr, op, tag_type());
          }
        }
      };

      // A type that is only used for comparisons against zero
      struct useless_clear_type {};

#ifdef BOOST_NO_SFINAE
      // These routines perform comparisons between a Boost.Function
      // object and an arbitrary function object (when the last
      // parameter is mpl::bool_<false>) or against zero (when the
      // last parameter is mpl::bool_<true>). They are only necessary
      // for compilers that don't support SFINAE.
      template<typename Function, typename Functor>
        bool
        compare_equal(const Function& f, const Functor&, int, mpl::bool_<true>)
        { return f.empty(); }

      template<typename Function, typename Functor>
        bool
        compare_not_equal(const Function& f, const Functor&, int,
                          mpl::bool_<true>)
        { return !f.empty(); }

      template<typename Function, typename Functor>
        bool
        compare_equal(const Function& f, const Functor& g, long,
                      mpl::bool_<false>)
        {
          if (const Functor* fp = f.template target<Functor>())
            return function_equal(*fp, g);
          else return false;
        }

      template<typename Function, typename Functor>
        bool
        compare_equal(const Function& f, const reference_wrapper<Functor>& g,
                      int, mpl::bool_<false>)
        {
          if (const Functor* fp = f.template target<Functor>())
            return fp == g.get_pointer();
          else return false;
        }

      template<typename Function, typename Functor>
        bool
        compare_not_equal(const Function& f, const Functor& g, long,
                          mpl::bool_<false>)
        {
          if (const Functor* fp = f.template target<Functor>())
            return !function_equal(*fp, g);
          else return true;
        }

      template<typename Function, typename Functor>
        bool
        compare_not_equal(const Function& f,
                          const reference_wrapper<Functor>& g, int,
                          mpl::bool_<false>)
        {
          if (const Functor* fp = f.template target<Functor>())
            return fp != g.get_pointer();
          else return true;
        }
#endif // BOOST_NO_SFINAE
    } // end namespace function
  } // end namespace detail

/**
 * The function_base class contains the basic elements needed for the
 * function1, function2, function3, etc. classes. It is common to all
 * functions (and as such can be used to tell if we have one of the
 * functionN objects).
 */
class function_base
{
public:
  function_base() : manager(0)
  {
    functor.obj_ptr = 0;
  }

  // Is this function empty?
  bool empty() const { return !manager; }

  template<typename Functor>
    Functor* target()
    {
      if (!manager) return 0;

      detail::function::any_pointer result =
        manager(detail::function::make_any_pointer(&typeid(Functor)),
                detail::function::check_functor_type_tag);
      if (!result.obj_ptr) return 0;
      else {
        typedef typename detail::function::get_function_tag<Functor>::type tag;
        return get_functor_pointer<Functor>(tag(), 0);
      }
    }

  template<typename Functor>

#if defined(BOOST_MSVC) && BOOST_WORKAROUND(BOOST_MSVC, < 1300)
    const Functor* target( Functor * = 0 ) const
#else
    const Functor* target() const
#endif
    {
      if (!manager) return 0;

      detail::function::any_pointer result =
        manager(detail::function::make_any_pointer(&typeid(Functor)),
                detail::function::check_functor_type_tag);
      if (!result.obj_ptr) return 0;
      else {
        typedef typename detail::function::get_function_tag<Functor>::type tag;

#if defined(BOOST_MSVC) && BOOST_WORKAROUND(BOOST_MSVC, < 1300)
        return get_functor_pointer(tag(), 0, (Functor*)0);
#else
        return get_functor_pointer<Functor>(tag(), 0);
#endif
      }
    }

  template<typename F>
    bool contains(const F& f) const
    {
#if defined(BOOST_MSVC) && BOOST_WORKAROUND(BOOST_MSVC, < 1300)
      if (const F* fp = this->target( (F*)0 )) {
#else
      if (const F* fp = this->template target<F>()) {
#endif
        return function_equal(*fp, f);
      } else {
        return false;
      }
    }

#if defined(__GNUC__) && __GNUC__ == 3 && __GNUC_MINOR__ <= 3
  // GCC 3.3 and newer cannot copy with the global operator==, due to
  // problems with instantiation of function return types before it
  // has been verified that the argument types match up.
  template<typename Functor>
    BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
    operator==(Functor g) const
    {
      if (const Functor* fp = target<Functor>())
        return function_equal(*fp, g);
      else return false;
    }

  template<typename Functor>
    BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
    operator!=(Functor g) const
    {
      if (const Functor* fp = target<Functor>())
        return !function_equal(*fp, g);
      else return true;
    }
#endif

public: // should be protected, but GCC 2.95.3 will fail to allow access
  detail::function::any_pointer (*manager)(
    detail::function::any_pointer,
    detail::function::functor_manager_operation_type);
  detail::function::any_pointer functor;

private:
  template<typename Functor>
#if defined(BOOST_MSVC) && BOOST_WORKAROUND(BOOST_MSVC, < 1300)
    Functor* get_functor_pointer(detail::function::function_ptr_tag, int, Functor * = 0)
#else
    Functor* get_functor_pointer(detail::function::function_ptr_tag, int)
#endif
    { return reinterpret_cast<Functor*>(&functor.func_ptr); }

  template<typename Functor, typename Tag>
#if defined(BOOST_MSVC) && BOOST_WORKAROUND(BOOST_MSVC, < 1300)
    Functor* get_functor_pointer(Tag, long, Functor * = 0)
#else
    Functor* get_functor_pointer(Tag, long)
#endif
    { return static_cast<Functor*>(functor.obj_ptr); }

  template<typename Functor>
    const Functor*
#if defined(BOOST_MSVC) && BOOST_WORKAROUND(BOOST_MSVC, < 1300)
    get_functor_pointer(detail::function::function_ptr_tag, int, Functor * = 0) const
#else
    get_functor_pointer(detail::function::function_ptr_tag, int) const
#endif
    { return reinterpret_cast<const Functor*>(&functor.func_ptr); }

  template<typename Functor, typename Tag>
#if defined(BOOST_MSVC) && BOOST_WORKAROUND(BOOST_MSVC, < 1300)
    const Functor* get_functor_pointer(Tag, long, Functor * = 0) const
#else
    const Functor* get_functor_pointer(Tag, long) const
#endif
    { return static_cast<const Functor*>(functor.const_obj_ptr); }
};

/**
 * The bad_function_call exception class is thrown when a boost::function
 * object is invoked
 */
class bad_function_call : public std::runtime_error
{
public:
  bad_function_call() : std::runtime_error("call to empty boost::function") {}
};

#ifndef BOOST_NO_SFINAE
inline bool operator==(const function_base& f,
                       detail::function::useless_clear_type*)
{
  return f.empty();
}

inline bool operator!=(const function_base& f,
                       detail::function::useless_clear_type*)
{
  return !f.empty();
}

inline bool operator==(detail::function::useless_clear_type*,
                       const function_base& f)
{
  return f.empty();
}

inline bool operator!=(detail::function::useless_clear_type*,
                       const function_base& f)
{
  return !f.empty();
}
#endif

#ifdef BOOST_NO_SFINAE
// Comparisons between boost::function objects and arbitrary function objects
template<typename Functor>
  inline bool operator==(const function_base& f, Functor g)
  {
    typedef mpl::bool_<(is_integral<Functor>::value)> integral;
    return detail::function::compare_equal(f, g, 0, integral());
  }

template<typename Functor>
  inline bool operator==(Functor g, const function_base& f)
  {
    typedef mpl::bool_<(is_integral<Functor>::value)> integral;
    return detail::function::compare_equal(f, g, 0, integral());
  }

template<typename Functor>
  inline bool operator!=(const function_base& f, Functor g)
  {
    typedef mpl::bool_<(is_integral<Functor>::value)> integral;
    return detail::function::compare_not_equal(f, g, 0, integral());
  }

template<typename Functor>
  inline bool operator!=(Functor g, const function_base& f)
  {
    typedef mpl::bool_<(is_integral<Functor>::value)> integral;
    return detail::function::compare_not_equal(f, g, 0, integral());
  }
#else

#  if !(defined(__GNUC__) && __GNUC__ == 3 && __GNUC_MINOR__ <= 3)
// Comparisons between boost::function objects and arbitrary function
// objects. GCC 3.3 and before has an obnoxious bug that prevents this
// from working.
template<typename Functor>
  BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
  operator==(const function_base& f, Functor g)
  {
    if (const Functor* fp = f.template target<Functor>())
      return function_equal(*fp, g);
    else return false;
  }

template<typename Functor>
  BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
  operator==(Functor g, const function_base& f)
  {
    if (const Functor* fp = f.template target<Functor>())
      return function_equal(g, *fp);
    else return false;
  }

template<typename Functor>
  BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
  operator!=(const function_base& f, Functor g)
  {
    if (const Functor* fp = f.template target<Functor>())
      return !function_equal(*fp, g);
    else return true;
  }

template<typename Functor>
  BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
  operator!=(Functor g, const function_base& f)
  {
    if (const Functor* fp = f.template target<Functor>())
      return !function_equal(g, *fp);
    else return true;
  }
#  endif

template<typename Functor>
  BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
  operator==(const function_base& f, reference_wrapper<Functor> g)
  {
    if (const Functor* fp = f.template target<Functor>())
      return fp == g.get_pointer();
    else return false;
  }

template<typename Functor>
  BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
  operator==(reference_wrapper<Functor> g, const function_base& f)
  {
    if (const Functor* fp = f.template target<Functor>())
      return g.get_pointer() == fp;
    else return false;
  }

template<typename Functor>
  BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
  operator!=(const function_base& f, reference_wrapper<Functor> g)
  {
    if (const Functor* fp = f.template target<Functor>())
      return fp != g.get_pointer();
    else return true;
  }

template<typename Functor>
  BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL(Functor, bool)
  operator!=(reference_wrapper<Functor> g, const function_base& f)
  {
    if (const Functor* fp = f.template target<Functor>())
      return g.get_pointer() != fp;
    else return true;
  }

#endif // Compiler supporting SFINAE

namespace detail {
  namespace function {
    inline bool has_empty_target(const function_base* f)
    {
      return f->empty();
    }

#if BOOST_WORKAROUND(BOOST_MSVC, <= 1310)
    inline bool has_empty_target(const void*)
    {
      return false;
    }
#else
    inline bool has_empty_target(...)
    {
      return false;
    }
#endif
  } // end namespace function
} // end namespace detail
} // end namespace boost

#undef BOOST_FUNCTION_ENABLE_IF_NOT_INTEGRAL
#undef BOOST_FUNCTION_COMPARE_TYPE_ID

#endif // BOOST_FUNCTION_BASE_HEADER

max_stars_repo_name: schinmayee/nimbus

max_issues_repo_name: schinmayee/nimbus

max_forks_repo_name: schinmayee/nimbus


text: #include <boost/metaparse/foldl_reject_incomplete_start_with_parser.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include <iostream>
#include <chrono>

#include <boost/program_options.hpp>
#include <crazyflie_cpp/Crazyflie.h>
#include <crazyflie_cpp/crtp.h>

struct crtpMyPacket
{
  crtpMyPacket(
    uint64_t id,
    uint64_t time_since_epoch)
    : header(15, 0) // Link, echo
    , id(id)
    , time_since_epoch(time_since_epoch)
  {
  }
  const crtp header;
  const uint64_t id;
  uint64_t time_since_epoch;
} __attribute__((packed));

uint64_t getTimestamp()
{
  auto now = std::chrono::high_resolution_clock::now();
  auto now_us = std::chrono::time_point_cast<std::chrono::milliseconds>(now);

  auto value = now_us.time_since_epoch();
  uint64_t time_since_epoch = value.count();
  return time_since_epoch;
}

// stats
uint32_t numPacketsReceived = 0;
uint64_t sumRoundtripTime = 0;

uint32_t linkQualitySent = 0;
uint32_t linkQualityAcked = 0;

void onGenericPacket(const ITransport::Ack& ack)
{
  uint64_t time_since_epoch = getTimestamp();

  const crtpMyPacket* packet = reinterpret_cast<const crtpMyPacket*>(ack.data);
  uint64_t roundtripTime = time_since_epoch - packet->time_since_epoch;
  // std::cout << "packet!" << (int)ack.size << " " << packet->id << " " << roundtripTime << " ms" << std::endl;

  sumRoundtripTime += roundtripTime;
  ++numPacketsReceived;
}

void onEmptyAck(const crtpPlatformRSSIAck* cb)
{
  std::cout << "RSSI: " << (int)cb->rssi << std::endl;
}

void onLinkQuality(float quality)
{
  linkQualitySent += 100;
  linkQualityAcked += quality * 100;
  // std::cout << "Link quality: " << quality << std::endl;
}

int main(int argc, char **argv)
{

  std::string uri;
  std::string defaultUri("radio://0/80/2M/E7E7E7E7E7");
  uint32_t numPackets;

  namespace po = boost::program_options;

  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("uri", po::value<std::string>(&uri)->default_value(defaultUri), "unique ressource identifier")
    ("numPackets", po::value<uint32_t>(&numPackets)->default_value(1000), "unique ressource identifier")
  ;

  try
  {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
  }
  catch(po::error& e)
  {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  try
  {
    Crazyflie cf(uri);

    cf.setEmptyAckCallback(onEmptyAck);
    cf.setLinkQualityCallback(onLinkQuality);

    // send pings to clear queue
    for (size_t i = 0; i < 1000; ++i) {
      cf.sendPing();
    }

    cf.setGenericPacketCallback(onGenericPacket);

    uint64_t start = getTimestamp();
    for (uint64_t id = 0; id < numPackets; ++id) {
      uint64_t time_since_epoch = getTimestamp();

      crtpMyPacket p(id, time_since_epoch);
      // std::cout << "send " << id << " " << time_since_epoch << std::endl;

      crtpPacket_t packet;
      packet.size = sizeof(crtpMyPacket)-1;
      packet.header = *reinterpret_cast<const uint8_t*>(&p.header);
      memcpy(packet.data, &p.id, sizeof(crtpMyPacket)-1);

      cf.queueOutgoingPacket(packet);
      cf.transmitPackets();
    }
    uint64_t timeToSent = getTimestamp() - start;

    // send pings to clear queue
    for (size_t i = 0; i < 1000; ++i) {
      cf.sendPing();
    }

    std::cout << "numPacketsReceived: " << numPacketsReceived / (double)numPackets * 100.0f << " %" << std::endl;
    std::cout << "Avg. roundtrip time: " << sumRoundtripTime / (double) numPacketsReceived << " ms" << std::endl;
    std::cout << numPackets / (timeToSent / 1000.0f) << " packets/s sent" << std::endl;
    std::cout << "link quality: " << linkQualityAcked / (float)linkQualitySent << std::endl;

    return 0;
  }
  catch(std::exception& e)
  {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}

ext: cpp


text:
#include "moebius_registration.h"

/*
Copyright (c) 2018, Michael Kazhdan, Alex Baden, and Keenan Crane
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution.

Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE  GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.
*/

#include <stdio.h>
#include <stdlib.h>
#include <omp.h>
#include <map>
#include <unordered_map>
#include <Eigen/Dense>
#include <Misha/CmdLineParser.h>
#include <Misha/Ply.h>
#include <Misha/SparseMatrix.h>
#include <Misha/Solver.h>
#include <Misha/Timer.h>
#include <Misha/MemoryUsage.h>
#include <Misha/SphericalGeometry.h>

enum
{
	MESH_TYPE_TRIANGLE,
	MESH_TYPE_POLYGON,
	MESH_TYPE_TRIANGULATED_POLYGON,
	MESH_TYPE_COUNT
};
enum
{
	HOLE_FILL_TYPE_NONE,
	HOLE_FILL_TYPE_CENTER_TRIANGULATION,
	HOLE_FILL_TYPE_MINIMAL_AREA_TRIANGULATION,
	HOLE_FILL_TYPE_POLYGON,
	HOLE_FILL_TYPE_COUNT
};
const char *MeshTypeNames[] = { "triangle mesh" , "polygon mesh" , "implicitly triangulated polygon mesh" };
const char* HoleFillTypeNames[] = { "none" , "center triangulation" , "minimal area triangulation" , "polygon" };
cmdLineParameter< char* > In("in"), Out("out"), OutGrid("outG"), OutTessellation("outT");
cmdLineParameter< int > Iterations("iters", 100), Threads("threads", omp_get_num_procs()), Resolution("res", 256), SHDegree("degree", 1), AdvectionSteps("aSteps", 10), MeshType("mesh", MESH_TYPE_TRIANGLE + 1), HoleFillType("fill", HOLE_FILL_TYPE_NONE), CenterToInversion("c2i", 2);
cmdLineParameter< float > StepSize("stepSize", 0.1f), CutOff("cutOff", 1e-10f), Smooth("smooth", 5e-4f), AdvectionStepSize("aStepSize", 0.05f), GSSTolerance("gssTolerance", (float)1e-6), PoincareMaxNorm("poincareMaxNorm", 2.f);
cmdLineReadable Verbose("verbose"), FullVerbose("fullVerbose"), ASCII("ascii"), Randomize("random"), NoCenter("noCenter"), Collapse("collapse"), NoOrient("noOrient"), Lump("lump"), Spherical("spherical");

void Usage(const char* ex)
{
	printf("Usage %s:\n", ex);
	printf("\t --%s <input mesh>\n", In.name);
	printf("\t[--%s <output mesh>]\n", Out.name);
	printf("\t[--%s <output spherical grid>]\n", OutGrid.name);
	printf("\t[--%s <output spherical tessellation>]\n", OutTessellation.name);
	printf("\t[--%s <mesh type>=%d]\n", MeshType.name, MeshType.value);
	for (int i = 0; i < MESH_TYPE_COUNT; i++) printf("\t\t%d] %s\n", i + 1, MeshTypeNames[i]);
	printf("\t[--%s <hole fill type>=%d]\n", HoleFillType.name, HoleFillType.value);
	for (int i = 0; i < HOLE_FILL_TYPE_COUNT; i++) printf("\t\t%d] %s\n", i, HoleFillTypeNames[i]);
	printf("\t[--%s <CMCF iterations>=%d]\n", Iterations.name, Iterations.value);
	printf("\t[--%s <CMCF step size>=%f]\n", StepSize.name, StepSize.value);
	printf("\t[--%s <Moebius centering cut-off>=%g]\n", CutOff.name, CutOff.value);
	printf("\t[--%s <spherical harmonic degree>=%d]\n", SHDegree.name, SHDegree.value);
	printf("\t[--%s <advection steps>=%d]\n", AdvectionSteps.name, AdvectionSteps.value);
	printf("\t[--%s <advection step size>=%f]\n", AdvectionStepSize.name, AdvectionStepSize.value);
	printf("\t[--%s <spherical resolution>=%d]\n", Resolution.name, Resolution.value);
	printf("\t[--%s <spherical diffusion time>=%g]\n", Smooth.name, Smooth.value);
	printf("\t[--%s <center to inversion type>=%d]\n", CenterToInversion.name, CenterToInversion.value);
	printf("\t\t0] Trivial\n");
	printf("\t\t1] Golden section search\n");
	printf("\t\t2] Poincare\n");
	printf("\t[--%s <golden section search tolerance>=%e]\n", GSSTolerance.name, GSSTolerance.value);
	printf("\t[--%s <Poincare max norm>=%e]\n", PoincareMaxNorm.name, PoincareMaxNorm.value);
	printf("\t[--%s]\n", Randomize.name);
	printf("\t[--%s]\n", NoCenter.name);
	printf("\t[--%s]\n", ASCII.name);
	printf("\t[--%s]\n", Collapse.name);
	printf("\t[--%s]\n", NoOrient.name);
	printf("\t[--%s]\n", Spherical.name);
	printf("\t[--%s]\n", Lump.name);
	printf("\t[--%s]\n", Verbose.name);
	printf("\t[--%s]\n", FullVerbose.name);
}
cmdLineReadable* params[] = { &In , &Out , &OutGrid , &OutTessellation , &Iterations , &StepSize , &Threads , &Verbose , &FullVerbose , &Randomize , &ASCII , &NoCenter , &Resolution , &Smooth , &Collapse , &NoOrient , &SHDegree , &AdvectionSteps , &AdvectionStepSize , &CenterToInversion , &GSSTolerance , &PoincareMaxNorm , &Lump , &MeshType , &HoleFillType , &Spherical , NULL };

template< class Real >
Point3D< Real > NormalColor(Point3D< Real > n)
{
	Point3D< Real > c = (-n + Point3D< Real >(1, 1, 1)) * Real(128);
	for (int d = 0; d < 3; d++)
	{
		if (c[d] > Real(255)) c[d] = Real(255);
		if (c[d] < Real(0)) c[d] = Real(0);
	}
	return c;
}

template< typename Real >
SquareMatrix< Real, 3 > MassMatrix(const Point3D< Real > vertices[])
{
	SquareMatrix< Real, 3 > mass;
	Real area = (Real)Length(Point3D< Real >::CrossProduct(vertices[1] - vertices[0], vertices[2] - vertices[0])) / 2;
	mass(1, 0) = mass(0, 1) = mass(2, 0) = mass(0, 1) = mass(0, 2) = mass(1, 2) = mass(2, 1) = area / 12;
	mass(0, 0) = mass(1, 1) = mass(2, 2) = area / 6;
	return mass;
}
template< typename Real >
SquareMatrix< Real, 3 > StiffnessMatrix(const Point3D< Real > vertices[])
{
	SquareMatrix< Real, 3 > stiffness;
	Real area = (Real)Length(Point3D< Real >::CrossProduct(vertices[1] - vertices[0], vertices[2] - vertices[0])) / 2;
	for (int i = 0; i < 3; i++)
	{
		int i0 = (i + 1) % 3, i1 = (i + 2) % 3;
		Real dot = Point3D< Real >::Dot(vertices[i0] - vertices[i], vertices[i1] - vertices[i]);
		stiffness(i0, i1) = stiffness(i1, i0) = -dot / area;
	}
	stiffness(0, 0) = -(stiffness(0, 1) + stiffness(0, 2));
	stiffness(1, 1) = -(stiffness(1, 2) + stiffness(1, 0));
	stiffness(2, 2) = -(stiffness(2, 0) + stiffness(2, 1));
	return stiffness;
}

template< typename Real >
struct Mesh
{
	virtual int faces(void) const = 0;
	virtual Point3D< Real > normal(int f) const = 0;
	virtual Point3D< Real > center(int f) const = 0;
	virtual Real area(int f) const = 0;
	virtual SparseMatrix< Real, int > massMatrix(bool lump) const = 0;
	virtual SparseMatrix< Real, int > stiffnessMatrix(void) const = 0;
	virtual std::vector< Real > vertexAreas(void) const = 0;
	virtual std::vector< Point3D< Real > > vertexNormals(void) const = 0;
	virtual void push_back(int v1, int v2, int v3) = 0;
	virtual void pop_back(void) = 0;
	virtual void update(void) {}

	std::vector< Point3D< Real > > vertices;

	Real area(void) const
	{
		Real a = 0;
#pragma omp parallel for reduction( + : a )
		for (int f = 0; f < faces(); f++) a += area(f);
		return a;
	}
	Point3D< Real > center(void) const
	{
		Real area = 0, x = 0, y = 0, z = 0;
#pragma omp parallel for reduction( + : area , x , y , z )
		for (int f = 0; f < faces(); f++)
		{
			Real a = this->area(f);
			Point3D< Real > c = center(f)*a;
			area += a, x += c[0], y += c[1], z += c[2];
		}
		return Point3D< Real >(x, y, z) / area;
	}
	Real makeUnitArea(void)
	{
		Real scl = (Real)(1. / sqrt(area()));
#pragma omp parallel for
		for (int i = 0; i < vertices.size(); i++) vertices[i] *= scl;
		return (Real)1. / scl;
	}
	void pose(Point3D< Real >& center, Real& scale)
	{
		center = this->center();
		for (int i = 0; i < vertices.size(); i++) vertices[i] -= center;
		scale = 0;
		for (int i = 0; i < vertices.size(); i++) for (int j = 0; j < 3; j++) scale = std::max< Real >(scale, (Real)fabs(vertices[i][j]));
		scale *= 2;
		for (int i = 0; i < vertices.size(); i++) vertices[i] /= scale;
	}
	void pose(void)
	{
		Point3D< Real > center;
		Real scale;
		pose(center, scale);
	}

	Real radialDeviation(void) const
	{
		Real area = 0;
		std::vector< Real > vertexAreas = this->vertexAreas();
#pragma omp parallel for reduction( + : area )
		for (int i = 0; i < vertexAreas.size(); i++) area += vertexAreas[i];
		Point3D< Real > center;
		for (int i = 0; i < vertices.size(); i++) center += vertices[i] * vertexAreas[i];
		center /= area;

		// set v = \sum_i[ w_i * v_i ]
		// var = \sum_i[ w_i * ( v_i -v )^2 ]
		//     = \sum_i[ w_i * ( v_i^2 + v^2 - 2 * v_i * v ) ]
		//     = \sum_i[ w_i * v_i^2 ] + \sum_i[ w_i * v^2 ] - \sum_i[ 2 * w_i * v_i * v ) ]
		//     = \sum_i[ w_i * v_i^2 ] + v^2 - 2 * v^2
		//     = \sum_i[ w_i * v_i^2 ] - v^2
		Real average = 0, var = 0;
#pragma omp parallel for reduction( + : average , var )
		for (int i = 0; i < vertices.size(); i++)
		{
			Real r = (Real)sqrt(Point3D< Real >::SquareNorm(Point3D< Real >(vertices[i] - center)));
			average += r * vertexAreas[i];
			var += r * r * vertexAreas[i];
		}
		average /= area, var /= area;
		var -= average * average;
		var = (Real)sqrt(var);
		return var / average;
	}
};

template< typename Real >
struct TriangleMesh : public Mesh< Real >
{
	std::vector< TriangleIndex > triangles;
	using Mesh< Real >::center;
	using Mesh< Real >::vertices;

	int faces(void) const { return (int)triangles.size(); }
	void push_back(int v1, int v2, int v3) { triangles.push_back(TriangleIndex(v1, v2, v3)); }
	void pop_back(void) { triangles.pop_back(); }

	Point3D< Real > normal(int t) const { return Point3D< Real >::CrossProduct(vertices[triangles[t][1]] - vertices[triangles[t][0]], vertices[triangles[t][2]] - vertices[triangles[t][0]]); }
	Point3D< Real > center(int t) const { return (vertices[triangles[t][0]] + vertices[triangles[t][1]] + vertices[triangles[t][2]]) / 3; }
	Real area(int t) const { return (Real)Length(normal(t)) / 2.; }

#if 0
	template< typename TriangleMatrixFunctor >
	SparseMatrix< Real, int > matrix(TriangleMatrixFunctor F) const
#else
	SparseMatrix< Real, int > matrix(SquareMatrix< Real, 3 >(*F)(const Point3D< Real >[])) const
#endif
	{
		struct Entry
		{
			Entry(void) : row(-1), col(-1), value(0) {}
			Entry(int r, int c, Real v) : row(r), col(c), value(v) {}
			int row, col;
			Real value;
		};
		SparseMatrix< Real, int > M;
		M.resize((int)vertices.size());
		std::vector< std::vector< Entry > > entries(omp_get_max_threads());
#pragma omp parallel for
		for (int t = 0; t < triangles.size(); t++)
		{
			int thread = omp_get_thread_num();
			Point3D< Real > v[] = { vertices[triangles[t][0]] , vertices[triangles[t][1]] , vertices[triangles[t][2]] };
			SquareMatrix< Real, 3 > m = F(v);
			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) entries[thread].push_back(Entry(triangles[t][i], triangles[t][j], m(i, j)));
		}
		for (int i = 0; i < entries.size(); i++) for (int j = 0; j < entries[i].size(); j++)	M.rowSizes[entries[i][j].row]++;
#pragma omp parallel for
		for (int i = 0; i < M.rows; i++)
		{
			int rowSize = M.rowSizes[i];
			M.rowSizes[i] = 0;
			M.SetRowSize(i, rowSize);
			M.rowSizes[i] = 0;
		}
		for (int i = 0; i < entries.size(); i++) for (int j = 0; j < entries[i].size(); j++) M[entries[i][j].row][M.rowSizes[entries[i][j].row]++] = MatrixEntry< Real, int >(entries[i][j].col, entries[i][j].value);
#pragma omp parallel for
		for (int i = 0; i < M.rows; i++)
		{
			std::unordered_map< int, Real > row;
			for (int j = 0; j < M.rowSizes[i]; j++) row[M[i][j].N] += M[i][j].Value;
			M.SetRowSize(i, (int)row.size());
			int j = 0;
			for (std::unordered_map< int, Real >::const_iterator iter = row.begin(); iter != row.end(); iter++) M[i][j++] = MatrixEntry< Real, int >(iter->first, iter->second);
		}
		return M;
	}
	SparseMatrix< Real, int > massMatrix(bool lump) const
	{
		SparseMatrix< Real, int > mass = matrix(MassMatrix);
		if (lump)
		{
			for (int i = 0; i < mass.rows; i++)
			{
				Real sum = 0;
				for (int j = 0; j < mass.rowSizes[i]; j++) { sum += mass[i][j].Value; mass[i][j].Value = 0; }
				for (int j = 0; j < mass.rowSizes[i]; j++) if (mass[i][j].N == i) mass[i][j].Value = sum;
			}
		}
		return mass;
	}
	SparseMatrix< Real, int > stiffnessMatrix(void) const { return matrix(StiffnessMatrix); }

	std::vector< Real > vertexAreas(void) const
	{
		std::vector< Real > areas(vertices.size(), 0);
		for (int t = 0; t < triangles.size(); t++)
		{
			Real a = area(t) / 3;
			for (int j = 0; j < 3; j++) areas[triangles[t][j]] += a;
		}
		return areas;
	}
	std::vector< Point3D< Real > > vertexNormals(void) const
	{
		std::vector< Point3D< Real > > normals(vertices.size());
		for (int t = 0; t < triangles.size(); t++)
		{
			Point3D< Real > n = normal(t);
			Real a = area(t);
			n *= a / (Real)Length(n);
			for (int j = 0; j < 3; j++) normals[triangles[t][j]] += n;
		}
		return normals;
	}
};

template< typename Real >
struct PolygonMesh : public Mesh< Real >
{
protected:
	std::unordered_map< unsigned long long, int > _edges;
public:
	std::vector< std::vector< int > > polygons;
	using Mesh< Real >::center;
	using Mesh< Real >::vertices;

	int faces(void) const { return (int)polygons.size(); }
	void push_back(int v1, int v2, int v3) { polygons.push_back(std::vector< int >({ v1 , v2 , v3 })); }
	void pop_back(void) { polygons.pop_back(); }

	Point3D< Real > normal(int p) const
	{
		Point3D< Real > n;
		for (int i = 0; i < polygons[p].size(); i++)
		{
			Point3D< Real > b = vertices[polygons[p][(i + 1) % polygons[p].size()]] + vertices[polygons[p][i]];
			Point3D< Real > e = vertices[polygons[p][(i + 1) % polygons[p].size()]] - vertices[polygons[p][i]];
			n += Point3D< Real >::CrossProduct(b, e) / 2;
		}
		return n;
	}
	Point3D< Real > center(int p) const
	{
		Point3D< Real > c;
		for (int i = 0; i < polygons[p].size(); i++) c += vertices[polygons[p][i]];
		return c / (int)polygons[p].size();
	}
	Real area(int p) const { return (Real)Length(normal(p)) / 2.; }

	SparseMatrix< Real, int > massMatrix(bool lump) const
	{
		SparseMatrix< Real, int > mass;
		mass.resize((int)vertices.size());
		Eigen::DiagonalMatrix< double, Eigen::Dynamic > M0 = mass0();
#pragma omp parallel for
		for (int i = 0; i < mass.rows; i++)
		{
			mass.SetRowSize(i, 1);
			mass[i][0] = MatrixEntry< Real, int >(i, M0.diagonal()(i));
		}
		return mass;
	}

	SparseMatrix< Real, int > stiffnessMatrix(void) const
	{
		SparseMatrix< Real, int > stiffness;
		stiffness.resize((int)vertices.size());
		Eigen::SparseMatrix< double > D = coBoundary();
		Eigen::SparseMatrix< double > M = mass1();
		Eigen::SparseMatrix< double > S = D.transpose() * M * D;

		for (int i = 0; i < S.outerSize(); i++) for (Eigen::SparseMatrix< double >::InnerIterator it(S, i); it; ++it) stiffness.rowSizes[it.row()]++;
		for (int i = 0; i < stiffness.rows; i++)
		{
			int cols = stiffness.rowSizes[i];
			stiffness.rowSizes[i] = 0;
			stiffness.SetRowSize(i, cols);
			stiffness.rowSizes[i] = 1;
		}
		for (int i = 0; i < S.outerSize(); i++) for (Eigen::SparseMatrix< double >::InnerIterator it(S, i); it; ++it)
		{
			if (it.row() == it.col()) stiffness[it.row()][0] = MatrixEntry< Real, int >(it.col(), it.value());
			else
			{
				int idx = stiffness.rowSizes[it.row()]++;
				stiffness[it.row()][idx] = MatrixEntry< Real, int >(it.col(), it.value());
			}
		}
		return stiffness;
	}
	std::vector< Real > vertexAreas(void) const
	{
		std::vector< Real > areas(vertices.size(), 0);
		for (int p = 0; p < polygons.size(); p++)
		{
			Real a = area(p) / (int)polygons[p].size();
			for (int j = 0; j < polygons[p].size(); j++) areas[polygons[p][j]] += a;
		}
		return areas;
	}
	std::vector< Point3D< Real > > vertexNormals(void) const
	{
		std::vector< Point3D< Real > > normals(vertices.size());
		for (int p = 0; p < polygons.size(); p++)
		{
			Point3D< Real > n = normal(p);
			Real a = area(p);
			n *= a / (Real)Length(n);
			for (int j = 0; j < polygons.size(); j++) normals[polygons[p][j]] += n;
		}
		return normals;
	}

	void update(void)
	{
		for (int i = 0; i < polygons.size(); i++) for (int j = 0; j < polygons[i].size(); j++) for (int k = 0; k < polygons[i].size(); k++) if (j != k) _edges[EdgeKey(polygons[i][j], polygons[i][k])] = 0;
		int idx = 0;
		for (auto e = _edges.begin(); e != _edges.end(); e++) e->second = idx++;
	}

	Eigen::DiagonalMatrix< double, Eigen::Dynamic > mass0(void) const
	{
		Eigen::DiagonalMatrix< double, Eigen::Dynamic > M0(vertices.size());
		std::vector< double > areas(polygons.size());
#pragma omp parallel for
		for (int i = 0; i < polygons.size(); i++) areas[i] = area(i) / polygons[i].size();
#pragma omp parallel for
		for (int i = 0; i < vertices.size(); i++) M0.diagonal()(i) = 0;
		for (int i = 0; i < polygons.size(); i++) for (int j = 0; j < polygons[i].size(); j++) M0.diagonal()(polygons[i][j]) += areas[i];
		return M0;
	}
	Eigen::SparseMatrix< double > mass1(double lambda = 1) const
	{
		std::vector< Eigen::Triplet< double > > triplets;
		for (int i = 0; i < polygons.size(); i++)
		{
			Eigen::MatrixXd S = faceMass1(i, lambda);
			const std::vector< int > &p = polygons[i];
			int pSize = (int)p.size();
			for (int j = 0; j < pSize; j++) for (int k = 0; k < pSize; k++)
			{
				auto it1 = _edges.find(EdgeKey(p[j], p[(j + 1) % pSize]));
				auto it2 = _edges.find(EdgeKey(p[k], p[(k + 1) % pSize]));
				double sign1 = p[j] < p[(j + 1) % pSize] ? 1 : -1;
				double sign2 = p[k] < p[(k + 1) % pSize] ? 1 : -1;
				triplets.push_back(Eigen::Triplet< double >(it1->second, it2->second, S(j, k)*sign1*sign2));
			}
		}
		Eigen::SparseMatrix< double > M1((int)_edges.size(), (int)_edges.size());
		M1.setFromTriplets(triplets.begin(), triplets.end());
		return M1;
	}
	Eigen::SparseMatrix< double > coBoundary(void) const
	{
		std::vector< Eigen::Triplet< double > > triplets;
		triplets.reserve(_edges.size() * 2);

		int idx = 0;
		for (auto e = _edges.begin(); e != _edges.end(); e++)
		{
			int i, j;
			FactorEdgeKey(e->first, i, j);
			triplets.push_back(Eigen::Triplet< double >(e->second, i, -1));
			triplets.push_back(Eigen::Triplet< double >(e->second, j, 1));
		}
		Eigen::SparseMatrix< double > D((int)_edges.size(), (int)vertices.size());
		D.setFromTriplets(triplets.begin(), triplets.end());
		return D;
	}

	Eigen::MatrixXd faceMass1(int p, double lambda) const
	{
		std::vector< Point3D< double > > _vertices(polygons[p].size());
		for (int i = 0; i < polygons[p].size(); i++) _vertices[i] = Point3D< double >(vertices[polygons[p][i]]);

		Eigen::MatrixXd B(polygons[p].size(), 3), E(polygons[p].size(), 3), _E(polygons[p].size(), 3);
		for (int i = 0; i < polygons[p].size(); i++)
		{
			Point3D< double > v[] = { _vertices[i] , _vertices[(i + 1) % polygons[p].size()] };
			for (int j = 0; j < 3; j++)
			{
				E(i, j) = v[1][j] - v[0][j];
				B(i, j) = (v[1][j] + v[0][j]) / 2.;
			}
		}
		Eigen::MatrixXd A = E.transpose() * B;
		Eigen::MatrixXd _S = B * B.transpose() * sqrt(2) / A.norm();
		Point3D< double > n(-A(1, 2), A(0, 2), -A(0, 1));
		n /= Length(n);
		for (int i = 0; i < polygons[p].size(); i++) _vertices[i] -= n * Point3D< double >::Dot(n, _vertices[i]);
		for (int i = 0; i < polygons[p].size(); i++)
		{
			Point3D< double > v[] = { _vertices[i] , _vertices[(i + 1) % polygons[p].size()] };
			for (int j = 0; j < 3; j++) _E(i, j) = v[1][j] - v[0][j];
		}

		Eigen::MatrixXd C = _E.transpose().fullPivLu().kernel();
		return _S + C * C.transpose() * lambda;
	}
};

// Triangulate the polygons by adding the mid-point but constrain the value at the mid-point to be the average of the values along the vertices of the face
template< typename Real >
struct TriangulatedPolygonMesh : public Mesh< Real >
{
	std::vector< std::vector< int > > polygons;
	using Mesh< Real >::center;
	using Mesh< Real >::vertices;

	int faces(void) const { return (int)polygons.size(); }
	void push_back(int v1, int v2, int v3) { polygons.push_back(std::vector< int >({ v1 , v2 , v3 })); }
	void pop_back(void) { polygons.pop_back(); }

	Point3D< Real > normal(int p) const
	{
		Point3D< Real > n;
		for (int i = 0; i < polygons[p].size(); i++)
		{
			Point3D< Real > b = vertices[polygons[p][(i + 1) % polygons[p].size()]] + vertices[polygons[p][i]];
			Point3D< Real > e = vertices[polygons[p][(i + 1) % polygons[p].size()]] - vertices[polygons[p][i]];
			n += Point3D< Real >::CrossProduct(b, e) / 2;
		}
		return n;
	}
	Point3D< Real > center(int p) const
	{
		Point3D< Real > c;
		for (int i = 0; i < polygons[p].size(); i++) c += vertices[polygons[p][i]];
		return c / (int)polygons[p].size();
	}
	Real area(int p) const
	{
		if (polygons[p].size() == 3)
		{
			Point3D< Real > v[] = { vertices[polygons[p][0]] , vertices[polygons[p][1]] , vertices[polygons[p][2]] };
			Real a = (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
			return a;
		} else
		{
			Real a = 0;
			Point3D< Real > c = center(p);
			for (int i0 = 0; i0 < polygons[p].size(); i0++)
			{
				int i1 = (i0 + 1) % polygons[p].size();
				Point3D< Real > v[] = { vertices[polygons[p][i0]] , vertices[polygons[p][i1]] , c };
				a += (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
			}
			return a;
		}
	}

	std::vector< Real > vertexAreas(void) const
	{
		std::vector< Real > areas(vertices.size(), 0);
		for (int p = 0; p < polygons.size(); p++)
		{
			if (polygons[p].size() == 3)
			{
				Point3D< Real > v[] = { vertices[polygons[p][0]] , vertices[polygons[p][1]] , vertices[polygons[p][2]] };
				Real a = (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
				a /= 3;
				for (int i = 0; i < 3; i++) areas[polygons[p][i]] += a;
			} else
			{
				Point3D< Real > c = center(p);
				for (int i0 = 0; i0 < polygons[p].size(); i0++)
				{
					int i1 = (i0 + 1) % polygons[p].size();
					Point3D< Real > v[] = { vertices[polygons[p][i0]] , vertices[polygons[p][i1]] , c };
					Real a = (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
					a /= 2;
					areas[polygons[p][i0]] += a, areas[polygons[p][i1]] += a;
				}
			}
		}
		return areas;
	}
	std::vector< Point3D< Real > > vertexNormals(void) const
	{
		std::vector< Point3D< Real > > normals(vertices.size());
		for (int p = 0; p < polygons.size(); p++)
		{
			Point3D< Real > n = normal(p);
			Real a = area(p);
			n *= a / (Real)Length(n);
			for (int j = 0; j < polygons.size(); j++) normals[polygons[p][j]] += n;
		}
		return normals;
	}

	SparseMatrix< Real, int > massMatrix(bool lump) const
	{
		SparseMatrix< Real, int > mass = matrix(MassMatrix);
		if (lump)
		{
#pragma omp parallel for
			for (int i = 0; i < mass.rows; i++)
			{
				Real sum = 0;
				for (int j = 0; j < mass.rowSizes[i]; j++) { sum += mass[i][j].Value; mass[i][j].Value = 0; }
				for (int j = 0; j < mass.rowSizes[i]; j++) if (mass[i][j].N == i) { mass[i][j].Value = sum; break; }
			}
		}
		return mass;
	}
	SparseMatrix< Real, int > stiffnessMatrix(void) const { return matrix(StiffnessMatrix); }

#if 0
	template< typename TriangleMatrixFunctor >
	SparseMatrix< Real, int > matrix(TriangleMatrixFunctor F) const
#else
	SparseMatrix< Real, int > matrix(SquareMatrix< Real, 3 >(*F)(const Point3D< Real >[])) const
#endif
	{
		struct Entry
		{
			Entry(void) : row(-1), col(-1), value(0) {}
			Entry(int r, int c, Real v) : row(r), col(c), value(v) {}
			int row, col;
			Real value;
		};
		SparseMatrix< Real, int > M;
		M.resize((int)vertices.size());
		std::vector< std::vector< Entry > > entries(omp_get_max_threads());
#pragma omp parallel for
		for (int p = 0; p < polygons.size(); p++)
		{
			int thread = omp_get_thread_num();
			int pSize = (int)polygons[p].size();
			Eigen::MatrixXd m = faceMatrix(p, F);
			for (int i = 0; i < pSize; i++) for (int j = 0; j < pSize; j++) entries[thread].push_back(Entry(polygons[p][i], polygons[p][j], m(i, j)));
		}
		for (int i = 0; i < entries.size(); i++) for (int j = 0; j < entries[i].size(); j++)	M.rowSizes[entries[i][j].row]++;
#pragma omp parallel for
		for (int i = 0; i < M.rows; i++)
		{
			int rowSize = M.rowSizes[i];
			M.rowSizes[i] = 0;
			M.SetRowSize(i, rowSize);
			M.rowSizes[i] = 0;
		}
		for (int i = 0; i < entries.size(); i++) for (int j = 0; j < entries[i].size(); j++) M[entries[i][j].row][M.rowSizes[entries[i][j].row]++] = MatrixEntry< Real, int >(entries[i][j].col, entries[i][j].value);
#pragma omp parallel for
		for (int i = 0; i < M.rows; i++)
		{
			std::unordered_map< int, Real > row;
			for (int j = 0; j < M.rowSizes[i]; j++) row[M[i][j].N] += M[i][j].Value;
			M.SetRowSize(i, (int)row.size());
			int j = 0;
			for (std::unordered_map< int, Real >::const_iterator iter = row.begin(); iter != row.end(); iter++) M[i][j++] = MatrixEntry< Real, int >(iter->first, iter->second);
		}
		return M;
	}

	Eigen::MatrixXd centerConstraintMatrix(int p) const
	{
		int pSize = (int)polygons[p].size();
		Eigen::MatrixXd C(pSize + 1, pSize);
		for (int i = 0; i < C.rows(); i++) for (int j = 0; j < C.cols(); j++) C(i, j) = 0;
		for (int i = 0; i < pSize; i++) C(i, i) = 1, C(pSize, i) = 1. / pSize;
		return C;
	}

	template< typename TriangleMatrixFunctor >
	Eigen::MatrixXd faceMatrix(int p, TriangleMatrixFunctor F) const
	{
		int pSize = (int)polygons[p].size();
		if (pSize == 3)
		{
			Eigen::MatrixXd M(pSize, pSize);
			Point3D< Real > v[] = { vertices[polygons[p][0]] , vertices[polygons[p][1]] , vertices[polygons[p][2]] };
			SquareMatrix< Real, 3 > m = F(v);
			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) M(i, j) = m(i, j);
			return M;
		} else
		{
			Eigen::MatrixXd M(pSize + 1, pSize + 1), C = centerConstraintMatrix(p);
			for (int i = 0; i < M.rows(); i++) for (int j = 0; j < M.cols(); j++) M(i, j) = 0;

			Point3D< Real > c = center(p);
			int idx[] = { 0 , 0 , pSize };
			for (int i0 = 0; i0 < pSize; i0++)
			{
				int i1 = (i0 + 1) % pSize;
				idx[0] = i0, idx[1] = i1;
				Point3D< Real > v[] = { vertices[polygons[p][i0]] , vertices[polygons[p][i1]] , c };
				SquareMatrix< Real, 3 > m = F(v);
				for (int j = 0; j < 3; j++) for (int k = 0; k < 3; k++) M(idx[j], idx[k]) += m(j, k);
			}
			return C.transpose() * M * C;
		}
	}
};

template< typename Real >
struct QuasiConformalRatio
{
	static const Real ConformalCutOff;
	std::vector< TriangleIndex > triangles;
	std::vector< SquareMatrix< Real, 2 > > massInvs;
	std::vector< Real > areas;

	QuasiConformalRatio(const            TriangleMesh< Real > & tMesh, int tCount) : QuasiConformalRatio(&tMesh.vertices[0], &tMesh.triangles[0], tCount) { }
	QuasiConformalRatio(const             PolygonMesh< Real > & pMesh, int tCount) : QuasiConformalRatio(&pMesh.vertices[0], &pMesh.polygons[0], tCount) { }
	QuasiConformalRatio(const TriangulatedPolygonMesh< Real > &tpMesh, int tCount) : QuasiConformalRatio(&tpMesh.vertices[0], &tpMesh.polygons[0], tCount) { }

	QuasiConformalRatio(const Point3D< Real > *v, const TriangleIndex *t, int tCount)
	{
		triangles.resize(tCount);
		for (int i = 0; i < tCount; i++) triangles[i] = t[i];
		_init(v);
	}
	QuasiConformalRatio(const Point3D< Real > *v, const std::vector< int > *p, int pCount)
	{
		MinimalAreaTriangulation< Real > MAT;
		int count = 0;
		for (int i = 0; i < pCount; i++) count += (int)p[i].size() - 2;
		triangles.reserve(count);
		for (int i = 0; i < pCount; i++)
		{
			const std::vector< int >& poly = p[i];
			std::vector< Point3D< Real > > _vertices(poly.size());
			std::vector< TriangleIndex > _triangles;
			for (int j = 0; j < poly.size(); j++) _vertices[j] = v[poly[j]];
			MAT.GetTriangulation(_vertices, _triangles);
			for (int j = 0; j < _triangles.size(); j++) triangles.push_back(TriangleIndex(poly[_triangles[j][0]], poly[_triangles[j][1]], poly[_triangles[j][2]]));
		}
		_init(v);
	}

	Real operator()(const std::vector< Point3D< Real > > &vertices) const
	{
		Real error = 0, area = 0;
#pragma omp parallel for reduction( + : error , area )
		for (int i = 0; i < triangles.size(); i++)
		{
			Point3D< Real > v[] = { vertices[triangles[i][0]] , vertices[triangles[i][1]] , vertices[triangles[i][2]] };
			SquareMatrix< Real, 2 > m = massInvs[i] * _MassMatrix(v);
			Real det = m.determinant(), trace = m.trace();
			trace /= 2;
			if (det / 4 <= ConformalCutOff * ConformalCutOff) continue;
			// The eigenvectors of this matrix are the roots of
			// P(x) = [x-d(0,0)]*[x-d(1,1)] - d(1,0)*d(0,1)
			//      = x^2 - x * Tr(d) + Det(d)
			// x = Tr(d)/2 +/- sqrt( Tr^2(d)/4 - Det(d) )
			Real disc = trace * trace - det;
			if (disc <= Real(0)) disc = 0;
			else                disc = sqrt(disc);
			Real x1 = trace - disc;
			Real x2 = trace + disc;
			if (x1 <= 0) continue;
			Real tError = sqrt(x2 / x1) * areas[i];
			error += tError;
			area += areas[i];
		}
		return error / area;
	}

protected:
	static SquareMatrix< Real, 2 > _MassMatrix(const Point3D< Real > v[3])
	{
		Point3D< Real > t[] = { v[1] - v[0] , v[2] - v[0] };
		SquareMatrix< Real, 2 > mass;
		for (int j = 0; j < 2; j++) for (int k = 0; k < 2; k++) mass(j, k) = Point3D< Real >::Dot(t[j], t[k]);
		return mass;
	}
	void _init(const Point3D< Real > *vertices)
	{
		massInvs.resize(triangles.size());
		areas.resize(triangles.size(), 0);

		Real area = 0;
#pragma omp parallel for reduction( + : area )
		for (int i = 0; i < triangles.size(); i++)
		{
			Point3D< Real > v[] = { vertices[triangles[i][0]] , vertices[triangles[i][1]] , vertices[triangles[i][2]] };
			areas[i] = (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
			area += areas[i];
			if (areas[i] <= ConformalCutOff) continue;
			massInvs[i] = _MassMatrix(v).inverse();
		}
#pragma omp parallel for
		for (int i = 0; i < areas.size(); i++) areas[i] /= area;

	}
};
template< typename Real > const Real QuasiConformalRatio< Real >::ConformalCutOff = (Real)1e-15;

int FaceSize(const TriangleIndex& tri) { return 3; }
int FaceSize(const std::vector< int > &poly) { return (int)poly.size(); }

template< typename Face >
std::vector< std::vector< int > > BoundaryLoops(const std::vector< Face > &faces)
{
	auto EdgeKey = [](unsigned int v1, unsigned int v2) { return ((unsigned long long)v1) << 32 | ((unsigned long long)v2); };
	auto FactorEdgeKey = [](unsigned long long key, int& v1, int& v2) { v1 = (int)(key >> 32), v2 = (int)((key << 32) >> 32); };

	std::unordered_set< unsigned long long > edges;
	for (int f = 0; f < faces.size(); f++)
	{
		int fSize = FaceSize(faces[f]);
		for (int v = 0; v < fSize; v++)
		{
			int v1 = faces[f][v], v2 = faces[f][(v + 1) % fSize];
			edges.insert(EdgeKey(v1, v2));
		}
	}

	std::vector< std::pair< int, int > > boundaryEdges;
	for (auto i = edges.begin(); i != edges.end(); i++)
	{
		int v1, v2;
		FactorEdgeKey(*i, v1, v2);
		if (edges.find(EdgeKey(v2, v1)) == edges.end()) boundaryEdges.push_back(std::pair< int, int >(v2, v1));
	}

	std::vector< std::vector< int > > boundaryLoops;
	while (boundaryEdges.size())
	{
		std::vector< int > boundaryLoop;
		std::pair< int, int > boundaryEdge = boundaryEdges.back();
		boundaryEdges.pop_back();
		int start = boundaryEdge.first, v = boundaryEdge.second;
		boundaryLoop.push_back(v);
		while (v != start)
		{
			for (int i = 0; i < boundaryEdges.size(); i++) if (boundaryEdges[i].first == v)
			{
				v = boundaryEdges[i].second;
				boundaryLoop.push_back(v);
				boundaryEdges[i] = boundaryEdges.back();
				boundaryEdges.pop_back();
				break;
			}
		}
		boundaryLoops.push_back(boundaryLoop);
	}
	return boundaryLoops;
}
template< typename Real >
std::vector< std::vector< int > > BoundaryLoops(const TriangleMesh< Real > &mesh) { return BoundaryLoops(mesh.triangles); }
template< typename Real >
std::vector< std::vector< int > > BoundaryLoops(const PolygonMesh< Real > &mesh) { return BoundaryLoops(mesh.polygons); }
template< typename Real >
std::vector< std::vector< int > > BoundaryLoops(const TriangulatedPolygonMesh< Real > &mesh) { return BoundaryLoops(mesh.polygons); }

template< class Real, typename Mesh >
void CMCF(Mesh &mesh, int iters, bool lump, int fCount)
{
	double stepSize = StepSize.value;
	QuasiConformalRatio< Real > qcRatio(mesh, fCount);
	std::vector< Point3D< Real > > &vertices = mesh.vertices;
	std::vector< Point3D< Real > > b(vertices.size()), oldX(vertices.size()), n(vertices.size());
	Vector< Real > _b(b.size()), _x(vertices.size());
	Point3D< Real > center;
	Real scale = 1.;
	mesh.pose(center, scale);
	scale *= mesh.makeUnitArea();

	// Initialize the matrices.
	// M is the current matrix which is initialized with the sparse matrix topology.
	// D stores the mass matrix.
	// L stores the stiffness matrix.
	// The stiffness matrix is unchanged throughout the flow so we can initialize it once.
	SparseMatrix< Real, int > D, L, M;
	M = L = mesh.stiffnessMatrix();
	if (Randomize.set) for (int i = 0; i < vertices.size(); i++) vertices[i] = RandomSpherePoint< Real >();
	EigenSolverCholeskyLLt< Real >* solver = NULL;

	auto SetStats = [&](Real &deformationScale, Real &quasiConformalRatio, Real &radialDeviation)
	{
		Real differenceNorm = 0, oldNorm = 0;
#pragma omp parallel for
		for (int j = 0; j < D.rows; j++)
		{
			differenceNorm += Point3D< Real >::Dot(oldX[j] - vertices[j], oldX[j] - vertices[j]) * D[j][0].Value;
			oldNorm += Point3D< Real >::Dot(oldX[j], oldX[j]) * D[j][0].Value;
		}
		deformationScale = sqrt(differenceNorm / oldNorm);
		quasiConformalRatio = qcRatio(vertices);
		radialDeviation = mesh.radialDeviation();
	};
	Timer timer;
	for (int i = 0; i < iters; i++)
	{
		double t;
		double tt = Timer::Time();

		t = Timer::Time();
		D = mesh.massMatrix(lump);
		double mTime = Timer::Time() - t;
		// Set the new constraint vector: b = D * x
		D.MultiplyParallel(&vertices[0], &b[0], Threads.value, MULTIPLY_CLEAR);
		// Set the system matrix: M = D + t * L
#pragma omp parallel for num_threads( Threads.value )
		for (int j = 0; j < M.rows; j++)
		{
			for (int k = 0; k < M.rowSizes[j]; k++) M[j][k].Value = L[j][k].Value * stepSize;
			for (int k = 0; k < D.rowSizes[j]; k++) M[j][k].Value += D[j][k].Value;
		}

		t = Timer::Time();
		// If this is the first solve perform both the symbolic and numerical factorization.
		if (!solver) solver = new EigenSolverCholeskyLLt< Real >(M);
		// Otherwise, if the system matrix has changed, update the numerical factorization.
		else solver->update(M);
		double sTime1 = Timer::Time() - t;

		t = Timer::Time();
		// Solve for each of the x, y, z coefficients independently
		for (int d = 0; d < 3; d++)
		{
#pragma omp parallel for num_threads( Threads.value )
			for (int j = 0; j < b.size(); j++) _b[j] = b[j][d], _x[j] = oldX[j][d] = vertices[j][d];
			solver->solve(&_b[0], &_x[0]);
#pragma omp parallel for num_threads( Threads.value )
			for (int j = 0; j < vertices.size(); j++) vertices[j][d] = _x[j];
		}
		double sTime2 = Timer::Time() - t;

		double mem = MemoryInfo::UsageMB();
		// Re-scale/center the mesh
		mesh.pose(), mesh.makeUnitArea();

		if (FullVerbose.set)
		{
			tt = Timer::Time() - tt;
			Real deformationScale, quasiConformalRatio, radialDeviation;
			SetStats(deformationScale, quasiConformalRatio, radialDeviation);
			printf("CMCF[%d / %d] %4.2f = %4.2f + %4.2f + %4.2f(s): D-Norm=%6.5f / QC-Ratio=%6.5f / R-Deviation=%6.5f\r", i + 1, iters, tt, mTime, sTime1, sTime2, deformationScale, quasiConformalRatio, radialDeviation);
		}
	}
	if (FullVerbose.set && iters > 0) printf("\n");
	if (Verbose.set && iters > 0)
	{
		Real deformationScale, quasiConformalRatio, radialDeviation;
		SetStats(deformationScale, quasiConformalRatio, radialDeviation);
		printf("CMCF[%d] %4.2f(s): D-Norm=%6.5f / QC-Ratio=%6.5f / R-Deviation=%6.5f\n", iters, timer.elapsed(), deformationScale, quasiConformalRatio, radialDeviation);
	}
	mesh.pose();

	for (int i = 0; i < vertices.size(); i++) vertices[i] = vertices[i] * scale + center;
	if (solver) delete solver;
}

template< typename Real >
void Center(SphericalGeometry::Mesh< Real >& mesh)
{
	static const int MAX_ITERATIONS = 50;
	int iters;
	int verbose = FullVerbose.set ? 2 : (Verbose.set ? 1 : 0);
	switch (CenterToInversion.value)
	{
	case 0:  iters = mesh.normalize(MAX_ITERATIONS, CutOff.value, true, typename SphericalGeometry::Mesh< Real >::CenterToInversion(), verbose); break;
	case 1:  iters = mesh.normalize(MAX_ITERATIONS, CutOff.value, true, typename SphericalGeometry::Mesh< Real >::GSSCenterToInversion((Real)GSSTolerance.value), verbose); break;
	default: iters = mesh.normalize(MAX_ITERATIONS, CutOff.value, true, typename SphericalGeometry::Mesh< Real >::PoincareCenterToInversion((Real)PoincareMaxNorm.value), verbose); break;
	}
	if (iters == MAX_ITERATIONS && CenterToInversion.value != 1)
	{
		fprintf(stderr, "[WARNING] Failed to meet centering threshold after %d iterations, trying golden-section search\n", MAX_ITERATIONS);
		iters = mesh.normalize(MAX_ITERATIONS, CutOff.value, true, typename SphericalGeometry::Mesh< Real >::GSSCenterToInversion((Real)GSSTolerance.value), verbose);
	}
	if (iters == MAX_ITERATIONS) fprintf(stderr, "[WARNING] Failed to meet centering threshold after %d iterations\n", MAX_ITERATIONS);
}
template< unsigned int SHDegree, typename Real >
void SHCenter(SphericalGeometry::Mesh< Real >& mesh)
{
	static const int MAX_ITERATIONS = 50;
	int verbose = FullVerbose.set ? 2 : (Verbose.set ? 1 : 0);
	if (mesh.template normalizeSH< SHDegree >(MAX_ITERATIONS, AdvectionSteps.value, AdvectionStepSize.value, CutOff.value, true, verbose) == MAX_ITERATIONS) fprintf(stderr, "[WARNING] Failed to meet centering threshold after %d iterations\n", MAX_ITERATIONS);
}

template< class Real >
void Execute(SphericalGeometry::Mesh< Real > &mesh, const std::vector< Point3D< Real > > &vertices, const std::vector< Point3D< Real > > &colors)
{
	// Center the spherical mesh
	if (!NoCenter.set)
	{
		Timer t;
		Center(mesh);
		if (SHDegree.value >= 2) SHCenter< 2 >(mesh);
		if (SHDegree.value >= 3) SHCenter< 3 >(mesh);
		if (SHDegree.value >= 4) SHCenter< 4 >(mesh);
	}

	if (Out.set)
	{
		if (Spherical.set)
		{
			std::vector< PlyColorVertex< float > > outVertices(vertices.size());
			for (int i = 0; i < vertices.size(); i++)
			{
				outVertices[i].point = Point3D< float >(mesh.vertices[i]);
				outVertices[i].color = Point3D< float >(colors[i]);
			}
			PlyWritePolygons(Out.value, outVertices, mesh.polygons, PlyColorVertex< float >::WriteProperties, PlyColorVertex< float >::WriteComponents, ASCII.set ? PLY_ASCII : PLY_BINARY_NATIVE, NULL, 0);
		} else
		{
			std::vector< PlyParametrizedColorVertex< float, Real > > outVertices(vertices.size());
			for (int i = 0; i < vertices.size(); i++)
			{
				outVertices[i].point = Point3D< float >(vertices[i]);
				outVertices[i].color = Point3D< float >(colors[i]);
				outVertices[i].param = mesh.vertices[i];
			}
			PlyWritePolygons(Out.value, outVertices, mesh.polygons, PlyParametrizedColorVertex< float, Real >::WriteProperties, PlyParametrizedColorVertex< float, Real >::WriteComponents, ASCII.set ? PLY_ASCII : PLY_BINARY_NATIVE, NULL, 0);
		}
	}

	if (OutGrid.set || OutTessellation.set)
	{
		struct VertexAndColor
		{
			VertexAndColor(void) {}
			VertexAndColor(Point3D< Real > v, Point3D< Real > c) : vertex(v), color(c) {}
			Point3D< Real > vertex, color;
			VertexAndColor operator * (Real s) const { return VertexAndColor(vertex*s, color*s); }
			VertexAndColor operator / (Real s) const { return VertexAndColor(vertex / s, color / s); }
			VertexAndColor operator + (const VertexAndColor& vc) const { return VertexAndColor(vertex + vc.vertex, color + vc.color); }
		};
		Timer timer;

		std::vector< VertexAndColor > verticesAndColors(vertices.size());
		for (int i = 0; i < vertices.size(); i++) verticesAndColors[i].vertex = Point3D< Real >(vertices[i]), verticesAndColors[i].color = Point3D< Real >(colors[i]);
		SphericalGeometry::Tessellation< Real > tessellator(mesh, verticesAndColors, Resolution.value);
		if (Collapse.set) tessellator.collapseCells(verticesAndColors);

		if (OutTessellation.set)
		{
			std::vector< Point3D< Real > > _vertices(verticesAndColors.size()), _colors(verticesAndColors.size());
			for (int i = 0; i < verticesAndColors.size(); i++) _vertices[i] = verticesAndColors[i].vertex, _colors[i] = verticesAndColors[i].color;
			tessellator.write(OutTessellation.value, _vertices, _colors, !ASCII.set);
		}

		SphericalGrid< Real > sGrid;
		sGrid.resize(Resolution.value);
		tessellator.createSGrid(sGrid, Smooth.value);

		if (OutGrid.set)
		{
			char* ext = GetFileExtension(OutGrid.value);
			if (!strcasecmp(ext, "ply")) tessellator.writeGrid(OutGrid.value, Smooth.value, [](Real v) { return v < 0 ? (Real)-sqrt(-v) : (Real)sqrt(v); }, !ASCII.set);
			else if (!strcasecmp(ext, "sgrid"))
			{
				SphericalGrid< float > _sGrid;
				_sGrid.resize(sGrid.resolution());
				for (int i = 0; i < sGrid.resolution(); i++) for (int j = 0; j < sGrid.resolution(); j++) _sGrid(i, j) = (float)sGrid(i, j);
				_sGrid.write(OutGrid.value);
			}
			delete[] ext;
		}
		if (Verbose.set) printf("Tessellated: %.2f(s)\n", timer.elapsed());
	}
}
template< class Real >
void Execute(TriangleMesh< Real > &mesh, const std::vector< Point3D< Real > > &vertices, const std::vector< Point3D< Real > > &colors)
{
	SphericalGeometry::Mesh< Real > sMesh;
	{
		// Convert the triangles to polygons
		std::vector< std::vector< int > > polygons(mesh.triangles.size());
		for (int i = 0; i < mesh.triangles.size(); i++)
		{
			polygons[i].resize(3);
			for (int j = 0; j < 3; j++) polygons[i][j] = mesh.triangles[i][j];
		}
		sMesh = SphericalGeometry::Mesh< Real >(vertices, mesh.vertices, polygons);
		sMesh.makeUnitMass();
	}
	Execute(sMesh, vertices, colors);
}
template< class Real >
void Execute(PolygonMesh< Real > &mesh, const std::vector< Point3D< Real > > &vertices, const std::vector< Point3D< Real > > &colors)
{
	SphericalGeometry::Mesh< Real > sMesh;
	{
		sMesh = SphericalGeometry::Mesh< Real >(vertices, mesh.vertices, mesh.polygons);
		sMesh.makeUnitMass();
	}
	Execute(sMesh, vertices, colors);
}
template< class Real >
void Execute(TriangulatedPolygonMesh< Real > &mesh, const std::vector< Point3D< Real > > &vertices, const std::vector< Point3D< Real > > &colors)
{
	SphericalGeometry::Mesh< Real > sMesh;
	{
		sMesh = SphericalGeometry::Mesh< Real >(vertices, mesh.vertices, mesh.polygons);
		sMesh.makeUnitMass();
	}
	Execute(sMesh, vertices, colors);
}
template< typename Real >
void HoleFillCenterTriangulation(Mesh< Real > &mesh, const std::vector< int > &loop)
{
	Point3D< Real > c;
	for (int j = 0; j < loop.size(); j++) mesh.push_back(loop[j], loop[(j + 1) % loop.size()], (int)mesh.vertices.size()), c += mesh.vertices[loop[j]];
	c /= (int)loop.size();
	mesh.vertices.push_back(c);
}
template< typename Real >
void HoleFillMinimalAreaTriangulation(Mesh< Real > &mesh, const std::vector< int > &loop)
{
	MinimalAreaTriangulation< Real > MAT;
	std::vector< TriangleIndex > triangles(loop.size() - 2);
	std::vector< Point3D< Real > > vertices(loop.size());
	for (int j = 0; j < loop.size(); j++) vertices[j] = mesh.vertices[loop[j]];
	MAT.GetTriangulation(vertices, triangles);
	for (int j = 0; j < triangles.size(); j++) mesh.push_back(loop[triangles[j][0]], loop[triangles[j][1]], loop[triangles[j][2]]);;
}
template< typename Real > void HoleFillPolygon(TriangleMesh< Real >  &tMesh, const std::vector< int > &loop) { fprintf(stderr, "[WARNING] Polygon hole-filling not supported for triangle mesh\n"); }
template< typename Real > void HoleFillPolygon(PolygonMesh< Real >  &pMesh, const std::vector< int > &loop) { pMesh.polygons.push_back(loop); }
template< typename Real > void HoleFillPolygon(TriangulatedPolygonMesh< Real > &tpMesh, const std::vector< int > &loop) { tpMesh.polygons.push_back(loop); }

template< class Real, typename Mesh >
void Execute(Mesh &mesh, const std::vector< Point3D< Real > > &colors)
{
	int fCount = mesh.faces(), vCount = (int)mesh.vertices.size();
	if (HoleFillType.value != HOLE_FILL_TYPE_NONE)
	{
		Timer t;
		std::vector< std::vector< int > > boundaryLoops = BoundaryLoops(mesh);
		for (int i = 0; i < boundaryLoops.size(); i++)
		{
			if (FullVerbose.set) printf("\t%d] Hole size: %d\n", i + 1, (int)boundaryLoops[i].size());
			switch (HoleFillType.value)
			{
			case HOLE_FILL_TYPE_CENTER_TRIANGULATION:       HoleFillCenterTriangulation(mesh, boundaryLoops[i]); break;
			case HOLE_FILL_TYPE_MINIMAL_AREA_TRIANGULATION: HoleFillMinimalAreaTriangulation(mesh, boundaryLoops[i]); break;
			case HOLE_FILL_TYPE_POLYGON:                    HoleFillPolygon(mesh, boundaryLoops[i]); break;
			default: fprintf(stderr, "[WARNING] Unrecognizd hole-fill type:%d\n", HoleFillType.value);
			}
		}
		mesh.update();
		if (Verbose.set) printf("Filled in %d holes: %.2f(s)\n", (int)boundaryLoops.size(), t.elapsed());
	}
	std::vector< Point3D< Real > > vertices = mesh.vertices;

	CMCF< Real >(mesh, Iterations.value, Lump.set, fCount);

	if (mesh.faces() != fCount || mesh.vertices.size() != vCount)
	{
		while (mesh.faces() > fCount) mesh.pop_back();
		while (mesh.vertices.size() > vCount) mesh.vertices.pop_back();
		mesh.update();
	}

	// Normalize the parameterization
	{
		Point3D< Real > center = mesh.center();
#pragma omp parallel for
		for (int i = 0; i < mesh.vertices.size(); i++) mesh.vertices[i] -= center;
#pragma omp parallel for
		for (int i = 0; i < mesh.vertices.size(); i++) mesh.vertices[i] /= Length(mesh.vertices[i]);
	}

	// Orient the spherical parameterization so that it's outward facing
	if (!NoOrient.set)
	{
		Real pArea = 0, nArea = 0;
#pragma omp parallel for reduction ( + : pArea , nArea )
		for (int f = 0; f < mesh.faces(); f++)
		{
			Point3D< Real > c = mesh.center(f);
			Point3D< Real > n = mesh.normal(f);
			Real l = (Real)Length(n);
			if (Point3D< Real >::Dot(c, n) > 0) pArea += l;
			else                                  nArea += l;
		}
		if (nArea > pArea)
#pragma omp parallel for
			for (int i = 0; i < mesh.vertices.size(); i++) mesh.vertices[i] = -mesh.vertices[i];
	}
	Execute(mesh, vertices, colors);
}


text:
//
// Created by victor on 4/11/17.
//

#include "UserRequestHandler.h"
#include "UserManager.h"
#include "Validations.h"
#include <boost/algorithm/string.hpp>
#include <proxygen/httpserver/ResponseBuilder.h>
#include <folly/json.h>
#include <folly/futures/Promise.h>
#include <folly/futures/Future.h>
#include <folly/io/async/EventBaseManager.h>
#include <folly/executors/GlobalExecutor.h>
using proxygen::ResponseBuilder;

namespace restdbxx {
class user_already_exists : public std::domain_error {
 public:
  user_already_exists() : domain_error("user already exists") {}
  virtual ~user_already_exists() = default;
};
class invalid_user_object : public std::domain_error {
 public:
  invalid_user_object() : domain_error("invalid user object") {}
  virtual ~invalid_user_object() = default;
};
static const char *const USERS_PATH = "/__users";
void UserRequestHandler::onError(proxygen::ProxygenError err) noexcept {
  delete this;
}
void UserRequestHandler::requestComplete() noexcept {
  delete this;
}
void UserRequestHandler::onEOM() noexcept {
  if (_method == proxygen::HTTPMethod::GET) {
    // we will respond to GET requests
    if (_path == USERS_PATH) {
      //return list
      folly::Promise<folly::dynamic> promise;
      auto f = promise.getFuture();
      folly::EventBaseManager::get()->getEventBase()->runInLoop([p = std::move(promise), this]() mutable {
        std::vector<folly::dynamic> users;
        DbManager::get_instance()->get_all(USERS_PATH, users);
        folly::dynamic jsonResponse = folly::dynamic::array(users);
        p.setValue(jsonResponse);
      });
      f.then([this](folly::dynamic &jsonResponse) {
        sendJsonResponse(jsonResponse);

      });
    } else {
      folly::Promise<folly::Optional<folly::dynamic>> promise;
      auto f = promise.getFuture();
      folly::EventBaseManager::get()->getEventBase()->runInLoop([p = std::move(promise), this]() mutable {
        std::vector<std::string> parts;
        boost::algorithm::split(parts, _path, boost::is_any_of("/"), boost::algorithm::token_compress_on);
        auto i = parts.rbegin();
        if (i != parts.rend()) {
          std::string username = std::string(*i);
          auto db = DbManager::get_instance();
          auto result = db->get_user(username);
          p.setValue(result);
        } else {
          p.setValue(folly::none);
        }
      });
      f.then([this](folly::Optional<folly::dynamic> &value) {
        if (value) {
          sendJsonResponse(value.value());
        } else {
          sendNotFound();
        }
      });
    }
  } else if (_method == proxygen::HTTPMethod::POST && _path == USERS_PATH) {
    folly::Promise<folly::dynamic> promise;
    auto f = promise.getFuture();
    folly::EventBaseManager::get()->getEventBase()->runInLoop([p = std::move(promise), this]() mutable {
      p.setTry(parseBody());
    });

    f.via(folly::getCPUExecutor().get())
        .then([this](folly::dynamic &obj) {
          auto db = DbManager::get_instance();
          bool valid = validateUser(obj);
          if (!valid) {
            return folly::makeFuture<std::unique_ptr<User>>(invalid_user_object());
          }
          auto maybeUser = db->get_user(obj["username"].asString());
          if (maybeUser) {
            return folly::makeFuture<std::unique_ptr<User>>(user_already_exists());
          }
          auto user_manager = UserManager::get_instance();
          std::unique_ptr<User>
              user = user_manager->create_user(obj.at("username").asString(), obj.at("password").asString());
          return folly::makeFuture<std::unique_ptr<User>>(std::move(user));
        }).via(folly::EventBaseManager::get()->getEventBase())
        .then([this](std::unique_ptr<User> user) {
          sendJsonResponse(user->toDynamic(), 201, "Created");

        }).onError([this](const user_already_exists &e) {
          sendJsonResponse(folly::dynamic::object("error", true)
                               ("message", "user with same username already exists"),
                           403,
                           "Conflict");
        }).onError([this](const invalid_user_object &e) {
          sendJsonResponse(folly::dynamic::object("error", true)
                               ("message", "user json body invalid"),
                           401,
                           "Bad arguments");
        }).onError([this](const std::exception &e) {
          sendStringResponse(e.what(), 500, "Internal Error");
        });
  } else if (_method == proxygen::HTTPMethod::OPTIONS && _path == USERS_PATH) {
    ResponseBuilder(downstream_)
        .status(200, "OK")
        .header(proxygen::HTTPHeaderCode::HTTP_HEADER_ACCESS_CONTROL_ALLOW_METHODS, "GET,POST.DELETE,PUT")
        .header(proxygen::HTTPHeaderCode::HTTP_HEADER_ACCESS_CONTROL_ALLOW_HEADERS, "content-type")
        .sendWithEOM();
  }
}

void UserRequestHandler::onBody(std::unique_ptr<folly::IOBuf> body) noexcept {
  if (_body) {
    _body->prependChain(std::move(body));
  } else {
    _body = std::move(body);
  }
}
void UserRequestHandler::onRequest(std::unique_ptr<proxygen::HTTPMessage> headers) noexcept {
  _method = headers->getMethod().get();
  _path = headers->getPath();
  Validations::sanitize_path(_path);

}

void UserRequestHandler::onUpgrade(proxygen::UpgradeProtocol prot) noexcept {
  // ...
}
UserRequestHandler::~UserRequestHandler() = default;
bool UserRequestHandler::validateUser(folly::dynamic &aDynamic) {
  return aDynamic.isObject()
      && aDynamic.find("username") != aDynamic.items().end()
      && aDynamic.find("password") != aDynamic.items().end();
}
UserRequestHandler::UserRequestHandler() = default;
}

max_stars_repo_name: baskeboler/restdbxx

max_issues_repo_name: baskeboler/restdbxx

max_forks_repo_name: baskeboler/restdbxx


text:
/*
 * This program 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 2
 * of the License, or (at your option) any later version.
 *
 * This program 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 program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The Original Code is Copyright (C) 2015 Blender Foundation.
 * All rights reserved.
 */

#ifndef __EIGEN3_SVD_C_API_CC__
#define __EIGEN3_SVD_C_API_CC__

/* Eigen gives annoying huge amount of warnings here, silence them! */
#if defined(__GNUC__) && !defined(__clang__)
#  pragma GCC diagnostic ignored "-Wlogical-op"
#endif

#ifdef __EIGEN3_SVD_C_API_CC__ /* quiet warning */
#endif

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>

#include "svd.h"

using Eigen::JacobiSVD;

using Eigen::NoQRPreconditioner;

using Eigen::ComputeThinU;
using Eigen::ComputeThinV;

using Eigen::Map;
using Eigen::MatrixXf;
using Eigen::VectorXf;

using Eigen::Matrix4f;

void EIG_svd_square_matrix(const int size, const float *matrix, float *r_U, float *r_S, float *r_V)
{
  /* Since our matrix is squared, we can use thinU/V. */
  unsigned int flags = (r_U ? ComputeThinU : 0) | (r_V ? ComputeThinV : 0);

  /* Blender and Eigen matrices are both column-major. */
  JacobiSVD<MatrixXf, NoQRPreconditioner> svd(Map<MatrixXf>((float *)matrix, size, size), flags);

  if (r_U) {
    Map<MatrixXf>(r_U, size, size) = svd.matrixU();
  }

  if (r_S) {
    Map<VectorXf>(r_S, size) = svd.singularValues();
  }

  if (r_V) {
    Map<MatrixXf>(r_V, size, size) = svd.matrixV();
  }
}

#endif /* __EIGEN3_SVD_C_API_CC__ */

ext: cc

max_stars_repo_name: rotoglup/blender

max_stars_repo_licenses: Naumen Condor-1.1 MS-PL

max_issues_repo_name: rotoglup/blender

max_issues_repo_licenses: Naumen Condor-1.1 MS-PL

max_forks_repo_name: rotoglup/blender

max_forks_repo_licenses: Naumen Condor-1.1 MS-PL