text:
// Boost.Geometry

// Copyright (c) 2016, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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)

#ifndef BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP
#define BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP

#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/arithmetic/normalize.hpp>

#include <boost/geometry/formulas/eccentricity_sqr.hpp>
#include <boost/geometry/formulas/flattening.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry {

namespace formula {

template <typename Point3d, typename PointGeo, typename Spheroid>
inline Point3d geo_to_cart3d(PointGeo const& point_geo, Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type calc_t;

    calc_t const c1 = 1;
    calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid);

    calc_t const lon = get_as_radian<0>(point_geo);
    calc_t const lat = get_as_radian<1>(point_geo);

    Point3d res;

    calc_t const sin_lat = sin(lat);

    // "unit" spheroid, a = 1
    calc_t const N = c1 / math::sqrt(c1 - e_sqr * math::sqr(sin_lat));
    calc_t const N_cos_lat = N * cos(lat);

    set<0>(res, N_cos_lat * cos(lon));
    set<1>(res, N_cos_lat * sin(lon));
    set<2>(res, N * (c1 - e_sqr) * sin_lat);

    return res;
}

template <typename PointGeo, typename Spheroid, typename Point3d>
inline void geo_to_cart3d(PointGeo const& point_geo, Point3d & result, Point3d & north, Point3d & east, Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type calc_t;

    calc_t const c1 = 1;
    calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid);

    calc_t const lon = get_as_radian<0>(point_geo);
    calc_t const lat = get_as_radian<1>(point_geo);

    calc_t const sin_lon = sin(lon);
    calc_t const cos_lon = cos(lon);
    calc_t const sin_lat = sin(lat);
    calc_t const cos_lat = cos(lat);

    // "unit" spheroid, a = 1
    calc_t const N = c1 / math::sqrt(c1 - e_sqr * math::sqr(sin_lat));
    calc_t const N_cos_lat = N * cos_lat;

    set<0>(result, N_cos_lat * cos_lon);
    set<1>(result, N_cos_lat * sin_lon);
    set<2>(result, N * (c1 - e_sqr) * sin_lat);

    set<0>(east, -sin_lon);
    set<1>(east, cos_lon);
    set<2>(east, 0);

    set<0>(north, -sin_lat * cos_lon);
    set<1>(north, -sin_lat * sin_lon);
    set<2>(north, cos_lat);
}

template <typename PointGeo, typename Point3d, typename Spheroid>
inline PointGeo cart3d_to_geo(Point3d const& point_3d, Spheroid const& spheroid)
{
    typedef typename coordinate_type<PointGeo>::type coord_t;
    typedef typename coordinate_type<Point3d>::type calc_t;

    calc_t const c1 = 1;
    //calc_t const c2 = 2;
    calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid);

    calc_t const x = get<0>(point_3d);
    calc_t const y = get<1>(point_3d);
    calc_t const z = get<2>(point_3d);
    calc_t const xy_l = math::sqrt(math::sqr(x) + math::sqr(y));

    calc_t const lonr = atan2(y, x);

    // NOTE: Alternative version
    // http://www.iag-aig.org/attach/989c8e501d9c5b5e2736955baf2632f5/V60N2_5FT.pdf
    // calc_t const lonr = c2 * atan2(y, x + xy_l);

    calc_t const latr = atan2(z, (c1 - e_sqr) * xy_l);

    // NOTE: If h is equal to 0 then there is no need to improve value of latitude
    //       because then N_i / (N_i + h_i) = 1
    // http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion

    PointGeo res;

    set_from_radian<0>(res, lonr);
    set_from_radian<1>(res, latr);

    coord_t lon = get<0>(res);
    coord_t lat = get<1>(res);

    math::normalize_spheroidal_coordinates
        <
            typename coordinate_system<PointGeo>::type::units,
            coord_t
        >(lon, lat);

    set<0>(res, lon);
    set<1>(res, lat);

    return res;
}

template <typename Point3d, typename Spheroid>
inline Point3d projected_to_xy(Point3d const& point_3d, Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type coord_t;

    // len_xy = sqrt(x^2 + y^2)
    // r = len_xy - |z / tan(lat)|
    // assuming h = 0
    // lat = atan2(z, (1 - e^2) * len_xy);
    // |z / tan(lat)| = (1 - e^2) * len_xy
    // r = e^2 * len_xy
    // x_res = r * cos(lon) = e^2 * len_xy * x / len_xy = e^2 * x
    // y_res = r * sin(lon) = e^2 * len_xy * y / len_xy = e^2 * y

    coord_t const c0 = 0;
    coord_t const e_sqr = formula::eccentricity_sqr<coord_t>(spheroid);

    Point3d res;

    set<0>(res, e_sqr * get<0>(point_3d));
    set<1>(res, e_sqr * get<1>(point_3d));
    set<2>(res, c0);

    return res;
}

template <typename Point3d, typename Spheroid>
inline Point3d projected_to_surface(Point3d const& direction, Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type coord_t;

    //coord_t const c0 = 0;
    coord_t const c2 = 2;
    coord_t const c4 = 4;

    // calculate the point of intersection of a ray and spheroid's surface
    // the origin is the origin of the coordinate system
    //(x*x+y*y)/(a*a) + z*z/(b*b) = 1
    // x = d.x * t
    // y = d.y * t
    // z = d.z * t
    coord_t const dx = get<0>(direction);
    coord_t const dy = get<1>(direction);
    coord_t const dz = get<2>(direction);

    //coord_t const a_sqr = math::sqr(get_radius<0>(spheroid));
    //coord_t const b_sqr = math::sqr(get_radius<2>(spheroid));
    // "unit" spheroid, a = 1
    coord_t const a_sqr = 1;
    coord_t const b_sqr = math::sqr(get_radius<2>(spheroid) / get_radius<0>(spheroid));

    coord_t const param_a = (dx*dx + dy*dy) / a_sqr + dz*dz / b_sqr;
    coord_t const delta = c4 * param_a;
    // delta >= 0
    coord_t const t = math::sqrt(delta) / (c2 * param_a);

    // result = direction * t
    Point3d result = direction;
    multiply_value(result, t);

    return result;
}

template <typename Point3d, typename Spheroid>
inline bool projected_to_surface(Point3d const& origin, Point3d const& direction, Point3d & result1, Point3d & result2, Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type coord_t;

    coord_t const c0 = 0;
    coord_t const c1 = 1;
    coord_t const c2 = 2;
    coord_t const c4 = 4;

    // calculate the point of intersection of a ray and spheroid's surface
    //(x*x+y*y)/(a*a) + z*z/(b*b) = 1
    // x = o.x + d.x * t
    // y = o.y + d.y * t
    // z = o.z + d.z * t
    coord_t const ox = get<0>(origin);
    coord_t const oy = get<1>(origin);
    coord_t const oz = get<2>(origin);
    coord_t const dx = get<0>(direction);
    coord_t const dy = get<1>(direction);
    coord_t const dz = get<2>(direction);

    //coord_t const a_sqr = math::sqr(get_radius<0>(spheroid));
    //coord_t const b_sqr = math::sqr(get_radius<2>(spheroid));
    // "unit" spheroid, a = 1
    coord_t const a_sqr = 1;
    coord_t const b_sqr = math::sqr(get_radius<2>(spheroid) / get_radius<0>(spheroid));

    coord_t const param_a = (dx*dx + dy*dy) / a_sqr + dz*dz / b_sqr;
    coord_t const param_b = c2 * ((ox*dx + oy*dy) / a_sqr + oz*dz / b_sqr);
    coord_t const param_c = (ox*ox + oy*oy) / a_sqr + oz*oz / b_sqr - c1;

    coord_t const delta = math::sqr(param_b) - c4 * param_a*param_c;

    // equals() ?
    if (delta < c0 || param_a == 0)
    {
        return false;
    }

    // result = origin + direction * t

    coord_t const sqrt_delta = math::sqrt(delta);
    coord_t const two_a = c2 * param_a;

    coord_t const t1 = (-param_b + sqrt_delta) / two_a;
    result1 = direction;
    multiply_value(result1, t1);
    add_point(result1, origin);

    coord_t const t2 = (-param_b - sqrt_delta) / two_a;
    result2 = direction;
    multiply_value(result2, t2);
    add_point(result2, origin);

    return true;
}

template <typename Point3d, typename Spheroid>
inline bool great_elliptic_intersection(Point3d const& a1, Point3d const& a2,
                                        Point3d const& b1, Point3d const& b2,
                                        Point3d & result,
                                        Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type coord_t;

    coord_t c0 = 0;
    coord_t c1 = 1;

    Point3d n1 = cross_product(a1, a2);
    Point3d n2 = cross_product(b1, b2);

    // intersection direction
    Point3d id = cross_product(n1, n2);
    coord_t id_len_sqr = dot_product(id, id);

    if (math::equals(id_len_sqr, c0))
    {
        return false;
    }

    // no need to normalize a1 and a2 because the intersection point on
    // the opposite side of the globe is at the same distance from the origin
    coord_t cos_a1i = dot_product(a1, id);
    coord_t cos_a2i = dot_product(a2, id);
    coord_t gri = math::detail::greatest(cos_a1i, cos_a2i);
    Point3d neg_id = id;
    multiply_value(neg_id, -c1);
    coord_t cos_a1ni = dot_product(a1, neg_id);
    coord_t cos_a2ni = dot_product(a2, neg_id);
    coord_t grni = math::detail::greatest(cos_a1ni, cos_a2ni);

    if (gri >= grni)
    {
        result = projected_to_surface(id, spheroid);
    }
    else
    {
        result = projected_to_surface(neg_id, spheroid);
    }

    return true;
}

template <typename Point3d1, typename Point3d2>
static inline int elliptic_side_value(Point3d1 const& origin, Point3d1 const& norm, Point3d2 const& pt)
{
    typedef typename coordinate_type<Point3d1>::type calc_t;
    calc_t c0 = 0;

    // vector oposite to pt - origin
    // only for the purpose of assigning origin
    Point3d1 vec = origin;
    subtract_point(vec, pt);

    calc_t d = dot_product(norm, vec);

    // since the vector is opposite the signs are opposite
    return math::equals(d, c0) ? 0
        : d < c0 ? 1
        : -1; // d > 0
}

template <typename Point3d, typename Spheroid>
inline bool planes_spheroid_intersection(Point3d const& o1, Point3d const& n1,
                                         Point3d const& o2, Point3d const& n2,
                                         Point3d & ip1, Point3d & ip2,
                                         Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type coord_t;

    coord_t c0 = 0;
    coord_t c1 = 1;

    // Below
    // n . (p - o) = 0
    // n . p - n . o = 0
    // n . p + d = 0
    // n . p = h

    // intersection direction
    Point3d id = cross_product(n1, n2);

    if (math::equals(dot_product(id, id), c0))
    {
        return false;
    }

    coord_t dot_n1_n2 = dot_product(n1, n2);
    coord_t dot_n1_n2_sqr = math::sqr(dot_n1_n2);

    coord_t h1 = dot_product(n1, o1);
    coord_t h2 = dot_product(n2, o2);

    coord_t denom = c1 - dot_n1_n2_sqr;
    coord_t C1 = (h1 - h2 * dot_n1_n2) / denom;
    coord_t C2 = (h2 - h1 * dot_n1_n2) / denom;

    // C1 * n1 + C2 * n2
    Point3d C1_n1 = n1;
    multiply_value(C1_n1, C1);
    Point3d C2_n2 = n2;
    multiply_value(C2_n2, C2);
    Point3d io = C1_n1;
    add_point(io, C2_n2);

    if (! projected_to_surface(io, id, ip1, ip2, spheroid))
    {
        return false;
    }

    return true;
}

template <typename Point3d, typename Spheroid>
inline void experimental_elliptic_plane(Point3d const& p1, Point3d const& p2,
                                        Point3d & v1, Point3d & v2,
                                        Point3d & origin, Point3d & normal,
                                        Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type coord_t;

    Point3d xy1 = projected_to_xy(p1, spheroid);
    Point3d xy2 = projected_to_xy(p2, spheroid);

    // origin = (xy1 + xy2) / 2
    origin = xy1;
    add_point(origin, xy2);
    multiply_value(origin, coord_t(0.5));

    // v1 = p1 - origin
    v1 = p1;
    subtract_point(v1, origin);
    // v2 = p2 - origin
    v2 = p2;
    subtract_point(v2, origin);

    normal = cross_product(v1, v2);
}

template <typename Point3d, typename Spheroid>
inline void experimental_elliptic_plane(Point3d const& p1, Point3d const& p2,
                                        Point3d & origin, Point3d & normal,
                                        Spheroid const& spheroid)
{
    Point3d v1, v2;
    experimental_elliptic_plane(p1, p2, v1, v2, origin, normal, spheroid);
}

template <typename Point3d, typename Spheroid>
inline bool experimental_elliptic_intersection(Point3d const& a1, Point3d const& a2,
                                               Point3d const& b1, Point3d const& b2,
                                               Point3d & result,
                                               Spheroid const& spheroid)
{
    typedef typename coordinate_type<Point3d>::type coord_t;

    coord_t c0 = 0;
    coord_t c1 = 1;

    Point3d a1v, a2v, o1, n1;
    experimental_elliptic_plane(a1, a2, a1v, a2v, o1, n1, spheroid);
    Point3d b1v, b2v, o2, n2;
    experimental_elliptic_plane(b1, b2, b1v, b2v, o2, n2, spheroid);

    if (! detail::vec_normalize(n1) || ! detail::vec_normalize(n2))
    {
        return false;
    }

    Point3d ip1_s, ip2_s;
    if (! planes_spheroid_intersection(o1, n1, o2, n2, ip1_s, ip2_s, spheroid))
    {
        return false;
    }

    // NOTE: simplified test, may not work in all cases
    coord_t dot_a1i1 = dot_product(a1, ip1_s);
    coord_t dot_a2i1 = dot_product(a2, ip1_s);
    coord_t gri1 = math::detail::greatest(dot_a1i1, dot_a2i1);
    coord_t dot_a1i2 = dot_product(a1, ip2_s);
    coord_t dot_a2i2 = dot_product(a2, ip2_s);
    coord_t gri2 = math::detail::greatest(dot_a1i2, dot_a2i2);

    result = gri1 >= gri2 ? ip1_s : ip2_s;

    return true;
}

} // namespace formula

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP

max_stars_repo_name: ZCube/boost-cmake

max_issues_repo_name: ZCube/boost-cmake

max_forks_repo_name: ZCube/boost-cmake


text:
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <eigen-checks/gtest.h>
#include <gtest/gtest.h>

#include "kindr/minimal/transform-2d.h"

#ifndef TEST
#define TEST(a, b) int Test_##a##_##b()
#endif

namespace kindr {
namespace minimal {

constexpr double kEpsilon = 1e-10;
static const Rotation2D r(0.77);
static const Position2D t(1.23597132, -2.247102);
static Eigen::Vector2d v(6.26257419, 1.58356548);

TEST(TestTransformation2D, TestInitialization) {
  const Transformation2D T;
  EXPECT_NEAR(T.getRotation().angle(), 0.0, kEpsilon);
  EXPECT_TRUE(
      EIGEN_MATRIX_NEAR(T.getPosition(), Eigen::Vector2d(0.0, 0.0), kEpsilon));

  const Transformation2D T_rot_pos(r, t);
  EXPECT_NEAR(T_rot_pos.getRotation().angle(), r.angle(), kEpsilon);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(T_rot_pos.getPosition(), t, kEpsilon));

  const Eigen::Matrix<double, 3, 3> transformation_matrix =
      (Eigen::Matrix<double, 3, 3>() << std::cos(r.angle()),
       -std::sin(r.angle()), t.x(), std::sin(r.angle()), std::cos(r.angle()),
       t.y(), 0.0, 0.0, 1.0)
          .finished();
  const Transformation2D T_from_mat(transformation_matrix);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
      transformation_matrix, T_from_mat.getTransformationMatrix(), kEpsilon));
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
      transformation_matrix, T_rot_pos.getTransformationMatrix(), kEpsilon));

  const Eigen::Matrix<double, 3, 3> invalid_transformation_matrix =
      (Eigen::Matrix<double, 3, 3>() << 1.0, 1.0, t.x(), 1.0, 1.0, t.y(), 0.0,
       0.0, 1.1)
          .finished();
  EXPECT_DEATH(
      const Transformation2D T_invalid(invalid_transformation_matrix), "");
}

TEST(TestTransformation2D, TestGettersAndSetters) {
  Transformation2D T;
  T.getRotation() = r;
  T.getPosition() = t;
  EXPECT_NEAR(T.getRotation().angle(), r.angle(), kEpsilon);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(T.getPosition(), t, kEpsilon));

  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
      T.asVector(), (Eigen::Vector3d() << r.angle(), t).finished(), kEpsilon));

  constexpr double angle_new = 3.1415;
  T.getRotation().angle() = angle_new;
  EXPECT_NEAR(T.getRotation().angle(), angle_new, kEpsilon);
}

Eigen::Vector2d fromHomogeneous(const Eigen::Vector3d& v) {
  return v.head<2>() / v[2];
}

Eigen::Vector3d toHomogeneous(const Eigen::Vector2d& v) {
  return (Eigen::Vector3d() << v, 1.0).finished();
}

TEST(TestTransformation2D, TestComposition) {
  const Eigen::Vector3d vh = toHomogeneous(v);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(v, fromHomogeneous(vh), kEpsilon));

  const Transformation2D T(r, t);
  const Eigen::Matrix<double, 3, 3> T_mat = T.getTransformationMatrix();
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(T * v, fromHomogeneous(T_mat * vh), kEpsilon));
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
      T * T * v, fromHomogeneous(T_mat * T_mat * vh), kEpsilon));

  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
      T.inverse().getTransformationMatrix(), T_mat.inverse().eval(), kEpsilon));

  const Eigen::MatrixXd v_vectorized = Eigen::MatrixXd::Random(2, 10);
  const Eigen::MatrixXd v_result = T.transformVectorized(v_vectorized);
  for (int i = 0; i < v_vectorized.cols(); ++i) {
    EXPECT_TRUE(
        EIGEN_MATRIX_NEAR(v_result.col(i), T * v_vectorized.col(i), kEpsilon));
  }
}

TEST(TestTransformation2D, TestCast) {
  const Transformation2D T_double(r, t);
  const Transformation2DTemplate<float> T_float =
      T_double.template cast<float>();

  EXPECT_NEAR(
      T_double.getRotation().angle(),
      static_cast<double>(T_float.getRotation().angle()), 1e-5);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
      T_double.getPosition(), T_float.getPosition().cast<double>(), 1e-5));
}

}  // namespace minimal
}  // namespace kindr

ext: cc


text:
#pragma once
#ifndef _QI_SOCK_SOCKETPTR_HPP
#define _QI_SOCK_SOCKETPTR_HPP
#include <utility>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <qi/messaging/sock/socketwithcontext.hpp>
#include <ka/utility.hpp>

/// @file
/// Contains socket and socket pointer traits used in the sock namespace.

namespace qi { namespace sock {

  /// NetSslSocket Socket
  template<typename Socket>
  using SocketPtr = boost::shared_ptr<Socket>;

  template<typename N>
  using SslSocketPtr = SocketPtr<SslSocket<N>>;
  template<typename N>
  using SocketWithContextPtr = SocketPtr<SocketWithContext<N>>;

  /// Constructs a simple SslSocket. The caller is responsible of managing the
  /// lifetime of the SslContext.
  ///
  /// Network N
  template<typename N>
  SslSocketPtr<N> makeSslSocketPtr(IoService<N>& io, SslContext<N>& context)
  {
    return boost::make_shared<SslSocket<N>>(io, context);
  }

  /// Constructs a SocketWithContext that will ensure that the SslContext shares the
  /// lifetime of the socket.
  ///
  /// Network N
  template<typename N>
  SocketWithContextPtr<N> makeSocketWithContextPtr(IoService<N>& io, const SslContextPtr<N>& context)
  {
    return boost::make_shared<SocketWithContext<N>>(io, context);
  }
}} // namespace qi::sock

#endif // _QI_SOCK_SOCKETPTR_HPP

ext: hpp

max_stars_repo_name: yumilceh/libqi

max_issues_repo_name: yumilceh/libqi

max_forks_repo_name: yumilceh/libqi


text:
#include <tman.h>
#include <boost/python.hpp>

using namespace boost::python;

void export_tman()
{
  class_<TrafficManager, bases<Element>, boost::shared_ptr<TrafficManager>, boost::noncopyable>
        ("TrafficManager", init<string, string, unsigned int, unsigned int, double>())
    .def("class_name", &TrafficManager::class_name)
    .def("processing", &TrafficManager::processing)
    .def("flow_code",  &TrafficManager::flow_code)
  ;
}

ext: cpp


text:
#include "Client.hpp"
#include <boost/log/trivial.hpp>
#include "util.hpp"

Client::Client(xcb_connection_t *conn, xcb_screen_t *screen, xcb_window_t window)
    : conn_(conn),
      screen_(screen),
      window_(window),
      frame_(0),
      moving_(false),
      resizing_(RESIZE_NONE),
      focused_(false),
      close_button_(std::unique_ptr<Button>(new Button()))
{
  CreateFrame();

  const uint32_t values[] = {0};
  xcb_configure_window(conn_, window_,
                       XCB_CONFIG_WINDOW_BORDER_WIDTH,
                       values);
}

Client::~Client()
{
  DestroyFrame();
}

void Client::CreateFrame()
{
  frame_ = xcb_generate_id(conn_);

  xcb_get_geometry_cookie_t cookie = xcb_get_geometry(conn_, window_);
  xcb_get_geometry_reply_t *geometry = xcb_get_geometry_reply(conn_, cookie, nullptr);

  const uint16_t frame_width = geometry->width + BORDER_WIDTH * 2 + 1;
  const uint16_t frame_height = geometry->height + BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1;
  const uint32_t values[] = {XCB_EVENT_MASK_SUBSTRUCTURE_REDIRECT |
                             XCB_EVENT_MASK_SUBSTRUCTURE_NOTIFY |
                             XCB_EVENT_MASK_FOCUS_CHANGE |
                             XCB_EVENT_MASK_POINTER_MOTION |
                             XCB_EVENT_MASK_BUTTON_MOTION |
                             XCB_EVENT_MASK_BUTTON_PRESS |
                             XCB_EVENT_MASK_BUTTON_RELEASE |
                             XCB_EVENT_MASK_KEY_PRESS |
                             XCB_EVENT_MASK_KEY_RELEASE |
                             XCB_EVENT_MASK_FOCUS_CHANGE};

  xcb_create_window(conn_, screen_->root_depth, frame_,
                    screen_->root, geometry->x, geometry->y, frame_width,
                    frame_height, 0, XCB_COPY_FROM_PARENT,
                    screen_->root_visual, XCB_CW_EVENT_MASK, values);

  xcb_change_save_set(conn_, XCB_SET_MODE_INSERT, window_);
  xcb_reparent_window(conn_, window_, frame_, BORDER_WIDTH + 1, TITLEBAR_HEIGHT + BORDER_WIDTH + 1);
  xcb_map_window(conn_, frame_);

  const uint32_t event_mask = XCB_EVENT_MASK_STRUCTURE_NOTIFY |
                              XCB_EVENT_MASK_PROPERTY_CHANGE |
                              XCB_EVENT_MASK_FOCUS_CHANGE;
  xcb_change_window_attributes(conn_, window_, XCB_CW_EVENT_MASK, &event_mask);

  const uint32_t back_pixel = 0xc0c0c0;
  xcb_change_window_attributes(conn_, frame_, XCB_CW_BACK_PIXEL, &back_pixel);
  xcb_clear_area(conn_, 0, frame_, 0, 0, 0, 0);

  close_button_->width_ = 16;
  close_button_->height_ = 14;

  visualtype_ = FindVisualtype(screen_);
  Redraw();
}

void Client::DestroyFrame()
{
  if (frame_ == 0)
  {
    return;
  }

  xcb_unmap_window(conn_, frame_);
  xcb_reparent_window(conn_, window_, screen_->root, 0, 0);
  xcb_change_save_set(conn_, XCB_SET_MODE_DELETE, window_);
  xcb_destroy_window(conn_, frame_);
  frame_ = 0;
}

void Client::Redraw()
{
  if (frame_ == 0)
  {
    return;
  }

  xcb_get_geometry_cookie_t cookie = xcb_get_geometry(conn_, window_);
  xcb_get_geometry_reply_t *geometry = xcb_get_geometry_reply(conn_, cookie, nullptr);

  const uint16_t frame_width = geometry->width + BORDER_WIDTH * 2 + 1;
  const uint16_t frame_height = geometry->height + BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1;

  xcb_pixmap_t pixmap = xcb_generate_id(conn_);
  xcb_create_pixmap(conn_, screen_->root_depth, pixmap, frame_, frame_width, frame_height);
  xcb_aux_sync(conn_);

  cairo_surface_t *surface = cairo_xcb_surface_create(conn_, pixmap, visualtype_, frame_width, frame_height);

  DrawFrame(surface, frame_width, frame_height);

  cairo_surface_finish(surface);
  cairo_surface_destroy(surface);
  surface = nullptr;

  xcb_change_window_attributes(conn_, frame_, XCB_CW_BACK_PIXMAP, &pixmap);
  xcb_clear_area(conn_, 0, frame_, 0, 0, frame_width, frame_height);
}

void Client::DrawFrame(cairo_surface_t *surface, uint16_t frame_width, uint16_t frame_height)
{
  if (frame_ == 0)
  {
    return;
  }

  cairo_xcb_surface_set_size(surface, frame_width, frame_height);
  cairo_t *context = cairo_create(surface);

  cairo_set_antialias(context, CAIRO_ANTIALIAS_NONE);

  cairo_set_source_rgb(context, 0.753, 0.753, 0.753);
  cairo_rectangle(context, 0, 0, frame_width, frame_height);
  cairo_fill(context);

  cairo_rectangle(context, 2, 2, frame_width - 2, frame_height - 2);
  cairo_set_source_rgb(context, 1.0, 1.0, 1.0);
  cairo_set_line_width(context, 1.0);
  cairo_stroke(context);

  cairo_set_source_rgb(context, 0.502, 0.502, 0.502);
  cairo_move_to(context, frame_width, 1);
  cairo_line_to(context, frame_width, frame_height);
  cairo_stroke(context);
  cairo_move_to(context, frame_width, frame_height);
  cairo_line_to(context, 1, frame_height);
  cairo_stroke(context);

  cairo_rectangle(context, 4, 4, frame_width - 7, 18);
  if (focused_)
  {
    cairo_set_source_rgb(context, 0, 0, 0.502);
  }
  else
  {
    cairo_set_source_rgb(context, 0.502, 0.502, 0.502);
  }
  cairo_fill(context);

  xcb_get_property_cookie_t cookie = xcb_get_property(conn_, 0, window_, XCB_ATOM_WM_NAME, XCB_ATOM_STRING, 0, 100);
  xcb_get_property_reply_t *reply = xcb_get_property_reply(conn_, cookie, nullptr);

  if (focused_)
  {
    cairo_set_source_rgb(context, 1.0, 1.0, 1.0);
  }
  else
  {
    cairo_set_source_rgb(context, 0.753, 0.753, 0.753);
  }
  cairo_select_font_face(context, "Arial", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_BOLD);
  cairo_set_font_size(context, 12);

  cairo_font_options_t *font_options = cairo_font_options_create();
  cairo_font_options_set_antialias(font_options, CAIRO_ANTIALIAS_NONE);
  cairo_set_font_options(context, font_options);

  cairo_move_to(context, 7, 16);
  cairo_show_text(context, (char *)xcb_get_property_value(reply));

  close_button_->x_ = frame_width - 21;
  close_button_->y_ = 6;
  close_button_->Draw(context);

  cairo_surface_flush(surface);
  cairo_destroy(context);
}

void Client::OnConfigureRequest(const xcb_configure_request_event_t *e)
{
  const uint16_t frame_width = e->width + BORDER_WIDTH * 2 + 1;
  const uint16_t frame_height = e->height + BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1;

  const uint32_t geometry_window[] = {e->width, e->height};
  xcb_configure_window(conn_, window_,
                       XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                       geometry_window);

  const uint32_t geometry_frame[] = {frame_width, frame_height};
  xcb_configure_window(conn_, frame_,
                       XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                       geometry_frame);

  Redraw();
}

void Client::OnMotionNotify(const xcb_motion_notify_event_t *e)
{
  if (moving_)
  {
    const uint32_t geometry[] = {e->root_x - moving_offset_x_, e->root_y - moving_offset_y_};
    xcb_configure_window(conn_, frame_,
                         XCB_CONFIG_WINDOW_X | XCB_CONFIG_WINDOW_Y,
                         geometry);
    xcb_flush(conn_);
  }

  if (resizing_ != RESIZE_NONE)
  {
    switch (resizing_)
    {
    case RESIZE_NE:
    {
      const uint32_t geometry_window[] = {(resizing_original_x_ + resizing_original_width_ - e->root_x - (BORDER_WIDTH * 2 + 1)),
                                          (resizing_original_y_ + resizing_original_height_ - e->root_y - (BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1))};
      xcb_configure_window(conn_, window_,
                           XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_window);

      const uint32_t geometry_frame[] = {e->root_x, e->root_y,
                                         (resizing_original_x_ + resizing_original_width_ - e->root_x),
                                         (resizing_original_y_ + resizing_original_height_ - e->root_y)};
      xcb_configure_window(conn_, frame_,
                           XCB_CONFIG_WINDOW_X | XCB_CONFIG_WINDOW_Y |
                               XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_frame);
    }
    break;
    case RESIZE_NW:
    {
      const uint32_t geometry_window[] = {e->event_x - (BORDER_WIDTH * 2 + 1),
                                          (resizing_original_y_ + resizing_original_height_ - e->root_y - (BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1))};
      xcb_configure_window(conn_, window_,
                           XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_window);

      const uint32_t geometry_frame[] = {e->root_y,
                                         e->event_x,
                                         (resizing_original_y_ + resizing_original_height_ - e->root_y)};
      xcb_configure_window(conn_, frame_,
                           XCB_CONFIG_WINDOW_Y |
                               XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_frame);
    }
    break;
    case RESIZE_N:
    {
      const uint32_t geometry_window[] = {(resizing_original_y_ + resizing_original_height_ - e->root_y - (BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1))};
      xcb_configure_window(conn_, window_,
                           XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_window);

      const uint32_t geometry_frame[] = {e->root_y,
                                         (resizing_original_y_ + resizing_original_height_ - e->root_y)};
      xcb_configure_window(conn_, frame_,
                           XCB_CONFIG_WINDOW_Y | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_frame);
    }
    break;
    case RESIZE_E:
    {
      const uint32_t geometry_window[] = {(resizing_original_x_ + resizing_original_width_ - e->root_x - (BORDER_WIDTH * 2 + 1))};
      xcb_configure_window(conn_, window_,
                           XCB_CONFIG_WINDOW_WIDTH,
                           geometry_window);

      const uint32_t geometry_frame[] = {e->root_x, (resizing_original_x_ + resizing_original_width_ - e->root_x)};
      xcb_configure_window(conn_, frame_,
                           XCB_CONFIG_WINDOW_X | XCB_CONFIG_WINDOW_WIDTH,
                           geometry_frame);
    }
    break;
    case RESIZE_SE:
    {
      const uint32_t geometry_window[] = {(resizing_original_x_ + resizing_original_width_ - e->root_x - (BORDER_WIDTH * 2 + 1)),
                                          e->event_y - (BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1)};
      xcb_configure_window(conn_, window_,
                           XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_window);

      const uint32_t geometry_frame[] = {e->root_x,
                                         (resizing_original_x_ + resizing_original_width_ - e->root_x),
                                         e->event_y};
      xcb_configure_window(conn_, frame_,
                           XCB_CONFIG_WINDOW_X |
                               XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_frame);
    }
    break;
    case RESIZE_S:
    {
      const uint32_t geometry_window[] = {e->event_y - (BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1)};
      xcb_configure_window(conn_, window_,
                           XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_window);

      const uint32_t geometry_frame[] = {e->event_y};
      xcb_configure_window(conn_, frame_,
                           XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_frame);
    }
    break;
    case RESIZE_SW:
    {
      const uint32_t geometry_window[] = {e->event_x - (BORDER_WIDTH * 2 + 1),
                                          e->event_y - (BORDER_WIDTH * 2 + TITLEBAR_HEIGHT + 1)};
      xcb_configure_window(conn_, window_,
                           XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_window);

      const uint32_t geometry_frame[] = {e->event_x, e->event_y};
      xcb_configure_window(conn_, frame_,
                           XCB_CONFIG_WINDOW_WIDTH | XCB_CONFIG_WINDOW_HEIGHT,
                           geometry_frame);
    }
    break;
    case RESIZE_W:
    {
      const uint32_t geometry_window[] = {e->event_x - (BORDER_WIDTH * 2 + 1)};
      xcb_configure_window(conn_, window_,
                           XCB_CONFIG_WINDOW_WIDTH,
                           geometry_window);

      const uint32_t geometry_frame[] = {e->event_x};
      xcb_configure_window(conn_, frame_,
                           XCB_CONFIG_WINDOW_WIDTH,
                           geometry_frame);
    }
    break;
    default:
      // Shouldn't happen.
      BOOST_LOG_TRIVIAL(info) << "Invalid value of resizing_";
    }

    xcb_flush(conn_);

    if (resizing_previous_mouse_x_ != e->event_x || resizing_previous_mouse_y_ != e->event_y)
    {
      Redraw();
    }

    resizing_previous_mouse_x_ = e->event_x;
    resizing_previous_mouse_y_ = e->event_y;
  }
}

void Client::OnButtonPress(const xcb_button_press_event_t *e)
{
  if (e->detail == XCB_BUTTON_INDEX_1)
  {
    moving_ = false;
    resizing_ = RESIZE_NONE;
    resizing_previous_mouse_x_ = 0;
    resizing_previous_mouse_y_ = 0;

    if (e->event_y > BORDER_WIDTH && e->event_y < (TITLEBAR_HEIGHT + BORDER_WIDTH * 2 + 1))
    {
      if (close_button_->CheckRect(e->event_x, e->event_y))
      {
        close_button_->pressed_ = true;

        // FIXME: Temporary
        Redraw();
        Redraw();
      }
      else
      {
        moving_ = true;
        moving_offset_x_ = e->event_x;
        moving_offset_y_ = e->event_y;
      }
    }
    else
    {
      xcb_get_geometry_cookie_t cookie = xcb_get_geometry(conn_, frame_);
      xcb_get_geometry_reply_t *geometry = xcb_get_geometry_reply(conn_, cookie, nullptr);

      resizing_original_x_ = geometry->x;
      resizing_original_y_ = geometry->y;
      resizing_original_width_ = geometry->width;
      resizing_original_height_ = geometry->height;

      if (e->event_x <= BORDER_WIDTH && e->event_y <= BORDER_WIDTH)
      {
        resizing_ = RESIZE_NE;
      }
      else if (e->event_x <= BORDER_WIDTH && e->event_y < geometry->height - BORDER_WIDTH * 2)
      {
        resizing_ = RESIZE_N;
      }
      else if (e->event_y <= BORDER_WIDTH)
      {
        resizing_ = RESIZE_NW;
      }
      else if (e->event_x <= BORDER_WIDTH && e->event_y < geometry->height - BORDER_WIDTH * 2)
      {
        resizing_ = RESIZE_E;
      }
      else if (e->event_x <= BORDER_WIDTH)
      {
        resizing_ = RESIZE_SE;
      }
      else if (e->event_x >= geometry->width - BORDER_WIDTH * 2 && e->event_y < geometry->height - BORDER_WIDTH * 2)
      {
        resizing_ = RESIZE_W;
      }
      else if (e->event_x >= geometry->width - BORDER_WIDTH * 2)
      {
        resizing_ = RESIZE_SW;
      }
      else
      {
        resizing_ = RESIZE_S;
      }

      // xcb_warp_pointer(conn_, XCB_NONE, frame_, 0, 0, 0, 0, geometry->width, geometry->height);
    }
  }
}

void Client::OnButtonRelease(const xcb_button_release_event_t *e)
{
  if (e->detail == XCB_BUTTON_INDEX_1)
  {
    moving_ = false;
    resizing_ = false;

    if (close_button_->pressed_)
    {
      close_button_->pressed_ = false;
      Redraw();

      if (close_button_->CheckRect(e->event_x, e->event_y))
      {
        xcb_intern_atom_cookie_t cookie = xcb_intern_atom_unchecked(conn_, false, 16, "WM_DELETE_WINDOW");
        xcb_intern_atom_reply_t *reply = xcb_intern_atom_reply(conn_, cookie, nullptr);

        xcb_client_message_event_t event;
        event.response_type = XCB_CLIENT_MESSAGE;
        event.window = window_;
        event.format = 32;
        event.sequence = 0;
        event.type = 0;
        event.data.data32[1] = e->time;
        event.data.data32[0] = reply->atom;

        xcb_send_event(conn_, false, window_, XCB_EVENT_MASK_NO_EVENT, (char *)&event);
      }
    }
  }
}

void Client::OnKeyPress(const xcb_key_press_event_t *e) {}

void Client::OnKeyRelease(const xcb_key_release_event_t *e) {}

void Client::OnFocusIn(const xcb_focus_in_event_t *e)
{
  if (!focused_)
  {
    focused_ = true;
    Redraw();
  }
}

void Client::OnFocusOut(const xcb_focus_out_event_t *e)
{
  if (focused_)
  {
    focused_ = false;
    Redraw();
  }
}


text:
#define BOOST_TEST_MODULE algorithm
#include <boost/test/unit_test.hpp>
#include <insertionfinder/algorithm.hpp>

BOOST_AUTO_TEST_CASE(test) {
    BOOST_TEST(true);
}

max_stars_repo_name: cmsxbc/insertionfinder

max_issues_repo_name: cmsxbc/insertionfinder

max_forks_repo_name: cmsxbc/insertionfinder


text:
#include "benchmark.h"

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <chrono>
#include <iostream>
#include <cstdlib>

void benchmark_Eigen(
    const std::vector<std::string>& parameterNames,
    const Json::Array& parameters,
    const std::vector<std::string>& returnNames,
    Json::Array& returnValues)
{
	//number of runs for time measures
	const int runs = 2;

	int numConfigs = parameters.Size();
	for (int config = 0; config < numConfigs; ++config)
	{
		//Input
		int gridSize = parameters[config][0].AsInt32();
		double totalTime = 0;
		std::cout << "  Grid Size: " << gridSize << std::flush;
		int matrixSize = gridSize * gridSize;

		//Create matrix
#define IDX(x, y) ((y) + (x)*gridSize)
		typedef Eigen::SparseMatrix<float, Eigen::RowMajor> ESMatrix;
		ESMatrix matrix(matrixSize, matrixSize);
		matrix.reserve(Eigen::VectorXi::Constant(matrixSize, 5));
		for (int x = 0; x < gridSize; ++x) for (int y = 0; y < gridSize; ++y)
		{
			int row = IDX(x, y);
			if (x > 0) matrix.insert(row, IDX(x - 1, y)) = -1;
			if (y > 0) matrix.insert(row, IDX(x, y - 1)) = -1;
			matrix.insert(row, row) = 4;
			if (y < gridSize - 1) matrix.insert(row, IDX(x, y + 1)) = -1;
			if (x < gridSize - 1) matrix.insert(row, IDX(x + 1, y)) = -1;
		}
		matrix.makeCompressed();

		//Create vector
		Eigen::VectorXf rhs = Eigen::VectorXf::Zero(matrixSize);
		std::srand(42);
		for (int x = 0; x < gridSize; ++x) for (int y = 0; y < gridSize; ++y) {
			if (x == 0 || y == 0 || x == gridSize - 1 || y == gridSize - 1)
				rhs[IDX(x, y)] = std::rand() / float(RAND_MAX);
		}
		Eigen::VectorXf result = Eigen::VectorXf::Zero(matrixSize);

        //Run it multiple times
		int iterations = 0; float error = 0;
        for (int run = 0; run < runs; ++run)
        {
            //Main logic
            auto start = std::chrono::steady_clock::now();

			Eigen::ConjugateGradient<ESMatrix> cg(matrix);
			cg.setTolerance(1e-4);
			result.noalias() = cg.solve(rhs);

            auto finish = std::chrono::steady_clock::now();
            double elapsed = std::chrono::duration_cast<
                std::chrono::duration<double> >(finish - start).count() * 1000;
            totalTime += elapsed;

			iterations = cg.iterations();
			error = cg.error();
        }

        //Result
        Json::Array resultJson;
        double finalTime = totalTime / runs;
		resultJson.PushBack(finalTime);
        returnValues.PushBack(resultJson);
		std::cout << " -> " << finalTime << "ms" << " (iterations=" << iterations << ", error=" << error << ")" << std::endl;
    }
}

max_stars_repo_name: chrismile/cuMat

max_issues_repo_name: chrismile/cuMat

max_forks_repo_name: chrismile/cuMat


text:
/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#include <iostream>
#include <istream>

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

#include "support/parsers/mcc.hh"
#include "support/parsers/ndr.hh"
#include "support/parsers/net.hh"
#include "support/parsers/nupn.hh"
#include "support/parsers/parse_pn.hh"
#include "support/parsers/pnml.hh"

namespace pnmc { namespace parsers {

/*------------------------------------------------------------------------------------------------*/

std::shared_ptr<pn::net>
parse(const pn_configuration& conf)
{
  boost::filesystem::ifstream file_stream;
  std::istream* in = conf.file
                   ? file_stream.open(*conf.file), &file_stream
                   : &std::cin;

  if (in->peek() == std::char_traits<char>::eof())
  {
    throw std::runtime_error("Empty input");
  }

  switch (conf.file_type)
  {
    case (conf::pn_format::ndr)  : return parsers::ndr(*in);
    case (conf::pn_format::net)  : return parsers::net(*in);
    case (conf::pn_format::nupn) : return parsers::nupn(*in);
    case (conf::pn_format::pnml) : return parsers::pnml(*in);
  }
  __builtin_unreachable();
}

/*------------------------------------------------------------------------------------------------*/

}} // namespace pnmc::parsers

max_stars_repo_name: ahamez/pnmc

max_issues_repo_name: ahamez/pnmc

max_forks_repo_name: ahamez/pnmc


text:
//  (C) Copyright John Maddock 2002 - 2003.
//  (C) Copyright Jens Maurer 2002 - 2003.
//  (C) Copyright Beman Dawes 2002 - 2003.
//  Use, modification and distribution are 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 most recent version.

//  Comeau STL:

#if !defined(__LIBCOMO__)
#include <boost/config/no_tr1/utility.hpp>
#if !defined(__LIBCOMO__)
#error "This is not the Comeau STL!"
#endif
#endif

//
// std::streambuf<wchar_t> is non-standard
// NOTE: versions of libcomo prior to beta28 have octal version numbering,
// e.g. version 25 is 21 (dec)
#if __LIBCOMO_VERSION__ <= 22
#define BOOST_NO_STD_WSTREAMBUF
#endif

#if (__LIBCOMO_VERSION__ <= 31) && defined(_WIN32)
#define BOOST_NO_SWPRINTF
#endif

#if __LIBCOMO_VERSION__ >= 31
#define BOOST_HAS_HASH
#define BOOST_HAS_SLIST
#endif

//  C++0x headers not yet implemented
//
#define BOOST_NO_CXX11_HDR_ARRAY
#define BOOST_NO_CXX11_HDR_CHRONO
#define BOOST_NO_CXX11_HDR_CODECVT
#define BOOST_NO_CXX11_HDR_CONDITION_VARIABLE
#define BOOST_NO_CXX11_HDR_EXCEPTION
#define BOOST_NO_CXX11_HDR_FORWARD_LIST
#define BOOST_NO_CXX11_HDR_FUTURE
#define BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#define BOOST_NO_CXX11_HDR_MUTEX
#define BOOST_NO_CXX11_HDR_RANDOM
#define BOOST_NO_CXX11_HDR_RATIO
#define BOOST_NO_CXX11_HDR_REGEX
#define BOOST_NO_CXX11_HDR_SYSTEM_ERROR
#define BOOST_NO_CXX11_HDR_THREAD
#define BOOST_NO_CXX11_HDR_TUPLE
#define BOOST_NO_CXX11_HDR_TYPE_TRAITS
#define BOOST_NO_CXX11_HDR_TYPEINDEX
#define BOOST_NO_CXX11_HDR_UNORDERED_MAP
#define BOOST_NO_CXX11_HDR_UNORDERED_SET
#define BOOST_NO_CXX11_NUMERIC_LIMITS
#define BOOST_NO_CXX11_ALLOCATOR
#define BOOST_NO_CXX11_POINTER_TRAITS
#define BOOST_NO_CXX11_ATOMIC_SMART_PTR
#define BOOST_NO_CXX11_SMART_PTR
#define BOOST_NO_CXX11_HDR_FUNCTIONAL
#define BOOST_NO_CXX11_HDR_ATOMIC
#define BOOST_NO_CXX11_STD_ALIGN
#define BOOST_NO_CXX11_ADDRESSOF

#if defined(__has_include)
#if !__has_include(<shared_mutex>)
#define BOOST_NO_CXX14_HDR_SHARED_MUTEX
#elif __cplusplus < 201402
#define BOOST_NO_CXX14_HDR_SHARED_MUTEX
#endif
#else
#define BOOST_NO_CXX14_HDR_SHARED_MUTEX
#endif

// C++14 features
#define BOOST_NO_CXX14_STD_EXCHANGE

// C++17 features
#define BOOST_NO_CXX17_STD_APPLY
#define BOOST_NO_CXX17_STD_INVOKE
#define BOOST_NO_CXX17_ITERATOR_TRAITS

//
// Intrinsic type_traits support.
// The SGI STL has it's own __type_traits class, which
// has intrinsic compiler support with SGI's compilers.
// Whatever map SGI style type traits to boost equivalents:
//
#define BOOST_HAS_SGI_TYPE_TRAITS

#define BOOST_STDLIB                                                           \
  "Comeau standard library " BOOST_STRINGIZE(__LIBCOMO_VERSION__)

max_stars_repo_name: henrywarhurst/matrix

max_issues_repo_name: henrywarhurst/matrix

max_forks_repo_name: henrywarhurst/matrix


text: #include <boost/mpl/aux_/preprocessed/bcc551/full_lambda.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*
 * Copyright (c) 2011-2017, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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 HOLDER 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.
 */

#ifndef DART_MATH_HELPERS_HPP_
#define DART_MATH_HELPERS_HPP_

// Standard Libraries
#include <cfloat>
#include <climits>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <iomanip>
#include <iostream>

// External Libraries
#include <Eigen/Dense>
// Local Headers
#include "dart/math/Constants.hpp"
#include "dart/math/MathTypes.hpp"

namespace dart {
namespace math {

//==============================================================================
template <typename T>
constexpr T toRadian(const T& degree)
{
  return degree * constants<T>::pi() / 180.0;
}

//==============================================================================
template <typename T>
constexpr T toDegree(const T& radian)
{
  return radian * 180.0 / constants<T>::pi();
}

/// \brief a cross b = (CR*a) dot b
/// const Matd CR(2,2,0.0,-1.0,1.0,0.0);
const Eigen::Matrix2d CR((Eigen::Matrix2d() << 0.0, -1.0, 1.0, 0.0).finished());

inline int delta(int _i, int _j) {
  if (_i == _j)
    return 1;
  return 0;
}

template <typename T> inline constexpr
int sign(T x, std::false_type)
{
  return static_cast<T>(0) < x;
}

template <typename T> inline constexpr
int sign(T x, std::true_type)
{
  return (static_cast<T>(0) < x) - (x < static_cast<T>(0));
}

template <typename T> inline constexpr
int sign(T x)
{
  return sign(x, std::is_signed<T>());
}

inline double sqr(double _x) {
  return _x*_x;
}

inline double Tsinc(double _theta) {
  return 0.5-sqrt(_theta)/48;
}

inline bool isZero(double _theta) {
  return (std::abs(_theta) < 1e-6);
}

inline double asinh(double _X) {
  return log(_X + sqrt(_X * _X + 1));
}

inline double acosh(double _X) {
  return log(_X + sqrt(_X * _X - 1));
}

inline double atanh(double _X) {
  return log((1 + _X)/(1 - _X))/ 2;
}

inline double asech(double _X) {
  return log((sqrt(-_X * _X + 1) + 1) / _X);
}

inline double acosech(double _X) {
  return log((sign(_X) * sqrt(_X * _X + 1) +1) / _X);
}

inline double acotanh(double _X) {
  return log((_X + 1) / (_X - 1)) / 2;
}

inline double round(double _x) {
  return floor(_x + 0.5);
}

inline double round2(double _x) {
  int gintx = static_cast<int>(std::floor(_x));
  if (_x - gintx < 0.5)
    return static_cast<double>(gintx);
  else
    return static_cast<double>(gintx + 1.0);
}

template <typename T>
inline T clip(const T& val, const T& lower, const T& upper)
{
  return std::max(lower, std::min(val, upper));
}

template <typename DerivedA, typename DerivedB>
inline typename DerivedA::PlainObject clip(
    const Eigen::MatrixBase<DerivedA>& val,
    const Eigen::MatrixBase<DerivedB>& lower,
    const Eigen::MatrixBase<DerivedB>& upper)
{
  return lower.cwiseMax(val.cwiseMin(upper));
}

inline bool isEqual(double _x, double _y) {
  return (std::abs(_x - _y) < 1e-6);
}

// check if it is an integer
inline bool isInt(double _x) {
  if (isEqual(round(_x), _x))
    return true;
  return false;
}

/// \brief Returns whether _v is a NaN (Not-A-Number) value
inline bool isNan(double _v) {
#ifdef _WIN32
  return _isnan(_v) != 0;
#else
  return std::isnan(_v);
#endif
}

/// \brief Returns whether _m is a NaN (Not-A-Number) matrix
inline bool isNan(const Eigen::MatrixXd& _m) {
  for (int i = 0; i < _m.rows(); ++i)
    for (int j = 0; j < _m.cols(); ++j)
      if (isNan(_m(i, j)))
        return true;

  return false;
}

/// \brief Returns whether _v is an infinity value (either positive infinity or
/// negative infinity).
inline bool isInf(double _v) {
#ifdef _WIN32
  return !_finite(_v);
#else
  return std::isinf(_v);
#endif
}

/// \brief Returns whether _m is an infinity matrix (either positive infinity or
/// negative infinity).
inline bool isInf(const Eigen::MatrixXd& _m) {
  for (int i = 0; i < _m.rows(); ++i)
    for (int j = 0; j < _m.cols(); ++j)
      if (isInf(_m(i, j)))
        return true;

  return false;
}

/// \brief Returns whether _m is symmetric or not
inline bool isSymmetric(const Eigen::MatrixXd& _m, double _tol = 1e-6) {
  std::size_t rows = _m.rows();
  std::size_t cols = _m.cols();

  if (rows != cols)
    return false;

  for (std::size_t i = 0; i < rows; ++i) {
    for (std::size_t j = i + 1; j < cols; ++j) {
      if (std::abs(_m(i, j) - _m(j, i)) > _tol) {
        std::cout << "A: " << std::endl;
        for (std::size_t k = 0; k < rows; ++k) {
          for (std::size_t l = 0; l < cols; ++l)
            std::cout << std::setprecision(4) << _m(k, l) << " ";
          std::cout << std::endl;
        }

        std::cout << "A(" << i << ", " << j << "): " << _m(i, j) << std::endl;
        std::cout << "A(" << j << ", " << i << "): " << _m(i, j) << std::endl;
        return false;
      }
    }
  }

  return true;
}

inline unsigned seedRand() {
  time_t now = time(0);
  unsigned char* p = reinterpret_cast<unsigned char*>(&now);
  unsigned seed = 0;
  std::size_t i;

  for (i = 0; i < sizeof(now); i++)
    seed = seed * (UCHAR_MAX + 2U) + p[i];

  srand(seed);
  return seed;
}

inline double random(double _min, double _max) {
  return _min + ((static_cast<double>(rand()) / (RAND_MAX + 1.0))
                * (_max - _min));
}

template<int N>
Eigen::Matrix<double, N, 1> randomVector(double _min, double _max)
{
  Eigen::Matrix<double, N, 1> v;
  for(std::size_t i=0; i<N; ++i)
    v[i] = random(_min, _max);

  return v;
}

template<int N>
Eigen::Matrix<double, N, 1> randomVector(double _limit)
{
  return randomVector<N>(-std::abs(_limit), std::abs(_limit));
}

//==============================================================================
inline Eigen::VectorXd randomVectorXd(std::size_t size, double min, double max)
{
  Eigen::VectorXd v = Eigen::VectorXd::Zero(size);

  for (std::size_t i = 0; i < size; ++i)
    v[i] = random(min, max);

  return v;
}

//==============================================================================
inline Eigen::VectorXd randomVectorXd(std::size_t size, double limit)
{
  return randomVectorXd(size, -std::abs(limit), std::abs(limit));
}

namespace suffixes {

//==============================================================================
constexpr double operator"" _pi(long double x)
{
  return x * constants<double>::pi();
}

//==============================================================================
constexpr double operator"" _pi(unsigned long long int x)
{
  return operator"" _pi(static_cast<long double>(x));
}

//==============================================================================
constexpr double operator"" _rad(long double angle)
{
  return angle;
}

//==============================================================================
constexpr double operator"" _rad(unsigned long long int angle)
{
  return operator"" _rad(static_cast<long double>(angle));
}

//==============================================================================
constexpr double operator"" _deg(long double angle)
{
  return toRadian(angle);
}

//==============================================================================
constexpr double operator"" _deg(unsigned long long int angle)
{
  return operator"" _deg(static_cast<long double>(angle));
}

}  // namespace suffixes

}  // namespace math

namespace Color
{

inline Eigen::Vector4d Red(double alpha)
{
  return Eigen::Vector4d(0.9, 0.1, 0.1, alpha);
}

inline Eigen::Vector3d Red()
{
  return Eigen::Vector3d(0.9, 0.1, 0.1);
}

inline Eigen::Vector3d Fuchsia()
{
  return Eigen::Vector3d(1.0, 0.0, 0.5);
}

inline Eigen::Vector4d Fuchsia(double alpha)
{
  return Eigen::Vector4d(1.0, 0.0, 0.5, alpha);
}

inline Eigen::Vector4d Orange(double alpha)
{
  return Eigen::Vector4d(1.0, 0.63, 0.0, alpha);
}

inline Eigen::Vector3d Orange()
{
  return Eigen::Vector3d(1.0, 0.63, 0.0);
}

inline Eigen::Vector4d Green(double alpha)
{
  return Eigen::Vector4d(0.1, 0.9, 0.1, alpha);
}

inline Eigen::Vector3d Green()
{
  return Eigen::Vector3d(0.1, 0.9, 0.1);
}

inline Eigen::Vector4d Blue(double alpha)
{
  return Eigen::Vector4d(0.1, 0.1, 0.9, alpha);
}

inline Eigen::Vector3d Blue()
{
  return Eigen::Vector3d(0.1, 0.1, 0.9);
}

inline Eigen::Vector4d White(double alpha)
{
  return Eigen::Vector4d(1.0, 1.0, 1.0, alpha);
}

inline Eigen::Vector3d White()
{
  return Eigen::Vector3d(1.0, 1.0, 1.0);
}

inline Eigen::Vector4d Black(double alpha)
{
  return Eigen::Vector4d(0.05, 0.05, 0.05, alpha);
}

inline Eigen::Vector3d Black()
{
  return Eigen::Vector3d(0.05, 0.05, 0.05);
}

inline Eigen::Vector4d Gray(double alpha)
{
  return Eigen::Vector4d(0.6, 0.6, 0.6, alpha);
}

inline Eigen::Vector3d Gray()
{
  return Eigen::Vector3d(0.6, 0.6, 0.6);
}

inline Eigen::Vector4d Random(double alpha)
{
  return Eigen::Vector4d(math::random(0.0, 1.0),
                         math::random(0.0, 1.0),
                         math::random(0.0, 1.0),
                         alpha);
}

inline Eigen::Vector3d Random()
{
  return Eigen::Vector3d(math::random(0.0, 1.0),
                         math::random(0.0, 1.0),
                         math::random(0.0, 1.0));
}

} // namespace Color

}  // namespace dart

#endif  // DART_MATH_HELPERS_HPP_

max_stars_repo_name: malasiot/dart-vsim-octomap

max_issues_repo_name: malasiot/dart-vsim-octomap

max_forks_repo_name: malasiot/dart-vsim-octomap


text:
#pragma once

#include <memory>
#include <vector>
#include <set>
#include <map>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/graph/iteration_macros.hpp>

#include <boost/graph/graphviz.hpp>

#include "chart_traits.hpp"
#include "events.hpp"

namespace core { namespace graph {

namespace detail
{

	/*! We are building just a wrapper around boost::adjacency_list
	    to implement some custom/predefined functionalities.
	*/
	class chart_base
	{ // Just for RTTI
		public:
			virtual ~chart_base() {};
	};

	template <class VertexType, class EdgeType, int Behaviour>
	class chart_impl : public chart_base
	{
		public:
			typedef typename chart_traits<VertexType, EdgeType, Behaviour>::vertex_type vertex_type;
			typedef typename chart_traits<VertexType, EdgeType, Behaviour>::edge_type edge_type;
			typedef typename chart_traits<VertexType, EdgeType, Behaviour>::behaviour behaviour;

			typedef typename std::shared_ptr<vertex_type> vertex_type_ptr;
			typedef typename std::shared_ptr<edge_type> edge_type_ptr;
			typedef std::shared_ptr<chart_impl> chart_ptr;

		protected:
			/* Necesitamos que los vértices y aristas se almacenen en listas (boost::listS) para que no
			 *   se invaliden los 'vertex_id' y 'edge_id' que estamos devolviendo en las funciones.
			 *
			 *   From boost documentation:
			 *
			 *   > If the VertexList of the graph is vecS, then the graph has a builtin vertex indices
			 *   > accessed via the property map for the vertex_index_t property. The indices fall
			 *   > in the range [0, num_vertices(g)) and are contiguous. When a vertex is removed
			 *   > the indices are adjusted so that they retain these properties.
			 */
			typedef boost::adjacency_list<boost::listS, boost::listS, behaviour, boost::property<boost::vertex_index_t, size_t, vertex_type_ptr>, edge_type_ptr> _t_graph;
			//                                                                   ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ cuando utilizo listS no se crea un index para los vértices, el cual necesito para calcular los connected components.
			//                                   ^^^^^         ^^^^^ cannot use boost::vecS, see above.

		public:
			typedef typename _t_graph::vertex_descriptor vertex_id;
			typedef typename _t_graph::edge_descriptor edge_id;

			typedef std::map<vertex_type_ptr, vertex_id> _t_vertices;
			typedef std::map<edge_type_ptr, edge_id> _t_edges;

			typedef std::map<vertex_id, typename _t_graph::vertices_size_type> _t_connected_components;

		public:
			chart_impl() {};
			virtual ~chart_impl()
			{
				while (_edges.size())
				{
					this->remove_edge( _edges.begin()->first);
				}

				while(_vertices.size())
				{
					this->remove_vertex( _vertices.begin()->first);
				}
			};

			std::pair<vertex_id, bool> add_vertex(vertex_type_ptr ptr)
			{
				// Is it already added?
				_t_vertices::iterator it; bool inserted;
				std::tie(it, inserted) = _vertices.insert(std::make_pair(ptr, vertex_id()));
				if (inserted)
				{
					it->second = boost::add_vertex(boost::property<boost::vertex_index_t, size_t, vertex_type_ptr>(boost::num_vertices(_graph), ptr), _graph);
					ptr->insert_chart(this);
					events::on_vertex_added_to_chart(*it, *this);
				}
				return std::make_pair(it->second, inserted);
			};

			void remove_vertex(vertex_type_ptr ptr)
			{
				_t_vertices::iterator it = _vertices.find(ptr);
				if (it != _vertices.end())
				{
					/*! Si elimino un vértice se eliminan todos los edges conectados
					  en _graph, pero no se eliminan las conexiones establecidas entre
					  estos elementos. Sólo cuando se destruye un vértice se terminan
					  de eliminar las conexiones (o cuando se llama a la función disconnect
					  de los edges)
					*/
					boost::remove_vertex( it->second, _graph);
					_vertices.erase(it);
					ptr->erase_chart(this);
					events::on_vertex_removed_from_chart(ptr, *this);
				}
			};

			void remove_vertex(vertex_id v_id)
			{
				vertex_type_ptr ptr = _graph[v_id];
				this->remove_vertex(ptr);
			};

			std::pair<edge_id, bool> add_edge(edge_type_ptr ptr, const vertex_id& source, const vertex_id& target)
			{
				/*! Adds edge to chart and connect selected vertices, so:
				 * vertices must exists
				 * edge must not be connected
				 */
				if (ptr->is_connected())
				{
					throw std::runtime_error("edge is already connected");
				}

				vertex_type_ptr source_ptr = this->get_vertex(source);
				vertex_type_ptr target_ptr = this->get_vertex(target);
				_t_edges::iterator it; bool inserted;
				std::tie(it, inserted) = _edges.insert(std::make_pair(ptr, edge_id()));
				if (inserted)
				{
					std::tie(it->second, inserted) = boost::add_edge(source, target, ptr, _graph);
					/* boost::add_edge: Adds edge (u,v) to the graph and returns the edge descriptor for the new edge.
					    For graphs that do not allow parallel edges, if the edge is already
					    in the graph then a duplicate will not be added and the bool flag
					    will be false. When the flag is false, the returned edge descriptor points
					    to the already existing edge.
					*/
					if (inserted)
					{
						// Create connection
						ptr->connect(source_ptr.get(), target_ptr.get());
						ptr->set_chart(this, [this, it](){this->remove_edge(it->second);} );
						events::on_edge_added_to_chart(*it, *this);
					}
				}
				return std::make_pair(it->second, inserted);
			};

			void remove_edge(edge_type_ptr ptr)
			{
				_t_edges::iterator it = _edges.find(ptr);
				if (it != _edges.end())
				{
					boost::remove_edge(it->second, _graph);
					_edges.erase(it);
					ptr->remove_chart(this);
					events::on_edge_removed_from_chart(ptr, *this);
				}
			};

			void remove_edge(edge_id id)
			{
				edge_type_ptr ptr = _graph[id];
				this->remove_edge(ptr);
			};

			const _t_vertices& get_vertices() const { return _vertices;};
			const _t_edges& get_edges() const { return _edges;};

			vertex_type_ptr get_vertex(const vertex_id& id) const { return _graph[id];};
			edge_type_ptr get_edge(const edge_id& id) const { return _graph[id];};

			vertex_id get_vertex_id(const vertex_type_ptr& ptr) const { return _vertices.find(ptr)->second;};
			edge_id get_edge_id(const edge_type_ptr& ptr) const { return _edges.find(ptr)->second;};

			size_t connected_components(_t_connected_components& vertex_at_cmp) const
			{
				/* Refundido de: http://stackoverflow.com/questions/7935417/how-provide-a-vertex-index-property-for-my-graph
				                 http://stackoverflow.com/questions/3183186/perform-connected-components-with-boost-adjacency-list-where-vertexlist-lists
				                 http://lists.boost.org/boost-users/2007/08/30612.php
				                 http://www.boost.org/doc/libs/1_35_0/libs/graph/doc/faq.html
				*/

				boost::associative_property_map< _t_connected_components > component_map(vertex_at_cmp);

				size_t index = 0;
				BGL_FORALL_VERTICES(v, _graph, _t_graph)
				{
					boost::put(component_map, v, index++);
				}

				return boost::connected_components(_graph, component_map);
			}

		protected:
			_t_graph _graph;
			_t_vertices _vertices;
			_t_edges _edges;
	};

	/****************
	*   CHART with VERTEX_INNER
	****************/
	template <class VertexType, class EdgeType, int Behaviour, typename Enable=void>
	class chart_vertex_inner
	{
		static_assert(true, "NON");
	};

	template <class VertexType, class EdgeType, int Behaviour>
	class chart_vertex_inner<VertexType, EdgeType, Behaviour, typename std::enable_if<detail::is_vertex_inner<VertexType>::value>::type>
	: public virtual chart_impl<VertexType, EdgeType, Behaviour>
	{
			static_assert(true, "YES");
			// Add some functions to 'chart' when vertex class implements vertex_inner behaviour
		public:
			std::pair<vertex_id, bool> create_vertex(typename VertexType::inner_type_ptr inner)
			{
				vertex_type_ptr ptr(new vertex_type(inner));
				return this->add_vertex(ptr);
			};
	};

	/****************
	*   CHART with EDGE_INNER
	****************/
	template <class VertexType, class EdgeType, int Behaviour, typename Enable=void>
	class chart_edge_inner : public virtual chart_impl<VertexType, EdgeType, Behaviour>
	{
		public:
			std::pair<edge_id, bool> create_edge(const vertex_id& source, const vertex_id& target)
			{
				edge_type_ptr ptr(new edge_type());
				return this->add_edge(ptr, source, target);
			};
	};

	template <class VertexType, class EdgeType, int Behaviour>
	class chart_edge_inner<VertexType, EdgeType, Behaviour, typename std::enable_if<detail::is_edge_inner<EdgeType>::value>::type>
	: public virtual chart_impl<VertexType, EdgeType, Behaviour>
	{
		// Add some functions to 'chart' when vertex class implements vertex_inner behaviour
		public:
			std::pair<edge_id, bool> create_edge(typename EdgeType::inner_type_ptr inner, const vertex_id& source, const vertex_id& target)
			{
				edge_type_ptr ptr(new edge_type(inner));
				return this->add_edge(ptr, source, target);
			};
	};

	/****************
	*   Behaviour
	****************/
	template <class VertexType, class EdgeType, int Behaviour>
	class chart_behaviour
	{
	};

	template <class VertexType, class EdgeType>
	class chart_behaviour<VertexType, EdgeType, BIDIRECTIONAL> : public virtual chart_impl<VertexType, EdgeType, BIDIRECTIONAL>
	{
		public:
			// All this functions requires "Directional" graphs
			void get_edges_outgoing(const vertex_id& vertex, std::vector<edge_id>& edges) const
			{
				boost::graph_traits<_t_graph>::out_edge_iterator it_begin, it_end;
				boost::tie(it_begin, it_end) = boost::out_edges(vertex, _graph);
				//std::copy(it_begin, it_end, edges.begin()); //! \todo TODO: Why not?
				std::for_each(it_begin, it_end, [&edges](const edge_id& id) {edges.push_back(id);});
			};

			void get_edges_incoming(const vertex_id& vertex, std::vector<edge_id>& edges) const
			{
				//boost::graph_traits<_t_graph>::in_edge_iterator it_begin, it_end;
				_t_graph::in_edge_iterator it_begin, it_end;
				boost::tie(it_begin, it_end) = boost::in_edges(vertex, _graph);
				//std::copy(it_begin, it_end, edges.begin()); //! \todo TODO: Why not?
				std::for_each(it_begin, it_end, [&edges](const edge_id& id) {edges.push_back(id);});
			};

			vertex_id get_source(const edge_id& edge) const
			{
				return boost::source(edge, _graph);
			};

			vertex_id get_target(const edge_id& edge) const
			{
				return boost::target(edge, _graph);
			};
	};

	template <class VertexType, class EdgeType>
	class chart_behaviour<VertexType, EdgeType, UNDIRECTED> : public virtual chart_impl<VertexType, EdgeType, UNDIRECTED>
	{
		public:
			std::pair<vertex_id, vertex_id> get_connected(const edge_id& edge) const
			{
				return std::make_pair(boost::source(edge, _graph), boost::target(edge, _graph));
			};
	};
}

// Join all policies
template <class VertexType, class EdgeType, int Behaviour>
class chart : public virtual detail::chart_impl<VertexType, EdgeType, Behaviour>,
              public virtual detail::chart_vertex_inner<VertexType, EdgeType, Behaviour>,
              public virtual detail::chart_edge_inner<VertexType, EdgeType, Behaviour>,
              public virtual detail::chart_behaviour<VertexType, EdgeType, Behaviour>
{
	public:
		virtual ~chart() {};
};

}}

max_stars_repo_name: jgsogo/boost-graph-wrapper

max_issues_repo_name: jgsogo/boost-graph-wrapper

max_forks_repo_name: jgsogo/boost-graph-wrapper


text:
#include <string>
#include <vector>
#include <iostream>
#include <curl/curl.h>
#include <json_spirit_reader_template.h>
#include <boost/program_options.hpp>
#include <boost/algorithm/string/join.hpp>

size_t callback(void * ptr, size_t size, size_t nmemb, void *)
{
  json_spirit::Value value;
  if(json_spirit::read_string(std::string(static_cast<const char *>(ptr), size * nmemb), value))
  {
    if(value.type() == json_spirit::obj_type)
    {
      for(const auto & item : value.get_obj())
      {
        if(item.name_ == "text" && item.value_.type() == json_spirit::str_type)
        {
          std::cout << item.value_.get_str() << std::endl;
        }
      }
    }
  }
  return size * nmemb;
}

namespace po = boost::program_options;

int main(int argc, char ** argv)
{
  std::string username;
  std::string password;
  std::vector<std::string> keywords;
  po::options_description desc("Options");
  desc.add_options()
    ("help,h", "display this help message")
    ("username,u", po::value<std::string>(&username), "twitter account user name")
    ("password,p", po::value<std::string>(&password), "twitter account password")
    ("keyword,k", po::value<std::vector<std::string> >(&keywords), "keywords to search");

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

  if(vm.count("help"))
  {
    std::cout << desc << std::endl;
  }
  else
  {
    std::string track = std::string("track=") + boost::join(keywords, ",");

    CURL * curl = curl_easy_init();
    curl_easy_setopt(curl, CURLOPT_USERPWD, (username + ":" + password).c_str());
    curl_easy_setopt(curl, CURLOPT_URL, "https://stream.twitter.com/1/statuses/filter.json");
    curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, callback);
    curl_easy_setopt(curl, CURLOPT_POST, 1);
    curl_easy_setopt(curl, CURLOPT_POSTFIELDS, track.data());
    curl_easy_setopt(curl, CURLOPT_POSTFIELDSIZE, track.size());
    curl_easy_perform(curl);

    curl_easy_cleanup(curl);
  }
}


text:
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <math.h>
#include <cstdio>
#include <iomanip>
#include <algorithm>
#include <math.h>
#include <typeinfo>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#define IMDP 15

using namespace Eigen;
using namespace std;

bool isAddWeight = false;
bool isCutOffICP = false;

//Define the numPoint to get the Point's number
int numPoint = 0;

//Define the breakPoint
int breakPointNum = 0;

//Define the array to get slam track and GPS track
double x0GPS = 0.0;
double y0GPS = 0.0;
double *zTemp;
double *timeTemp;
double *gpsTimeTemp;
double *arrayWCTemp;

//Define string to get filename;
string fileSLAMTrack;
string fileGPSTrack;
string fileisWeightCoe;
string fileWeightCoe;
string fileDoneICPTrack;

//Define the matrix
MatrixXd slamCoordAll;
MatrixXd gpsCoordAll;
MatrixXd weightCoe;

MatrixXd aCoord;
MatrixXd bCoord;

MatrixXd transformT;
MatrixXd distancesD;

MatrixXd slamRotatedCoord;
MatrixXd gpsReCoord;
MatrixXd gpsReCoordAll;
MatrixXd gpsReAverCoordAll;

/*Read the matrix slam track to slamCoordAll
    Input: the txt of slamTrack
*/
void slamTrackRead(string fileTrack)
{
    cout<<"Start To Read The Slam Track: " << fileTrack <<endl;

    int sumTrack = 0;
    int numTrack = 0;
    int numLine = 0;
    int rows = 0;
    int cols = 3;

    double **mtTemp;

    ifstream ifsTemp(fileTrack.c_str());
    ifstream ifs(fileTrack.c_str());
    string readTrackTemp;
    string readTrack;

    while(getline(ifsTemp,readTrackTemp))
    {
        rows++;
    }

    mtTemp = new double *[rows];
    for(int i=0;i<=rows-1;i++)
    {
        mtTemp[i] = new double[cols];
    }

    zTemp = new double[rows];
    timeTemp = new double[rows];
    while(getline(ifs,readTrack))
    {
        istringstream getTrack(readTrack);
        string getTrackData;

        while(getline(getTrack,getTrackData,' '))
        {
            if(numTrack==0)
            {
                mtTemp[numLine][0] = atof(getTrackData.c_str());
            }
            else if(numTrack==1)
            {
                mtTemp[numLine][1] = atof(getTrackData.c_str());
            }
            else if(numTrack==2)
            {
                mtTemp[numLine][2] = atof(getTrackData.c_str());
                zTemp[numLine] = atof(getTrackData.c_str());
            }
            else
            {
                timeTemp[numLine] = atof(getTrackData.c_str());
                numTrack = 0;
                continue;
            }
            numTrack++;
        }
        numLine++;
        numPoint++;
    }

    slamCoordAll.setOnes(numLine, 4);

    for(int i= 0; i< numLine; i++)
    {
        for(int j= 0; j< 3; j++)
        {
            slamCoordAll(i,j) = mtTemp[i][j];
        }
    }

    for(int i=0;i<=rows-1;i++)
    {
        delete[] mtTemp[i];
    }
    delete mtTemp;
}

/*Read the matrix GPS track
    Input: the txt of GPS
*/
void gpsTrackRead(string fileTrack)
{
    cout<<"Start To Read The GPS Track: " << fileTrack <<endl;

    int sumTrack = 0;
    int numTrack = 0;
    int numLine = 0;
    int rows = 0;
    int cols = 3;

    double **mtTemp;

    ifstream ifsTemp(fileTrack.c_str());
    ifstream ifs(fileTrack.c_str());
    string readTrackTemp;
    string readTrack;

    while(getline(ifsTemp,readTrackTemp))
    {
        rows++;
    }

    mtTemp = new double *[rows];
    for(int i=0;i<=rows-1;i++)
    {
        mtTemp[i] = new double[cols];
    }

    gpsTimeTemp = new double[rows];
    while(getline(ifs,readTrack))
    {
        istringstream getTrack(readTrack);
        string getTrackData;

        while(getline(getTrack, getTrackData,' '))
        {
            if(numTrack==0)
            {
               mtTemp[numLine][numTrack] = atof(getTrackData.c_str());
            }
            else if(numTrack==1)
            {
               mtTemp[numLine][numTrack] = atof(getTrackData.c_str());
            }
            else if(numTrack==2)
            {
               mtTemp[numLine][numTrack] = atof(getTrackData.c_str());
            }
            else
            {
               gpsTimeTemp[numLine] = atof(getTrackData.c_str());
               numTrack = 0;
               continue;
            }
            sumTrack++;
            numTrack++;
        }
        numLine++;
    }

    gpsCoordAll.setOnes(numLine,4);

    for(int i= 0;i< numLine; i++)
    {
        for(int j= 0; j< 4; j++)
        {
            if(i==0)
            {
               if(j==0)
               {
                  x0GPS = mtTemp[i][j];
                  mtTemp[i][j] = 0.0;
                  gpsCoordAll(i,j) = 0.0;
               }
               else if(j==1)
               {
                  y0GPS = mtTemp[i][j];
                  mtTemp[i][j] = 0.0;
                  gpsCoordAll(i,j) = 0.0;
               }
               else
               {
                  gpsCoordAll(i,j) = mtTemp[i][j];
               }
            }
            else
            {
               if(j==0)
               {
                  gpsCoordAll(i,j) = mtTemp[i][j] - x0GPS;
               }
               else if(j==1)
               {
                  gpsCoordAll(i,j) = mtTemp[i][j] - y0GPS;
               }
               else
               {
                  gpsCoordAll(i,j) = mtTemp[i][j];
               }
            }
        }
    }

    for(int i=0;i<=rows-1;i++)
    {
        delete [] mtTemp[i];
    }
    delete mtTemp;
}

/*Read the weightCoe from WeightCoeFile
    Input: the txt of WeightCoeFile
*/
void weightCoeRead(string fileWC)
{
    cout<<"Start To Read The fileWeightCore : " << fileWC <<endl;

    int numLine = 0;
    int rows = 0;

    ifstream ifsTemp(fileWC.c_str());
    ifstream ifs(fileWC.c_str());
    string readWeightCoeTemp;
    string readWeightCoe;

    while(getline(ifsTemp,readWeightCoeTemp))
    {
	rows++;
    }

    arrayWCTemp = new double[rows];
    while(getline(ifs,readWeightCoe))
    {
        arrayWCTemp[numLine] = atof(readWeightCoe.c_str());
        numLine++;
    }
}

/*Find the nearest (Euclidean) neighbor in dst for each point in src
    Input:
        src: Nx3 array of points
        dst: Nx3 array of points
    Output:
        distances: Euclidean distances of the nearest neighbor
        indices: dst indices of the right neighbor
*/
void nearestNeighbor(MatrixXd inputSrc,MatrixXd inputDst,MatrixXd *outputDistances,MatrixXd *outputIndices)
{
    //the matrix of inputSrc's rows;
    int srcLength = inputSrc.rows();

    //count
    int jNum = 0;

    //N*1,index:0~N
    MatrixXd tempIndices;
    tempIndices.setOnes(srcLength,1);

    //N*N,euclidean distance
    MatrixXd tempDistances;
    tempDistances.setOnes(srcLength,srcLength);

    //N*1,euclidean distances of the nearest neighbor
    MatrixXd tempDst;
    tempDst.setOnes(srcLength,1);

    for(int i= 0; i< srcLength; i++)
    {
        tempIndices(i,0)= i;
    }

    *outputIndices = tempIndices;

    for(int i=0;i<srcLength;i++)
    {
        for(int j=0;j<srcLength;j++)
        {
            tempDistances(i,j) = sqrt((inputSrc(i,0)-inputDst(j,0))*(inputSrc(i,0)-inputDst(j,0)) + (inputSrc(i,1)-inputDst(j,1))*(inputSrc(i,1)-inputDst(j,1)));
        }
    }

    for(int i=0;i<srcLength;i++)
    {
        tempDst(i,0) = tempDistances(i,jNum);
        jNum++;
    }
    *outputDistances = tempDst;
}

/*Calculates the least-squares best-fit transform between corresponding 3D points A->B
    Input:
        A: N*3 the matrix of corresponding 3D points
        B: Nx3 the matrix of corresponding 3D points
    Returns:
        T: 4x4 homogeneous transformation matrix
        R: 3x3 rotation matrix
        t: 3x1 column vector
*/
void BFTWithWeight(MatrixXd A,MatrixXd B,MatrixXd *outputT)
{
    int ALength= A.rows();
    int BLength= B.rows();

    double sumA_X = 0.0;
    double sumA_Y = 0.0;
    double sumA_Z = 0.0;

    double sumB_X = 0.0;
    double sumB_Y = 0.0;
    double sumB_Z = 0.0;

    double sumW = 0.0;

    //N*3,translate points to their centroids
    MatrixXd centroid_A;
    MatrixXd centroid_B;

    //N*3
    MatrixXd A1;
    MatrixXd B1;

    //N*3
    MatrixXd AA;
    MatrixXd BB;

    //N*3
    MatrixXd tempA;
    MatrixXd tempB;

    //1*3
    MatrixXd tempAA;
    MatrixXd tempBB;

    A1.setOnes(ALength,3);
    B1.setOnes(BLength,3);

    centroid_A.setOnes(ALength, 3);
    centroid_B.setOnes(BLength, 3);

    AA.setOnes(ALength, 3);
    BB.setOnes(BLength, 3);

    tempA.setOnes(ALength, 3);
    tempB.setOnes(BLength, 3);

    tempAA.setOnes(1, 3);
    tempBB.setOnes(1, 3);

    for(int i=0;i<ALength;i++)
    {
        for(int j=0;j<3;j++)
        {
            if(j==0)
            {
                A1(i,j) = A(i,j) * arrayWCTemp[i];
                B1(i,j) = B(i,j) * arrayWCTemp[i];
            }
            else if(j==1)
            {
                A1(i,j) = A(i,j) * arrayWCTemp[i];
                B1(i,j) = B(i,j) * arrayWCTemp[i];
            }
            else
            {
                A1(i,j) = A(i,j) * arrayWCTemp[i];
                B1(i,j) = B(i,j) * arrayWCTemp[i];
            }
        }
    }

    for(int i=0;i<ALength;i++)
    {
        for(int j=0;j<3;j++)
        {
            if(j==0)
            {
                sumA_X += A1(i,j);
                sumB_X += B1(i,j);
            }
            else if(j==1)
            {
                sumA_Y += A1(i,j);
                sumB_Y += B1(i,j);
            }
            else
            {
                sumA_Z += A1(i,j);
                sumB_Z += B1(i,j);
            }
        }
        sumW += arrayWCTemp[i];
    }

    for(int i=0;i<ALength;i++)
    {
        for(int j=0;j<3;j++)
        {
            tempA(i,j) = A(i,j);
            tempB(i,j) = B(i,j);
        }
    }

    sumA_X = sumA_X/sumW;
    sumA_Y = sumA_Y/sumW;
    sumA_Z = sumA_Z/sumW;

    sumB_X = sumB_X/sumW;
    sumB_Y = sumB_Y/sumW;
    sumB_Z = sumB_Z/sumW;

    for(int i=0;i<3;i++)
    {
        if(i==0)
        {
            tempAA(0,i) = sumA_X;
            tempBB(0,i) = sumB_X;
        }
        else if(i==1)
        {
            tempAA(0,i) = sumA_Y;
            tempBB(0,i) = sumB_Y;
         }
        else
        {
            tempAA(0,i) = sumA_Z;
            tempBB(0,i) = sumB_Z;
        }
    }

    for(int i=0;i<ALength;i++)
    {
        centroid_A(i,0) = sumA_X;
        centroid_A(i,1) = sumA_Y;
        centroid_A(i,2) = sumA_Z;

        centroid_B(i,0) = sumB_X;
        centroid_B(i,1) = sumB_Y;
        centroid_B(i,2) = sumB_Z;
    }

    //translate points to their centroids
    AA= tempA- centroid_A;
    BB= tempB- centroid_B;

    for(int i=0;i<ALength;i++)
    {
        for(int j=0;j<3;j++)
        {
            if(j==0)
            {
                 AA(i,j) = AA(i,j) * arrayWCTemp[i];
                 BB(i,j) = BB(i,j) * arrayWCTemp[i];
            }
            else if(j==1)
            {
                 AA(i,j) = AA(i,j) * arrayWCTemp[i];
                 BB(i,j) = BB(i,j) * arrayWCTemp[i];
            }
            else
            {
                 AA(i,j) = AA(i,j) * arrayWCTemp[i];
		         BB(i,j) = BB(i,j) * arrayWCTemp[i];
            }
        }
    }

    //rotation matrix
    MatrixXd H= (AA.transpose())*(BB);

    JacobiSVD<Eigen::MatrixXd> svd(H,ComputeThinU | ComputeThinV );
    MatrixXd U= svd.matrixU();
    MatrixXd Vt= svd.matrixV();
    MatrixXd S= svd.singularValues();

    MatrixXd R = Vt*U.transpose();

    //special reflection case
    if(R.determinant()< 0)
    {
        for(int i= 0; i< Vt.cols(); i++)
        {
            Vt(i, 2) = (-1) * Vt(i, 2);
        }
        R = Vt* U.transpose();
    }

    //translation
    MatrixXd t = tempBB.transpose() - ((R)*(tempAA.transpose()));

    //homogeneous transformation
    MatrixXd T;
    T.setIdentity(4, 4);

    for(int i= 0; i< 3;i++)
    {
        for(int j= 0; j< 3; j++)
        {

            T(i, j)= R(i, j);
        }
        double temp = t(i,0);
        T(i,3) = temp ;
    }
    *outputT = T;
}

/*Calculates the least-squares best-fit transform between corresponding 3D points A->B
    Input:
        A: Nx3 numpy array of corresponding 3D points
        B: Nx3 numpy array of corresponding 3D points
    Returns:
        T: 4x4 homogeneous transformation matrix
        R: 3x3 rotation m)atrix
        t: 3x1 column vector
*/
void bestFitTransform(MatrixXd A,MatrixXd B,MatrixXd *outputT)
{
    int ALength= A.rows();
    int BLength= B.rows();

    double sumA_X = 0.0;
    double sumA_Y = 0.0;
    double sumA_Z = 0.0;

    double sumB_X = 0.0;
    double sumB_Y = 0.0;
    double sumB_Z = 0.0;

    //N*3,translate points to their centroids
    MatrixXd centroid_A;
    MatrixXd centroid_B;

    //N*3
    MatrixXd AA;
    MatrixXd BB;

    //N*3
    MatrixXd tempA;
    MatrixXd tempB;

    //1*3
    MatrixXd tempAA;
    MatrixXd tempBB;

    centroid_A.setOnes(ALength, 3);
    centroid_B.setOnes(BLength, 3);

    AA.setOnes(ALength, 3);
    BB.setOnes(BLength, 3);

    tempA.setOnes(ALength, 3);
    tempB.setOnes(BLength, 3);

    tempAA.setOnes(1, 3);
    tempBB.setOnes(1, 3);

    for(int i=0;i<ALength;i++)
    {
        for(int j=0;j<3;j++)
        {
            if(j==0)
            {
                sumA_X += A(i,j);
                sumB_X += B(i,j);
            }
            else if(j==1)
            {
                sumA_Y += A(i,j);
                sumB_Y += B(i,j);
            }
            else
            {
                sumA_Z += A(i,j);
                sumB_Z += B(i,j);
            }
        }
    }

    for(int i=0;i<ALength;i++)
    {
       for(int j=0;j<3;j++)
        {
            tempA(i,j) = A(i,j);
            tempB(i,j) = B(i,j);
        }
    }

    sumA_X = sumA_X/ALength;
    sumA_Y = sumA_Y/ALength;
    sumA_Z = sumA_Z/ALength;

    sumB_X = sumB_X/BLength;
    sumB_Y = sumB_Y/BLength;
    sumB_Z = sumB_Z/BLength;

    for(int i=0;i<3;i++)
    {
        if(i==0)
        {
            tempAA(0,i) = sumA_X;
            tempBB(0,i) = sumB_X;
        }
        else if(i==1)
        {
            tempAA(0,i) = sumA_Y;
            tempBB(0,i) = sumB_Y;
        }
        else
        {
            tempAA(0,i) = sumA_Z;
            tempBB(0,i) = sumB_Z;
        }
    }

    for(int i=0;i<ALength;i++)
    {
        centroid_A(i,0) = sumA_X;
        centroid_A(i,1) = sumA_Y;
        centroid_A(i,2) = sumA_Z;

        centroid_B(i,0) = sumB_X;
        centroid_B(i,1) = sumB_Y;
        centroid_B(i,2) = sumB_Z;
    }

    //translate points to their centroids
    AA= tempA- centroid_A;
    BB= tempB- centroid_B;

    //rotation matrix
    MatrixXd H= (AA.transpose())*(BB);

    JacobiSVD<Eigen::MatrixXd> svd(H,ComputeThinU | ComputeThinV );
    MatrixXd U= svd.matrixU();
    MatrixXd Vt= svd.matrixV();
    MatrixXd S= svd.singularValues();

    MatrixXd R = Vt*U.transpose();

    //special reflection case
    if(R.determinant()< 0)
    {
        for(int i= 0; i< Vt.cols(); i++)
        {
             Vt(i, 2) = (-1) * Vt(i, 2);
        }
        R = Vt* U.transpose();
    }

    //translation
    MatrixXd t = tempBB.transpose() - ((R)*(tempAA.transpose()));

    //homogeneous transformation
    MatrixXd T;
    T.setIdentity(4, 4);

    for(int i= 0; i< 3;i++)
    {
        for(int j= 0; j< 3; j++)
        {
            T(i, j)= R(i, j);
        }
        double temp = t(i,0);
        T(i,3) = temp ;
    }
    *outputT = T;
}

/*The Iterative Closet Point method
    Input:
        slamTrackAll: N*4 matrix of source 3D points
        gpsTrackAll: N*4 matrix of destination 3D points
        maxIterations: exit algorithm after maxIterations
        tolerance: convergence criteria
    Output:
        outT: final homogeneous transformation
        outD: Euclidean distances (errors) of the nearest neighbor
*/
void trackICP(MatrixXd inputA,MatrixXd inputB,MatrixXd *outT,MatrixXd *outD)
{
    int iNumStart = 0;
    int iNumEnd = inputA.rows();
    int numPointICP = iNumEnd - iNumStart;

    //get slamCoordAll's rows and cols
    int slamTrackRows = inputA.rows() ;
    int slamTrackCols = inputA.cols() ;

    //get gpsCoordAll's rows and cols
    int gpsTrackRows = inputB.rows() ;
    int gpsTrackCols = inputB.cols() ;

    //N*3 matrix of source 3D points
    MatrixXd src;
    //N*3 matrix of destination 3D point
    MatrixXd dst;

    //N*1 Euclidean distances (errors) of the nearest neighbor
    MatrixXd distances;
    MatrixXd indices;

    MatrixXd _T;

    //src:N*4,dst:N*4，use "1" to fill the matrix
    src.setOnes(slamTrackRows,slamTrackCols);
    dst.setOnes(gpsTrackRows,gpsTrackCols);
    _T.setOnes(4,4);

    //get the slam track
    for(int i=0;i<slamTrackRows;i++)
    {
       for(int j=0;j<slamTrackCols-1;j++)
       {
           src(i,j) = inputA(i,j);
       }
    }

    //get the GPS track
    for(int i=0;i<gpsTrackRows;i++)
    {
       for(int j=0;j<gpsTrackCols-1;j++)
       {
           dst(i,j) = inputB(i,j);
       }
    }

    int maxIterations = 500;

    double prevError = 0;

    for(int i = 0;i<maxIterations;i++)
    {
        //find the nearest neighbours between the current source and destination points
        nearestNeighbor(src,dst,&distances,&indices);

        //compute the transformation between the current source and nearest destination points
        if(isAddWeight)
        {
            BFTWithWeight(src,dst,&_T);
        }
        else
        {
            bestFitTransform(src,dst,&_T);
        }

        //update the current source
        src =  src * _T.transpose();

        //check error
        double meanError = 0.0;

        for(int j=0;j<distances.rows();j++)
        {
            meanError += distances(j,0);
        }
        meanError = meanError/(distances.rows());

        if(abs(prevError-meanError)<0.003)
        {
            break;
        }
        prevError = meanError;
    }

    //final homogeneous transformation
    MatrixXd tempTransformT;

    //calculate final transformation
    if(isAddWeight)
    {
        BFTWithWeight(inputA,src,&tempTransformT);
    }
    else
    {
        bestFitTransform(inputA,src,&tempTransformT);
    }

    *outT = tempTransformT;
    *outD = distances;
}

/*Regress the GPS track by the rotated slam track
    Input:
        gpsCoordInput: original GPS track
        slamRotatedCoordInput: ICP's rotated slam track
        numPoint: slamRotatedCoordInput's number
    Output:
        gpsReCoord: the regressed GPS points with the minimum variance of rotated slam track
        gpsReCoordAll: the whole regressed GPS track
        gpsReAverCoordAll: the average of whole regressed GPS track and rotated slam track
*/
void regressGpsWithSlamTrack(MatrixXd gpsCoordInput,MatrixXd slamRotatedCoordInput,int numPointInput,MatrixXd *gpsReCoord,MatrixXd *gpsReCoordAll,MatrixXd *gpsReAverCoordAll)
{
    int iNum = 0;
    int cNum = 0;
    int iCoord = 0;

    double variance;
    double averageX;
    double averageY;
    double minVar;
    double disX;
    double disY;

    MatrixXd tempGpsReCoord ;
    MatrixXd tempGpsReCoordAll;
    MatrixXd tempGpsReAverCoordAll;
    MatrixXd addToTempGpsReCoord;

    //regression
    tempGpsReCoord.setOnes(numPointInput,2);
    tempGpsReCoordAll.setOnes(numPointInput,2);
    tempGpsReAverCoordAll.setOnes(numPointInput,2);
    addToTempGpsReCoord.setOnes(numPointInput+2,2);

    while(iNum < numPointInput)
    {
        variance = 0.0;
        averageX = 0.0;
        averageY = 0.0;
        iCoord = 0;

        while(iCoord<numPointInput)
        {
            disX = slamRotatedCoordInput(iCoord,0) - slamRotatedCoordInput(iNum,0);
            disY = slamRotatedCoordInput(iCoord,1) - slamRotatedCoordInput(iNum,1);

            //for Mininum Variance calculation
            tempGpsReCoord(iCoord,0) = gpsCoordInput(iCoord,0) - disX - gpsCoordInput(iNum,0);
            tempGpsReCoord(iCoord,0) = gpsCoordInput(iCoord,1) - disY - gpsCoordInput(iNum,1);
            variance += pow((tempGpsReCoord(iCoord,0) * tempGpsReCoord(iCoord,0)+ tempGpsReCoord(iCoord,1) * tempGpsReCoord(iCoord,1)),0.5);

            //regressed GPS dots dopend on iNum Slam Track dot
            averageX += gpsCoordInput(iCoord,0) - disX;
            averageY += gpsCoordInput(iCoord,1) - disY;
            iCoord++;
        }

        tempGpsReCoordAll(iNum,0) = averageX/numPointInput;
        tempGpsReCoordAll(iNum,1) = averageY/numPointInput;

        tempGpsReAverCoordAll(iNum,0) = (tempGpsReCoordAll(iNum,0) + slamRotatedCoordInput(iNum,0))/2.0;
        tempGpsReAverCoordAll(iNum,1) = (tempGpsReCoordAll(iNum,1) + slamRotatedCoordInput(iNum,1))/2.0;

        if(iNum == 0)
        {
            minVar = variance;
        }

        if(variance < minVar)
        {
            minVar = variance;
            cNum = iNum;
        }
        iNum++;
    }

    //for Minimum Variance calculation-average coordinate calculation
    averageX = 0.0;
    averageY = 0.0;
    iCoord = 0;
    while(iCoord < numPointInput)
    {
        disX = slamRotatedCoordInput(iCoord,0) - slamRotatedCoordInput(cNum,0);
        disY = slamRotatedCoordInput(iCoord,1) - slamRotatedCoordInput(cNum,1);

        tempGpsReCoord(iCoord,0) = gpsCoordInput(iCoord,0) - disX;
        tempGpsReCoord(iCoord,1) = gpsCoordInput(iCoord,1) - disY;

        averageX += tempGpsReCoord(iCoord,0);
        averageY += tempGpsReCoord(iCoord,1);
        iCoord++;
    }

    averageX/=numPointInput;
    averageY/=numPointInput;

    //vstack
    for(int i=0;i<tempGpsReCoord.rows();i++)
    {
        for(int j=0;j<2;j++)
        {
             addToTempGpsReCoord(i,j) = tempGpsReCoord(i,j);
        }
    }

    for(int i=0;i<2;i++)
    {
        addToTempGpsReCoord(tempGpsReCoord.rows(),i) = slamRotatedCoordInput(cNum,i);
    }

    addToTempGpsReCoord(((tempGpsReCoord.rows())+1),0) = averageX;
    addToTempGpsReCoord(((tempGpsReCoord.rows())+1),1) = averageY;

    *gpsReCoord = addToTempGpsReCoord;
    *gpsReCoordAll = tempGpsReCoordAll;
    *gpsReAverCoordAll = tempGpsReAverCoordAll;
}

/*Output the ICPTrack to txt*/
void outPutXYZT(string inputFile)
{
    ofstream doneICPTrackWrite;
    doneICPTrackWrite.open(inputFile.c_str(),ios::out);

    for(int i=0;i<gpsReAverCoordAll.rows();i++)
    {
        double xTemp = gpsReAverCoordAll(i,0) + x0GPS;
        double yTemp = gpsReAverCoordAll(i,1) + y0GPS;
        doneICPTrackWrite.precision(IMDP);
        if(isAddWeight==true)
        {
	    doneICPTrackWrite<<xTemp<<" "<<yTemp<<" "<<zTemp[i]<<" "<<timeTemp[i]<<" "<<arrayWCTemp[i]<<endl;
        }
        else
        {
            doneICPTrackWrite<<xTemp<<" "<<yTemp<<" "<<zTemp[i]<<" "<<timeTemp[i]<<endl;
        }

    }
    doneICPTrackWrite.close();
}

/*Output the doneCutOffICPTrack to txt*/
void outPutWithCutOff(string inputFile)
{
    ofstream coICPTrackWrite;
    coICPTrackWrite.open(inputFile.c_str(),ios::out);

    for(int i=0;i<slamRotatedCoord.rows();i++)
    {
	    coICPTrackWrite.precision(IMDP);
        coICPTrackWrite<<slamRotatedCoord(i,0)<<" "<<slamRotatedCoord(i,1)<<" "<<slamRotatedCoord(i,2)<<" "<<gpsTimeTemp[i]<<endl;
    }
}

/*Cut off the slamTrack*/
void doCutOff()
{
    for(int i=0;i<numPoint;i++)
    {
	if(timeTemp[i]==gpsTimeTemp[0])
        {
            breakPointNum = i;
            break;
        }
    }

    int breakNum = slamCoordAll.rows() - breakPointNum;

    if(breakNum>=gpsCoordAll.rows())
    {
        aCoord.setOnes(gpsCoordAll.rows(),4);
        bCoord.setOnes(gpsCoordAll.rows(),4);

        for(int i=0;i<aCoord.rows();i++)
        {
            for(int j=0;j<aCoord.cols()-1;j++)
            {
                aCoord(i,j) = slamCoordAll(breakPointNum+i,j);
                bCoord(i,j) = gpsCoordAll(i,j);
            }
        }
    }
    else
    {
        aCoord.setOnes(breakNum,4);
        bCoord.setOnes(breakNum,4);

        for(int i=0;i<aCoord.rows();i++)
        {
            for(int j=0;j<aCoord.cols()-1;j++)
            {
                 aCoord(i,j) = slamCoordAll(breakPointNum+i,j);
                 bCoord(i,j) = gpsCoordAll(i,j);
            }
        }
    }
}

/*Get the rotation of slamTrack*/
void getSlamRotatedCoord(MatrixXd inputMatrix)
{
    //N*3
    MatrixXd tempSlamCoordAll;
    MatrixXd rTransformT;
    MatrixXd tTransformT;

    tempSlamCoordAll.setOnes(inputMatrix.rows(),3);
    rTransformT.setOnes(3,3);
    tTransformT.setOnes(inputMatrix.rows(),3);

    //Get t from transformT
    for(int i=0;i<inputMatrix.rows();i++)
    {
        for(int j=0;j<3;j++)
        {
            tempSlamCoordAll(i,j) = inputMatrix(i,j);
            tTransformT(i,j) = transformT(j,3);
        }
    }

    //Get r from transformT
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            rTransformT(i,j) = transformT(i,j);
        }
    }

    //Get the rotation of slam track
    slamRotatedCoord = tempSlamCoordAll*(rTransformT.transpose()) + tTransformT;
}

/*Run cutOffICP*/
void cutOffICPRun()
{
    cout<<"start Cutoff ICP"<<endl;
    trackICP(bCoord,aCoord,&transformT,&distancesD);
    cout<<"end Cutoff ICP"<<endl;

    getSlamRotatedCoord(gpsCoordAll);
    cout<<"rotated over"<<endl;

    outPutWithCutOff(fileDoneICPTrack);
    cout<<"data output over"<<endl;
}

/*Run ICP*/
void ICPRun()
{
    //Do the iterative closet points algorithm
    cout << "start ICP " << endl;
    trackICP(slamCoordAll,gpsCoordAll,&transformT, &distancesD);
    cout << "end ICP " << endl;

    getSlamRotatedCoord(slamCoordAll);

    //Regression Calculation
    regressGpsWithSlamTrack(gpsCoordAll,slamRotatedCoord,numPoint,&gpsReCoord,&gpsReCoordAll,&gpsReAverCoordAll);

    outPutXYZT(fileDoneICPTrack);
}

int main(int argc, char** argv)
{
    if(argc!=8)
    {
        printf("parameter error\n");
        return 0;
    }

    //Define variable name to save loam_track path
    fileSLAMTrack = argv[1];
    //Define variable name to save GPS_track path
    fileGPSTrack = argv[2];
    //Define variable name ,if '==1' to run ICP_with_weightCoe
    fileisWeightCoe = argv[5];
    //Define variable name to save ICP_track_result path
    fileDoneICPTrack = argv[4];

    //According to inputing to adjust to do ICP_with_weight
    if(fileisWeightCoe=="1")
    {
       //Define variable name to save Weight_Coe path
       fileWeightCoe = argv[3];
       isAddWeight = true;
    }

    cout << "slam track is: " << fileSLAMTrack << endl;
    cout << "gps track is: " << fileGPSTrack << endl;

    //Read slam track and GPS track
    slamTrackRead(fileSLAMTrack);
    gpsTrackRead(fileGPSTrack);

    //Read value of weight from fileWeightCoe
    if(isAddWeight == true)
    {
        weightCoeRead(fileWeightCoe);
    }

    /*According to whether the same time to determine whether next step.
      we use the strategy, because running times of LOAM are too long,
      the slam track is not very accurate, we have to run loam with short
      distance, and use the track of cutting off to do ICP, to make the
      result smoother
    */
    if(timeTemp[0]!=gpsTimeTemp[0])
    {
        isCutOffICP = true;
        //if here, cutting off
        doCutOff();
        //use the track of loam with short distance to ICP
        cutOffICPRun();
    }
    else
    {
        //if here,run ICP, to do ICP_with_weightCoe or not
        ICPRun();
    }

    delete[] zTemp;
    delete[] timeTemp;
    delete[] gpsTimeTemp;
    zTemp= NULL;
    timeTemp= NULL;
    gpsTimeTemp= NULL;
    if(isAddWeight == true)
    {
        delete[] arrayWCTemp;
        arrayWCTemp= NULL;
    }

    return 0;
}

max_stars_repo_name: Donnnnng/gpsCalibration

max_issues_repo_name: whydaydayup/gpsCalibration

max_forks_repo_name: whydaydayup/gpsCalibration


text:
#pragma once

#include <mtao/types.hpp>
#include <Eigen/Sparse>
#include "mandoline/cutface.hpp"

namespace mandoline {
    // BarycentricTriangleFan stores the vertices of a simple polygon
struct BarycentricTriangleFace {

    mtao::ColVecs3d barys;
    int parent_fid = -1;

    // reads off the vertex info in the format {cut-vertex index, triangle-vertex} = barycentric
    // this format is creates
    std::map<std::array<int, 2>, double> sparse_matrix_entries(const std::vector<int> &indices, const mtao::ColVecs3i &F) const;
    // we can just pass a cut-face in. Cut-faces of trianglees only have a single face so we can just extract that one
    std::map<std::array<int, 2>, double> sparse_matrix_entries(const CutFace<3> &face, const mtao::ColVecs3i &F) const {
        assert(face.is_mesh_face());
        assert(face.indices.size() == 1);
        return sparse_matrix_entries(*face.indices.begin(), F);
    }

    std::map<std::array<int, 2>, double> sparse_matrix_entries(const CutMeshFace<3> &face, const mtao::ColVecs3i &F) const {
        return sparse_matrix_entries(face.indices, F);
    }
    double volume() const;
};
}// namespace mandoline

max_stars_repo_name: mtao/mandoline

max_issues_repo_name: mtao/mandoline

max_forks_repo_name: mtao/mandoline


text:
//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <scalar_test.hpp>

namespace bs = boost::simd;

STF_CASE_TPL( "Check return type from std(ceil)", STF_NUMERIC_TYPES)
{
  using bs::ceil;
  STF_TYPE_IS(decltype(bs::std_(ceil)(T())), T);
}

STF_CASE_TPL( "Check std(ceil) behavior on IEEE types",  STF_IEEE_TYPES)
{
  using bs::ceil;

  STF_EQUAL(bs::std_(ceil)(T(2.5)), T(3));
  STF_EQUAL(bs::std_(ceil)(T(1.6)), T(2));
  STF_EQUAL(bs::std_(ceil)(T(1.5)), T(2));
  STF_EQUAL(bs::std_(ceil)(T(1.4)), T(2));
  STF_EQUAL(bs::std_(ceil)(T(0)), T(0));
  STF_EQUAL(bs::std_(ceil)(T(-1.4)), T(-1));
  STF_EQUAL(bs::std_(ceil)(T(-1.5)), T(-1));
  STF_EQUAL(bs::std_(ceil)(T(-1.6)), T(-1));
  STF_EQUAL(bs::std_(ceil)(T(-2.5)), T(-2));
}

STF_CASE_TPL( "Check std(ceil) behavior on limit cases", STF_NUMERIC_TYPES)
{
  using bs::ceil;
  using bs::Valmin;
  using bs::Valmax;

  STF_EQUAL(bs::std_(ceil)(Valmin<T>()), Valmin<T>());
  STF_EQUAL(bs::std_(ceil)(Valmax<T>()), Valmax<T>());
}

STF_CASE_TPL( "Check std(ceil) behavior on IEEE limit cases", STF_IEEE_TYPES)
{
  using bs::ceil;
  using bs::Valmin;
  using bs::Valmax;
  using bs::Inf;
  using bs::Minf;
  using bs::Nan;
  using bs::Maxflint;

  STF_EQUAL(bs::std_(ceil)(Inf<T>()) , Inf<T>());
  STF_EQUAL(bs::std_(ceil)(Minf<T>()), Minf<T>());
  STF_IEEE_EQUAL(bs::std_(ceil)(Nan<T>()) , Nan<T>());
  STF_EQUAL(bs::std_(ceil)(Maxflint<T>()+1) , Maxflint<T>()+1);
  STF_EQUAL(bs::std_(ceil)(Maxflint<T>()  ) , Maxflint<T>()  );
  STF_EQUAL(bs::std_(ceil)(Maxflint<T>()-1) , Maxflint<T>()-1);
}

STF_CASE_TPL( "Check std(ceil) behavior on integral types",  STF_INTEGRAL_TYPES)
{
  using bs::ceil;

  STF_EQUAL(bs::std_(ceil)(T(2)), T(2));
  STF_EQUAL(bs::std_(ceil)(T(1)), T(1));
  STF_EQUAL(bs::std_(ceil)(T(0)), T(0));
  STF_EQUAL(bs::std_(ceil)(T(-1)), T(-1));
  STF_EQUAL(bs::std_(ceil)(T(-2)), T(-2));
}