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

#include <boost/units/detail/utility.hpp>
#include <sstream>

#include "vector_feature_space.h"

#include "belief_feature_value.h"
#include "simple_feature_value.h"

namespace pomdp
{
/** Create from FeatureVector file. Will be empty (getNumStates() == 0) on failure. */
VectorFeatureSpace::VectorFeatureSpace(const char* path)
{
  FeatureVectorFactory factory;
  feature_vector_ = factory.loadFromFile(path);
  if(feature_vector_.size() > 0)
  //  if(feature_vector_ != FeatureVector::empty) // Success
    is_init_ = true;
}

bool VectorFeatureSpace::loadFromYAML(const YAML::Node& feature_space_root)
{
  FeatureVectorFactory fact;
  feature_vector_ = fact.loadFromYAML(feature_space_root);
  is_init_ = feature_vector_ != FeatureVector::empty;
  return is_init_;
}

bool VectorFeatureSpace::isValid(const FeatureValue& state) const
{
  if(typeid(state) == typeid(SimpleFeatureValue))
    return isValid(dynamic_cast<const SimpleFeatureValue&>(state));
  else if(typeid(state) == typeid(BeliefFeatureValue))
    return isValid(dynamic_cast<const BeliefFeatureValue&>(state));
  else if(typeid(state) == typeid(FeatureVectorValue))
    return isValid(dynamic_cast<const FeatureVectorValue&>(state));
  else
    return false;
}

bool VectorFeatureSpace::isValid(const SimpleFeatureValue& state) const
{
  return state.asInt() < getNumValues();
}

bool VectorFeatureSpace::isValid(const BeliefFeatureValue& belief) const
{
  return belief.asVector().size() == getNumValues();
}

bool VectorFeatureSpace::isValid(const FeatureVectorValue& state) const
{
  return feature_vector_.validValue(state);
}

FeatureValue VectorFeatureSpace::hash(const FeatureValue& state) const
{
  if(typeid(state) == typeid(SimpleFeatureValue))
    return hash(dynamic_cast<const SimpleFeatureValue&>(state));
  else if(typeid(state) == typeid(BeliefFeatureValue))
    return hash(dynamic_cast<const BeliefFeatureValue&>(state));
  else //if(typeid(state) == typeid(FeatureVectorValue))
    // @todo Make this not hideous
    return hash(dynamic_cast<const FeatureVectorValue&>(state));
}

SimpleFeatureValue VectorFeatureSpace::hash(const SimpleFeatureValue& state) const
{
  return SimpleFeatureValue(state);
}

BeliefFeatureValue VectorFeatureSpace::hash(const BeliefFeatureValue& belief) const
{
  throw std::runtime_error("VectorFeatureSpaces do not yet support hashing of BeliefStates.");
}

SimpleFeatureValue VectorFeatureSpace::hash(const FeatureVectorValue& state) const
{
  return SimpleFeatureValue(feature_vector_.toStateNumber(state));
}

FeatureValue VectorFeatureSpace::dehash(const FeatureValue& state) const
{
  if(typeid(state) == typeid(SimpleFeatureValue))
    return dehash(dynamic_cast<const SimpleFeatureValue&>(state));
  else if(typeid(state) == typeid(BeliefFeatureValue))
    return dehash(dynamic_cast<const BeliefFeatureValue&>(state));
  else //if(typeid(state) == typeid(FeatureVectorValue))
    // @todo Make this not hideous
    return dehash(dynamic_cast<const FeatureVectorValue&>(state));
}

FeatureVectorValue VectorFeatureSpace::dehash(const SimpleFeatureValue& state) const
{
  return feature_vector_.toFeatureVectorValue(state.asInt());
}

BeliefFeatureValue VectorFeatureSpace::dehash(const BeliefFeatureValue& belief) const
{
  throw std::runtime_error("VectorFeatureSpaces do not yet support dehashing of BeliefStates.");
}

FeatureVectorValue VectorFeatureSpace::dehash(const FeatureVectorValue& state) const
{
  return state;
}

std::string VectorFeatureSpace::toString() const
{
  std::stringstream ret;
  ret << "VectorStateSpace, size " << getNumValues() << ", has " << feature_vector_;
  return std::string(ret.str());
}

// @todo Refactor to FeatureVectorValue or FeatureVector
std::string VectorFeatureSpace::toString(const FeatureValue& state) const
{
  std::stringstream ret;
  ret << boost::units::detail::demangle(typeid(state).name());
  if(typeid(state) == typeid(SimpleFeatureValue))
    ret << ": " << toString(dynamic_cast<const SimpleFeatureValue&>(state));
  else if(typeid(state) == typeid(BeliefFeatureValue))
    ret << ": " << toString(dynamic_cast<const BeliefFeatureValue&>(state));
  else if(typeid(state) == typeid(FeatureVectorValue))
    ret << ": " << toString(dynamic_cast<const FeatureVectorValue&>(state));

  return ret.str();
}

std::string VectorFeatureSpace::toString(const SimpleFeatureValue& state) const
{
  return toString(feature_vector_.toFeatureVectorValue(state.asInt()));
}

/**
 * Same as SimpleStateSpace::toString(const BeliefState&). Need to find a better way.
 */
std::string VectorFeatureSpace::toString(const BeliefFeatureValue& belief) const
{
  return Pretty::vectorToString(belief.asVector());
}

std::string VectorFeatureSpace::toString(const FeatureVectorValue& state) const
{
  std::stringstream ret;

  // Print each Feature's value.
  ret << "[ ";
  for(int i = 0; i < feature_vector_.size(); i++)
  {
    Feature f = feature_vector_[i];
    ret << f.name_ << ": " << f.state_name_mapping_->toString(state[i]) << ", ";
  }
  ret << "]";

  return ret.str();
}

/**
 * Reads a feature vector value from a string.
 *
 * Values must be either a name (string) contained in this state space (e.g. "go_left") or a number within range
 * (e.g. 4). String elements should be delimited by spaces. @todo Or commas.
 *
 * @param str String representation of the value.
 * @return A FeatureVectorValue. Value is invalid on failure.
 */
shared_ptr<FeatureValue> VectorFeatureSpace::readFeatureValue(const std::string& str) const
{
  const std::vector<unsigned int> INVALID_VALUE;

  std::vector<unsigned int> feature_values;
  std::stringstream str_ss(str);
  std::string word;
  for(int i = 0; i < feature_vector_.size(); i++)
  {
    if(!(str_ss >> word))
    {
      ROS_ERROR_STREAM("Insufficient values in line '" << str << "' to read a FeatureVectorValue in space " << *this);
      return boost::make_shared<FeatureVectorValue>(INVALID_VALUE);
    }

    int value = Pretty::readNameOrNum(str, feature_vector_[i].state_name_mapping_);
    if(value == -1)
    {
      ROS_ERROR_STREAM("Invalid value '" << word << "'.");
      return boost::make_shared<FeatureVectorValue>(INVALID_VALUE);
    }
    else
      feature_values.push_back(value);
  }

  return boost::make_shared<FeatureVectorValue>(feature_values); // Doesn't check for validity - that will be done when isValid is called.
}

}
/* namespace pomdp */


text:
/*!
 * @file
 * Basic tests for `Map.hpp`
 *
 * This file is a part of easyLambda(ezl) project for parallel data
 * processing with modern C++ and MPI.
 *
 * @copyright Utkarsh Bhardwaj <haptork@gmail.com> 2015-2016
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying LICENSE.md or copy at * http://boost.org/LICENSE_1_0.txt)
 * */

#include <tuple>
#include <type_traits>
#include <assert.h>

#include <boost/mpi.hpp>

#include <ezl/mapreduce/Map.hpp>
#include <ezl/helper/meta/typeInfo.hpp>

#include <ctorTeller.hpp>

using namespace ezl::detail;

auto f2() {  // used as UDF in a test
  return 0;
}

void MapBasicCallTest();
void MapCopyPerformanceTest();
void MapReturnPerformanceTest();

void MapTest(int argc, char* argv[]) {
  boost::mpi::environment env(argc, argv);
  boost::mpi::communicator comm;
  MapBasicCallTest();
  MapCopyPerformanceTest();
  MapReturnPerformanceTest();
}

void MapReturnPerformanceTest() {
  using std::tuple;
  using std::make_tuple;
  using meta::slct;
  using std::vector;
  using ezl::detail::meta::MapTypes;

  auto f = [](const std::tuple<const ctorTeller&>& ) { return 3; };
  auto rec = std::make_shared<Map<MapTypes<tuple<const ctorTeller&>, slct<1>, decltype(f), slct<2>>>>(f);

  auto vecTupRet = [](const int& ) {
    vector<tuple<ctorTeller>> v;
    v.push_back(std::move(std::tuple<ctorTeller>{ctorTeller("Map-Return", true, false)}));
    assert(ctorTeller::movector() == 2);
    assert(ctorTeller::copyctor() == 0);
    return v;
  };
  auto vecTupRetMap = std::make_shared<Map<MapTypes<tuple<int>, slct<1>, decltype(vecTupRet), slct<2>>>>(vecTupRet);
  vecTupRetMap->next(rec, vecTupRetMap);

  auto tupRet = [](const int& ) {
    std::tuple<ctorTeller> t {ctorTeller("Map-Return", true, false)};
    assert(ctorTeller::movector() == 1);
    assert(ctorTeller::copyctor() == 0);
    return t;
  };
  auto tupRetMap = std::make_shared<Map<MapTypes<tuple<int>, slct<1>, decltype(tupRet), slct<2>>>>(tupRet);
  tupRetMap->next(rec, tupRetMap);

  auto plainRet = [](const int& ) {
    return ctorTeller{"Map-Return", true, false};
  };
  auto plainRetMap =
      std::make_shared<Map<MapTypes<tuple<int>, slct<1>, decltype(plainRet), slct<2>>>>(plainRet);
  //plainRetMap->next(rec, plainRetMap);

  auto vecPlainRet = [](const int& ) {
    vector<ctorTeller> v;
    v.emplace_back("Map-Return", true, false);
    assert(ctorTeller::movector() == 0);
    assert(ctorTeller::copyctor() == 0);
    return v;
  };
  auto vecPlainRetMap = std::make_shared<Map<MapTypes<tuple<int>, slct<1>, decltype(vecPlainRet), slct<2>>>>(vecPlainRet);
  vecPlainRetMap->next(rec, vecPlainRetMap);

  vecTupRetMap->dataEvent(std::make_tuple(1));
  //ctorTeller::display();
  assert(ctorTeller::movector() == 2);  // same as UDF
  assert(ctorTeller::copyctor() == 0);

  tupRetMap->dataEvent(std::make_tuple(1));
  //ctorTeller::display();
  assert(ctorTeller::movector() == 1);  // same as UDF
  assert(ctorTeller::copyctor() == 0);

  plainRetMap->dataEvent(std::make_tuple(1));
  //ctorTeller::display();
  assert(ctorTeller::movector() == 0);
  assert(ctorTeller::copyctor() == 0);

  vecPlainRetMap->dataEvent(std::make_tuple(1));
  //ctorTeller::display();
  assert(ctorTeller::movector() == 0);
  assert(ctorTeller::copyctor() == 0);
/*
  auto vecRet3 = [](const int& ) {
    vector<tuple<ctorTeller>> v;
    v.emplace_back(ctorTeller{"Map-Return", true, true});
    assert(ctorTeller::movector() == 1);
    assert(ctorTeller::copyctor() == 0);
    assert(ctorTeller::ctor() == 1);
    return v;
  };

  auto vecRet2 = [](const int& ) {
    vector<tuple<ctorTeller>> v;
    v.push_back(std::tuple<ctorTeller>{ctorTeller("Map-Return", true, true)});
    ctorTeller::display();
    return v;
  };
*/
}

void MapBasicCallTest() {
  using std::make_tuple;
  using meta::slct;
  using std::tuple;
  using ezl::detail::meta::MapTypes;

  auto t1 = make_tuple(3, 'c', 4.);
  char r1;
  auto f1 = [&r1](char ch) {
    r1 = ch;
    return false;
  };
  Map<MapTypes<decltype(t1), slct<2>, decltype(f1), slct<1,4>>> m1{f1};
  m1.dataEvent(t1);
  assert(r1 == 'c');

  Map<MapTypes<std::tuple<>, slct<>, decltype(&f2), slct<1>>> m2{f2};
  m2.dataEvent(std::tuple<>{});
}

void MapCopyPerformanceTest() {
  using std::make_tuple;
  using meta::slct;
  using std::tuple;
  using ezl::detail::meta::MapTypes;

  auto t2 = make_tuple(ctorTeller("Map: 1", true, false), 1, 'c');
  assert(ctorTeller::movector() == 1);
  assert(ctorTeller::copyctor() == 0);
  auto g1 = [](ctorTeller, int ) { return 0; };
  auto g2 = [](std::tuple<ctorTeller, int> ) { return 0; };
  auto g3 = [](const std::tuple<ctorTeller, int>& ) { return 0; };
  auto g4 = [](const std::tuple<const ctorTeller&, const int&>& ) { return 0; };
  auto g5 = [](const ctorTeller&, const int& ) { return 0; };

  auto n1 = std::make_shared<Map<MapTypes<decltype(t2), slct<1, 2>, decltype(g1), slct<3, 1>>>>(g1);
  auto n2 = std::make_shared<Map<MapTypes<decltype(t2), slct<1, 2>, decltype(g2), slct<3, 1>>>>(g2);
  auto n3 = std::make_shared<Map<MapTypes<decltype(t2), slct<1, 2>, decltype(g3), slct<3, 1>>>>(g3);
  auto n4 = std::make_shared<Map<MapTypes<decltype(t2), slct<1, 2>, decltype(g4), slct<3, 1>>>>(g4);
  auto n5 = std::make_shared<Map<MapTypes<decltype(t2), slct<1, 2>, decltype(g5), slct<3, 1>>>>(g5);

  n1->dataEvent(t2); // one copy
  n2->dataEvent(t2); // one copy
  n3->dataEvent(t2); // one copy
  assert(ctorTeller::copyctor() == 3);
  assert(ctorTeller::movector() == 1);

  auto t3 = make_tuple(ctorTeller("Map: 2", true, false), 3);

  assert(ctorTeller::copyctor() == 0);
  assert(ctorTeller::movector() == 1);
  n4->dataEvent(t2); // no copy
  n5->dataEvent(t2); // no copy
  assert(ctorTeller::copyctor() == 0);
  assert(ctorTeller::movector() == 1);

  auto h1 = [](const std::tuple<const ctorTeller&, const int&>& ) { return 3; };
  auto h3 = [](const int& ) { return 4; };

  auto x1 = std::make_shared<Map<MapTypes<decltype(t3), slct<1, 2>, decltype(h1), slct<3>>>>(h1);
  auto y1 = std::make_shared<Map<MapTypes<tuple<const int&>, slct<1>, decltype(h3), slct<2>>>>(h3);

  x1->next(y1, x1);
  x1->dataEvent(t3); // no copy
  assert(ctorTeller::copyctor() == 0);
  assert(ctorTeller::movector() == 1);

  using tt = tuple<const ctorTeller&, const int&>;
  auto h2 = [](const ctorTeller&, const int& ) { return 4; };

  auto x2 = std::make_shared<Map<MapTypes<decltype(t3), slct<1, 2>, decltype(h2), slct<1,2>>>>(h2);
  auto y2 = std::make_shared<Map<MapTypes<tt, slct<1,2>, decltype(h2), slct<1,2>>>>(h2);
  x2->next(y2, x2);
  x2->dataEvent(t3); // no copy
  assert(ctorTeller::copyctor() == 0);
  assert(ctorTeller::movector() == 1);
}

ext: cpp

max_stars_repo_name: YcheParallelStudio/easyLambda

max_issues_repo_name: YcheParallelStudio/easyLambda

max_forks_repo_name: YcheParallelStudio/easyLambda


text:
#include <boost/python.hpp>
#include <boost/test/unit_test.hpp>

#include <ackward/logging/FileHandler.hpp>
#include <ackward/logging/Handler.hpp>
#include <ackward/logging/Logger.hpp>
#include <ackward/logging/NullHandler.hpp>
#include <ackward/logging/handlers/SocketHandler.hpp>
#include <ackward/logging/handlers/StreamHandler.hpp>
#include <ackward/logging/handlers/WatchedFileHandler.hpp>

using namespace ackward::logging;
using namespace ackward::logging::handlers;
using namespace boost::python;

BOOST_AUTO_TEST_SUITE( logging )
BOOST_AUTO_TEST_SUITE( handler )

// Handler(boost::python::object);
// void setLevel(Level l);
// void setFormatter(Formatter f);
// void addFilter(Filter f);
// void removeFilter(Filter f);
// void flush() const;
// void close();

BOOST_AUTO_TEST_CASE( StreamHandler_test )
{
    StreamHandler h;
    StreamHandler h2(import("sys").attr("stdout"));
}

BOOST_AUTO_TEST_CASE( FileHandler_test )
{
    FileHandler(L"delete_me.log");
    FileHandler(L"delete_me2.log",
                "w");
    FileHandler(L"delete_me3.log",
                "a",
                "utf8");
    FileHandler(L"delete_me3.log",
                "a",
                "utf8",
                true);
}

BOOST_AUTO_TEST_CASE( WatchedFileHandler_test )
{
    WatchedFileHandler(L"delete_me.log");
    WatchedFileHandler(L"delete_me2.log",
                       "w");
    WatchedFileHandler(L"delete_me3.log",
                       "a",
                       "utf8");
    WatchedFileHandler(L"delete_me3.log",
                       "a",
                       "utf8",
                       true);
}

BOOST_AUTO_TEST_CASE( SocketHandler_test )
{
    SocketHandler h(L"localhost", 1234);
}

BOOST_AUTO_TEST_CASE( NullHandler_test )
{
    NullHandler h;
    Logger l = getLogger();
    l.addHandler(h);
    try{
        l.debug(L"Hola!");
    } catch (...)
    {
        PyErr_Print();
        throw;
    }
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: abingham/ackward

max_issues_repo_name: abingham/ackward

max_forks_repo_name: abingham/ackward


text:
//
//  PLYParser.cpp
//  Kuplung
//
//  Created by Sergey Petrov on 1/2/16.
//  Copyright © 2016 supudo.net. All rights reserved.
//

#include "PLYParser.hpp"
#include <fstream>
#include <sstream>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/algorithm/string/replace.hpp>

PLYParser::PLYParser() {
	this->Setting_Axis_Forward = 0;
	this->Setting_Axis_Up = 0;
}

PLYParser::~PLYParser() {
}

void PLYParser::init(const std::function<void(float)>& doProgress) {
    this->funcProgress = doProgress;
}

std::vector<MeshModel> PLYParser::parse(const FBEntity& file, const std::vector<std::string>& settings) {
    if (settings.size() > 0 && !settings[0].empty())
        this->Setting_Axis_Forward = std::stoi(settings[0]);
    if (settings.size() > 1 && !settings[1].empty())
        this->Setting_Axis_Up = std::stoi(settings[1]);

    std::ifstream ply_file_data(file.path.c_str());
    if (!ply_file_data.is_open()) {
        Settings::Instance()->funcDoLog("[PLYParser] Cannot open .ply file - " + file.path + "!");
        ply_file_data.close();
        return {};
    }

    std::string singleLine;
    ply_file_data.seekg(0);
    std::getline(ply_file_data, singleLine);
    if (singleLine != "ply" && singleLine != "PLY") {
        Settings::Instance()->funcDoLog("[PLYParser] File is not in PLY format - " + file.path + "!");
        ply_file_data.close();
        return {};
    }
    ply_file_data.seekg(0);

    while (std::getline(ply_file_data, singleLine)) {
        if (boost::starts_with(singleLine, "format")) {
            boost::replace_first(singleLine, "format ", "");
            if (boost::starts_with(singleLine, "ascii")) {
                this->loadAsciiFile(file);
                break;
            }
        }
    }

    ply_file_data.close();

    return this->models;
}

void PLYParser::loadAsciiFile(const FBEntity& file) {
    Settings::Instance()->funcDoLog("[PLYParser] Parsing ascii PLY file...");

    int currentModelID = 0;
    MeshModel entityModel;
    entityModel.File = file;
    entityModel.ID = currentModelID;
    entityModel.ModelTitle = file.title;
    entityModel.countVertices = 0;
    entityModel.countTextureCoordinates = 0;
    entityModel.countNormals = 0;
    entityModel.countIndices = 0;

    MeshModelMaterial entityMaterial = {};
    entityMaterial.MaterialID = 1;
    entityMaterial.MaterialTitle = "<PLY-MAT>";
    entityMaterial.DiffuseColor = glm::vec3(0.7, 0.7, 0.7);
    entityMaterial.Transparency = 1.0f;
    entityMaterial.IlluminationMode = 2;
    entityModel.ModelMaterial = entityMaterial;

    entityModel.countIndices = int(entityModel.indices.size());
    entityModel.countNormals = int(entityModel.normals.size());
    entityModel.countVertices = int(entityModel.vertices.size());

    unsigned int countVertices = 0, counterVertices = 0;
    unsigned int countFaces = 1, counterFaces = 1;
    bool inElementVertex = false;
    std::map<std::string, bool> ply_props;

    std::ifstream ply_file_data(file.path.c_str());
    std::string singleLine;
    bool headerEnded = false;
    while (std::getline(ply_file_data, singleLine)) {
        // elements
        if (boost::starts_with(singleLine, "element")) {
            boost::replace_first(singleLine, "element ", "");

            // vertices
            if (boost::starts_with(singleLine, "vertex")) {
                boost::replace_first(singleLine, "vertex ", "");
                countVertices = std::stoi(singleLine);
                inElementVertex = true;
            }

            // faces
            if (boost::starts_with(singleLine, "face")) {
                boost::replace_first(singleLine, "face ", "");
                countFaces = std::stoi(singleLine);
                inElementVertex = false;
            }

            // Face properties are not supported ... too unclear for me still.
        }

        // vertex properties
        if (boost::starts_with(singleLine, "property") && inElementVertex) {
            boost::replace_first(singleLine, "property ", "");
            std::stringstream valReader(singleLine);
            std::string prop_type, prop_value;
            valReader >> prop_type >> prop_value;
            if (prop_value == "nx")
                ply_props["normals"] = true;
            else if (prop_value == "red")
                ply_props["colors"] = true;
            else if (prop_value == "s")
                ply_props["textures"] = true;
        }

        // data
        if (headerEnded) {
            if (counterVertices < countVertices) {
                std::stringstream valReader(singleLine);

                glm::vec3 vertex = glm::vec3(0.0);
                valReader >> vertex.x >> vertex.y >> vertex.z;
                entityModel.vertices.push_back(vertex);

                if (ply_props["normals"]) {
                    glm::vec3 normal = glm::vec3(0.0);
                    valReader >> normal.x >> normal.y >> normal.z;
                    entityModel.normals.push_back(normal);
                }

                if (ply_props["textures"]) {
                    glm::vec2 tc = glm::vec2(0.0);
                    valReader >> tc.s >> tc.t;
                    entityModel.texture_coordinates.push_back(tc);
                }

                if (ply_props["colors"]) {
                    glm::vec3 color = glm::vec3(0.0);
                    valReader >> color.r >> color.g >> color.b;
                    entityMaterial.DiffuseColor = color;
                }

                counterVertices += 1;
            }
            if (counterVertices >= countVertices && counterFaces < countFaces) {
                std::stringstream valReader(singleLine);
                int fCount;
                valReader >> fCount;
                if (fCount == 3) {
                    unsigned int index1, index2, index3;
                    valReader >> index1 >> index2 >> index3;
                    entityModel.indices.push_back(index1);
                    entityModel.indices.push_back(index2);
                    entityModel.indices.push_back(index3);
                }
                else if (fCount == 4) {
                    unsigned int index1, index2, index3, index4;
                    valReader >> index1 >> index2 >> index3 >> index4;
                    entityModel.indices.push_back(index1);
                    entityModel.indices.push_back(index2);
                    entityModel.indices.push_back(index3);
                    entityModel.indices.push_back(index4);
                }

                counterFaces += 1;
            }
        }

        if (boost::starts_with(singleLine, "end_header"))
            headerEnded = true;
    }
    ply_file_data.close();

    entityModel.ModelMaterial = entityMaterial;
    entityModel.countVertices = int(entityModel.vertices.size()) * 3;
    entityModel.countTextureCoordinates = int(entityModel.texture_coordinates.size()) * 2;
    entityModel.countNormals = int(entityModel.normals.size()) * 3;
    entityModel.countIndices = int(entityModel.indices.size());

    currentModelID += 1;
    this->models.push_back(entityModel);

    Kuplung_printObjModels(this->models, false);
}

max_stars_repo_name: supudo/Kuplung

max_issues_repo_name: supudo/Kuplung

max_forks_repo_name: supudo/Kuplung


text:
/*
	Copyright (C) 2003-2013 by David White <davewx7@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgment in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/

#pragma once

#include <boost/shared_array.hpp>

#include "asserts.hpp"
#include "graphics.hpp"
#include "shaders.hpp"

namespace graphics
{
	class fbo;
	typedef std::shared_ptr<fbo> fbo_ptr;

	class fbo
	{
	public:
		explicit fbo(int x, int y, int width, int height, int screen_width, int screen_height);
		explicit fbo(int x, int y, int width, int height, int screen_width, int screen_height, const gles2::shader_program_ptr& shader);
		virtual ~fbo();

		size_t width() const { return width_; }
		size_t height() const { return height_; }

		size_t awidth() const { return awidth_; }
		size_t aheight() const { return aheight_; }

		size_t x() const { return x_; }
		size_t y() const { return y_; }

		int letterbox_width() const { return letterbox_width_; }
		int letterbox_height() const { return letterbox_height_; }

		void enable_depth_test(bool dt = true) { depth_test_enable_ = dt; }
		bool is_depth_test_enabled() const { return depth_test_enable_; }

		void set_final_shader(const gles2::shader_program_ptr& shader) { final_shader_ = shader; }

		void init();
		void render_to_screen();
		void render_to_screen(const gles2::shader_program_ptr& shader);

		void draw_begin();
		void draw_end();

		struct render_manager {
			render_manager(fbo_ptr f) : fbo_(f) {
				ASSERT_LOG(fbo_ != NULL, "fbo::render_manager() called on null fbo pointer.");
				fbo_->draw_begin();
			}
			virtual ~render_manager() {
				fbo_->draw_end();
			}
			fbo_ptr fbo_;
		private:
			render_manager();
			render_manager(const render_manager&);
		};
	private:
		void calculate_letterbox();

		glm::mat4 proj_;
		size_t tex_width_;
		size_t tex_height_;
		int x_;
		int y_;
		int width_;
		int height_;

		int awidth_;
		int aheight_;

		int letterbox_width_;
		int letterbox_height_;

		GLint video_framebuffer_id_;

		bool depth_test_enable_;

		gles2::shader_program_ptr final_shader_;

		boost::shared_array<GLuint> framebuffer_id_;
		boost::shared_array<GLuint> render_buffer_id_;
		boost::shared_array<GLuint> final_texture_id_;

		fbo(const fbo&);
	};
}

ext: hpp

max_stars_repo_name: sweetkristas/anura

max_issues_repo_name: sweetkristas/anura

max_forks_repo_name: sweetkristas/anura


text:
#include <omni/core/context.hpp>
#include <omni/core/model/module.hpp>
#include <omni/core/model/block.hpp>
#include <omni/core/model/type.hpp>
#include <omni/core/model/function.hpp>
#include <omni/core/model/entity.hpp>
#include <omni/core/model/return_statement.hpp>
#include <omni/core/model/builtin_literal_expression.hpp>
#include <omni/core/model/function_call_expression.hpp>
#include <omni/tests/test_file_manager.hpp>
#include <omni/tests/test_utils.hpp>
#include <set>
#include <memory>
#include <iostream>

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(contextTests)

BOOST_AUTO_TEST_CASE(ctor)
{
    using namespace omni::core;
    context c;
}

/**
Creates 20 ids for each domain and checks whether they are unique.
**/
BOOST_AUTO_TEST_CASE(createId)
{
    using namespace omni::core;
    context c;
    model::module mod (c, "test");
    std::set <id> ids;
    for (domain d = domain::first; d < domain::last; d = static_cast <domain> (static_cast <int> (d) + 1)) {
        for (int i = 0; i < 20; ++ i) {
            id id = mod.createId (d);
            BOOST_CHECK (ids.find (id) == ids.end ());
            ids.insert (id);
            BOOST_CHECK (ids.find (id) != ids.end ());
        }
    }
}

/**
Tests that a function can be created using createFunction and later be found via findFunctionByName and findEntityById.
**/
BOOST_AUTO_TEST_CASE (createAndFindFunction)
{
    using namespace omni::core;
    context c;
    model::module mod (c, "test");
    std::shared_ptr <model::block> emptyBody (new model::block ());
    const std::string functionName = "test";
    std::shared_ptr <model::function> func = mod.createFunction (
        functionName,
        static_cast <std::shared_ptr <model::type>> (model::type::sharedBasicType (c, model::type_class::t_signedInt)),
        emptyBody);

    BOOST_CHECK (func->getName () == functionName);

    BOOST_CHECK (mod.findFunctionByName (func->getName ()) != nullptr);
    BOOST_CHECK (mod.findFunctionByName (func->getName ())->getName () == functionName);

    BOOST_CHECK (mod.findFunctionByName (functionName) != nullptr);
    BOOST_CHECK (mod.findFunctionByName (functionName)->getName () == functionName);

    BOOST_CHECK (mod.lookupComponentById (func->getId ()) != std::shared_ptr <model::entity> ());
    BOOST_CHECK (std::dynamic_pointer_cast <model::function> (mod.lookupComponentById (func->getId ()))->getName () == functionName);
}

/**
Tests that a manually created function can be added using addFunction and later be found via findFunctionByName and findEntityById.
**/
BOOST_AUTO_TEST_CASE (addAndFindFunction)
{
    using namespace omni::core;
    context c;
    model::module mod (c, "test");
    std::shared_ptr <model::block> emptyBody (new model::block ());
    const std::string functionName = "test";
    std::shared_ptr <model::function> func (new model::function (
        functionName,
        model::type::sharedBasicType (c, model::type_class::t_signedInt),
        emptyBody));

    BOOST_CHECK (func->getContext () == nullptr);

    mod.addFunction (func);

    BOOST_CHECK (func->getName () == functionName);

    BOOST_CHECK (func->getContext () == & c);

    BOOST_CHECK (mod.findFunctionByName (func->getName ()) != nullptr);
    BOOST_CHECK (mod.findFunctionByName (func->getName ())->getName () == functionName);

    BOOST_CHECK (mod.findFunctionByName (functionName) != nullptr);
    BOOST_CHECK (mod.findFunctionByName (functionName)->getName () == functionName);

    BOOST_CHECK (mod.lookupComponentById (func->getId ()) != std::shared_ptr <model::entity> ());
    BOOST_CHECK (std::dynamic_pointer_cast <model::function> (mod.lookupComponentById (func->getId ()))->getName () == functionName);
}

/**
Tests that a previously added function that is verified to exist in a context can be removed using removeFunction.
**/
BOOST_AUTO_TEST_CASE (removeFunction)
{
    using namespace omni::core;
    context c;
    model::module mod (c, "test");
    std::shared_ptr <model::block> emptyBody (new model::block ());
    const std::string functionName = "test";
    std::shared_ptr <model::function> func = mod.createFunction (
        functionName,
        static_cast <std::shared_ptr <model::type>> (model::type::sharedBasicType (c, model::type_class::t_signedInt)),
        emptyBody);

    BOOST_CHECK (mod.findFunctionByName (func->getName ()) != nullptr);
    BOOST_CHECK (mod.findFunctionByName (func->getName ())->getName () == functionName);

    BOOST_CHECK (mod.removeFunction (func));

    BOOST_CHECK (func->getContext () == nullptr);

    BOOST_CHECK (mod.findFunctionByName (func->getName ()) == nullptr);
    BOOST_CHECK (mod.findFunctionByName (functionName) == nullptr);
}

/**
Writes an assembly (.ll) file and checks, whether it exists.
TODO: Check, whether llvm can interpret the resulting ll file.
**/
BOOST_AUTO_TEST_CASE (emitAssemblyFile)
{
    using namespace omni::core;
    context c;
    model::module mod (c, "test");
    std::shared_ptr <model::block> body (new model::block ());
    std::shared_ptr <model::builtin_literal_expression <signed int>> literal42 (new model::builtin_literal_expression <signed int> (c, 42));
    std::shared_ptr <model::statement> return42 (new model::return_statement (literal42));
    body->appendStatement (return42);
    const std::string functionName = "test";
    std::shared_ptr <model::function> func = mod.createFunction (
        functionName,
        static_cast <std::shared_ptr <model::type>> (model::type::sharedBasicType (c, model::type_class::t_signedInt)),
        body);
    omni::tests::test_file_manager testFileManager;
    std::string assemblyFileName = testFileManager.getTestFileName ("emitAssemblyFile.ll").string ();
    mod.emitAssemblyFile (assemblyFileName);
    BOOST_CHECK (boost::filesystem::exists(assemblyFileName));
}

/**
Writes an object file (.o on Linux/Unix, .obj on Windows) and checks, whether it exists.
**/
BOOST_AUTO_TEST_CASE (emitObjectFile)
{
    using namespace omni::core;
    context c;
    model::module mod (c, "test");
    std::shared_ptr <model::block> body (new model::block ());
    std::shared_ptr <model::builtin_literal_expression <signed int>> literal42 (new model::builtin_literal_expression <signed int> (c, 42));
    std::shared_ptr <model::statement> return42 (new model::return_statement (literal42));
    body->appendStatement (return42);
    const std::string functionName = "test";
    std::shared_ptr <model::function> func = mod.createFunction (
        functionName,
        static_cast <std::shared_ptr <model::type>> (model::type::sharedBasicType (c, model::type_class::t_signedInt)),
        body);
    omni::tests::test_file_manager testFileManager;
    boost::filesystem::path objectFilePath = testFileManager.getTestFileName ("emitObjectFile.obj");
    std::string objectFileName = objectFilePath.string ();
    mod.emitObjectFile (objectFileName);
    BOOST_CHECK (boost::filesystem::exists (objectFileName));
}

/**
Writes an shared object file (.so on Linux/Unix, .dll on Windows) and checks, whether it exists, tries to load it and then tries
to call the function exported from it.
**/
BOOST_AUTO_TEST_CASE (emitSharedLibraryFile)
{
    using namespace omni::core;
    context c;
    model::module mod (c, "test");
    std::shared_ptr <model::block> body (new model::block ());
    std::shared_ptr <model::builtin_literal_expression <signed int>> literal42 (new model::builtin_literal_expression <signed int> (c, 42));
    std::shared_ptr <model::statement> return42 (new model::return_statement (literal42));
    body->appendStatement (return42);
    const std::string functionName = "test";
    std::shared_ptr <model::function> func = mod.createFunction (functionName, model::type::sharedBasicType (c, model::type_class::t_signedInt), body);
    omni::tests::test_file_manager testFileManager;
    int functionCallResult = omni::tests::runFunction <int> (func, testFileManager, "emitSharedLibraryFile");
    BOOST_CHECK_EQUAL (functionCallResult, 42);
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: daniel-kun/omni

max_issues_repo_name: daniel-kun/omni

max_forks_repo_name: daniel-kun/omni


text:
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/convex_hull_3.h>
#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_3 Point;
typedef CGAL::Surface_mesh<Point> Surface_mesh;
typedef Surface_mesh::Vertex_index Vertex;
typedef Surface_mesh::Edge_index Edge;
typedef Surface_mesh::Face_index Face;
typedef Surface_mesh::Halfedge_index Halfedge;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 10000; ++i){
        vtkNew<vtkPolyDataReader> reader;
        reader->SetFileName("Bad.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = static_cast<double_t*>(poly->GetPoints()->GetData()->GetVoidPointer(0));
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        std::vector<Point> spherePoints(N);
        for( auto i=0; i < N; ++i){
            spherePoints[i] = Point(points(0,i),points(1,i),points(2,i));
        }

        // Make a mesh object
        Surface_mesh sm;

        // Calculate the convex hull
        CGAL::convex_hull_3(spherePoints.begin(),
                            spherePoints.end(), sm);

        // To extract the surface
        // Write to a vtk file
        vtkNew<vtkCellArray> triangles;
        for( auto f : sm.faces() ){
            triangles->InsertNextCell(3);
	    auto he = sm.halfedge( f );
            for( auto j=0; j < 3; ++j){
                triangles->InsertCellPoint( static_cast<size_t>(sm.target(he)) );
		he = sm.next( he );
            }
        }
        poly->SetPolys(triangles);
        vtkNew<vtkPolyDataWriter> writer;
        writer->SetFileName("CH_Mesh.vtk");
        writer->SetInputData(poly);
        writer->Write();
    }
    float diff(static_cast<float>(clock()) - static_cast<float>(t1));
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}

ext: cxx


text:
/*<-
Copyright (c) 2016 Barrett Adair

Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
->*/

//[ is_const_member
#include <type_traits>
#include <boost/callable_traits/is_const_member.hpp>

namespace ct = boost::callable_traits;

struct foo;

static_assert(ct::is_const_member<int(foo::*)() const>::value, "");
static_assert(!ct::is_const_member<int(foo::*)()>::value, "");

int main() {}
//]


text:
#include "vaizon_logger.h"

// clang-format off
#include "spdlog/sinks/stdout_color_sinks.h"
#include "spdlog/sinks/rotating_file_sink.h"
#include "dev_sink.h"
// clang-format on
#include <boost/filesystem.hpp>

#include <cstdlib>
#include <fstream>
#include <iostream>

namespace vaizon {

namespace fs = boost::filesystem;

bool parse_log_level(const std::string& input,
                     spdlog::level::level_enum& logLevel) {

    const auto it = std::find_if(
        logLevelMap.begin(), logLevelMap.end(),
        [&](const LogLevelPair& pair) { return pair.first == input; });
    if (it != logLevelMap.end()) {
        logLevel = it->second;
        return true;
    }
    return false;
}

void print_log_levels() {
    std::cerr << "  Log Levels:\n";
    for (const auto& logLevel : logLevelMap) {
        std::cerr << "    " << logLevel.first << "\n";
    }
}

void init_logging(const std::string& data_dir,
                  spdlog::level::level_enum log_level) {

    const std::string log_location =
        (fs::path(data_dir) / "storage.logs").string();
    // Log to disk output stream
    const auto input = std::shared_ptr<std::ofstream>(
        new std::ofstream(log_location, std::ios::out | std::ios::app));
    if (input->is_open()) {
        input->close();
    } else {
        std::cerr << "Could not open " << log_location << std::endl;
        return;
    }

    constexpr size_t LOG_FILE_SIZE_LIMIT = 1024 * 1024 * 50; // 50Mb
    constexpr size_t EXTRA_FILES = 1;

    auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
    console_sink->set_level(log_level);

    // setting this to `true` can be useful for debugging on testnet
    bool rotate_on_open = false;

    auto file_sink = std::make_shared<spdlog::sinks::rotating_file_sink_mt>(
        log_location, LOG_FILE_SIZE_LIMIT, EXTRA_FILES, rotate_on_open);
    file_sink->set_level(log_level);

    auto developer_sink = std::make_shared<vaizon::dev_sink_mt>();

    /// IMPORTANT: get_logs endpoint assumes that sink #3 is a dev sink
    std::vector<spdlog::sink_ptr> sinks = {console_sink, file_sink,
                                           developer_sink};

    auto logger = std::make_shared<spdlog::logger>("vaizon_logger", sinks.begin(),
                                                   sinks.end());
    logger->set_level(log_level);
    logger->flush_on(spdlog::level::err);

    developer_sink->set_level(spdlog::level::warn);
    spdlog::register_logger(logger);
    spdlog::flush_every(std::chrono::seconds(1));

    spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v");

    VAIZON_LOG(info,
             "\n**************************************************************"
             "\nOutputting logs to {}",
             log_location);
}
} // namespace vaizon

max_stars_repo_name: Vaizon-Project/vaizon-storage-server

max_issues_repo_name: Vaizon-Project/vaizon-storage-server

max_forks_repo_name: Vaizon-Project/vaizon-storage-server


text:
//
// Copyright (c) 2010-2015 Marat Abrarov (abrarov@gmail.com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#include <cstddef>
#include <algorithm>
#include <iostream>
#include <vector>
#include <string>
#include <boost/cstdint.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/optional.hpp>
#include <boost/noncopyable.hpp>
#include <boost/date_time/posix_time/ptime.hpp>
#include <gtest/gtest.h>
#include <ma/config.hpp>
#include <ma/thread_group.hpp>
#include <ma/limited_int.hpp>
#include <ma/shared_ptr_factory.hpp>
#include <ma/detail/memory.hpp>
#include <ma/detail/functional.hpp>
#include <ma/detail/thread.hpp>
#include <ma/detail/sp_singleton.hpp>
#include <ma/detail/random.hpp>

namespace ma {
namespace test {

template <typename Integer>
std::string integer_to_string(Integer value)
{
  return boost::lexical_cast<std::string>(static_cast<boost::uintmax_t>(value));
}

template <typename Integer>
std::string to_string(const ma::limited_int<Integer>& limited_value)
{
  if (limited_value.overflowed())
  {
    return ">" + integer_to_string(limited_value.value());
  }
  else
  {
    return integer_to_string(limited_value.value());
  }
}

namespace sp_singleton_construction {

class foo;
typedef detail::shared_ptr<foo> foo_ptr;

class foo : private boost::noncopyable
{
private:
  typedef foo this_type;

public:
  static foo_ptr get_nullable_instance();
  static foo_ptr get_instance();

  int data() const;

protected:
  typedef detail::sp_singleton<this_type>::instance_guard instance_guard_type;

  foo(const instance_guard_type&, int);
  ~foo();

private:
  struct factory;

  instance_guard_type instance_guard_;
  int data_;
}; // class foo

TEST(sp_singleton, construction)
{
  std::vector<int> data;
  for (std::size_t i = 0; i != 2; ++i)
  {
    foo_ptr foo0 = foo::get_nullable_instance();
    ASSERT_FALSE(static_cast<bool>(foo0));
    const foo_ptr foo1 = foo::get_instance();
    const foo_ptr foo2 = foo::get_instance();
    ASSERT_EQ(foo1->data(), foo2->data());
    foo0 = foo::get_nullable_instance();
    ASSERT_TRUE(static_cast<bool>(foo0));
    ASSERT_EQ(foo0->data(), foo2->data());
    data.push_back(foo0->data());
  }
  ASSERT_NE(data[0], data[1]);
} // TEST(sp_singleton, construction)

struct foo::factory
{
  foo_ptr operator()(const instance_guard_type& instance_guard)
  {
    typedef ma::shared_ptr_factory_helper<foo> helper;
    static int data = 0;
    return detail::make_shared<helper>(instance_guard, data++);
  }
}; // struct factory

foo_ptr foo::get_nullable_instance()
{
  return detail::sp_singleton<foo>::get_nullable_instance();
}

foo_ptr foo::get_instance()
{
  return detail::sp_singleton<foo>::get_instance(factory());
}

int foo::data() const
{
  return data_;
}

foo::foo(const instance_guard_type& instance_guard, int data)
  : instance_guard_(instance_guard)
  , data_(data)
{
}

foo::~foo()
{
}

} // namespace sp_singleton_construction

namespace sp_singleton_thread {

class foo;
typedef detail::shared_ptr<foo> foo_ptr;
typedef detail::weak_ptr<foo>   foo_weak_ptr;

class foo : private boost::noncopyable
{
private:
  typedef foo this_type;

public:
  static foo_ptr get_nullable_instance();
  static foo_ptr get_instance();

  int data() const;

protected:
  typedef detail::sp_singleton<this_type>::instance_guard instance_guard_type;

  foo(const instance_guard_type&, int);
  ~foo();

private:
  struct factory;

  instance_guard_type instance_guard_;
  int data_;
}; // class foo

void thread_func(foo_ptr foo)
{
  (void) foo->data();
}

void thread_func2(const foo_weak_ptr& weak_foo, detail::latch& latch1,
    detail::latch& latch2, detail::latch& latch3)
{
  if (foo_ptr foo = weak_foo.lock())
  {
    latch1.count_down();
    (void) foo->data();
    latch2.wait();
  }
  latch3.count_down();
}

TEST(sp_singleton, thread)
{
  {
    detail::thread t(detail::bind(thread_func, foo::get_instance()));
    t.join();
    const foo_ptr foo = foo::get_instance();
    ASSERT_EQ(1, foo->data());
  }

  {
    detail::latch latch1(1);
    detail::latch latch2(1);
    detail::latch latch3(1);

#if defined(MA_USE_CXX11_STDLIB_MEMORY)
    detail::unique_ptr<detail::thread> t;
#else
    detail::scoped_ptr<detail::thread> t;
#endif

    {
      const foo_ptr thread_foo = foo::get_instance();
      t.reset(new detail::thread(detail::bind(thread_func2,
          foo_weak_ptr(thread_foo), detail::ref(latch1), detail::ref(latch2),
          detail::ref(latch3))));
      latch1.wait();
    }
    {
      const foo_ptr foo = foo::get_nullable_instance();
      latch2.count_down();
      ASSERT_TRUE(static_cast<bool>(foo));
    }
    {
      ma::limited_int<std::size_t> count = 0;
      while (const foo_ptr foo = foo::get_nullable_instance())
      {
        ++count;
      }
      std::cout << to_string(count) << std::endl;
    }
    {
      latch3.wait();
      const foo_ptr foo = foo::get_nullable_instance();
      ASSERT_FALSE(static_cast<bool>(foo));
    }
    t->join();
  }
} // TEST(sp_singleton, thread)

struct foo::factory
{
  foo_ptr operator()(const instance_guard_type& instance_guard)
  {
    typedef ma::shared_ptr_factory_helper<foo> helper;
    static int data = 0;
    return detail::make_shared<helper>(instance_guard, data++);
  }
}; // struct foo::factory

foo_ptr foo::get_nullable_instance()
{
  return detail::sp_singleton<foo>::get_nullable_instance();
}

foo_ptr foo::get_instance()
{
  return detail::sp_singleton<foo>::get_instance(factory());
}

int foo::data() const
{
  return data_;
}

foo::foo(const instance_guard_type& instance_guard, int data)
  : instance_guard_(instance_guard)
  , data_(data)
{
}

foo::~foo()
{
}

} // namespace sp_singleton_thread

namespace sp_singleton_sync {

detail::latch destroy_start_latch;
detail::latch destroy_complete_latch;

class foo;
typedef detail::shared_ptr<foo> foo_ptr;

class foo : private boost::noncopyable
{
private:
  typedef foo this_type;

public:
  static foo_ptr get_nullable_instance();
  static foo_ptr get_instance();

  int data() const;

protected:
  typedef detail::sp_singleton<this_type>::instance_guard instance_guard_type;

  foo(const instance_guard_type&, int);
  ~foo();

private:
  struct factory;

  static std::size_t instance_count_;

  boost::optional<instance_guard_type> instance_guard_;
  int data_;
}; // class foo

std::size_t foo::instance_count_ = 0;

void thread_func()
{
  const foo_ptr f = foo::get_instance();
  ASSERT_TRUE(static_cast<bool>(f));
  ASSERT_EQ(0, f->data());
  (void) f;
}

TEST(sp_singleton, sync)
{
  {
    destroy_start_latch.count_up();
    destroy_complete_latch.count_up();
    detail::thread t(thread_func);
    destroy_start_latch.wait();
    {
      const foo_ptr f = foo::get_nullable_instance();
      ASSERT_FALSE(static_cast<bool>(f));
    }
    destroy_complete_latch.count_down();
    {
      const foo_ptr f = foo::get_instance();
      ASSERT_TRUE(static_cast<bool>(f));
      ASSERT_EQ(1, f->data());
      destroy_start_latch.count_up();
    }
    t.join();
  }
} // TEST(sp_singleton, sync)

struct foo::factory
{
  foo_ptr operator()(const instance_guard_type& instance_guard)
  {
    typedef ma::shared_ptr_factory_helper<foo> helper;
    static int data = 0;
    return detail::make_shared<helper>(instance_guard, data++);
  }
}; // struct foo::factory

foo_ptr foo::get_nullable_instance()
{
  return detail::sp_singleton<foo>::get_nullable_instance();
}

foo_ptr foo::get_instance()
{
  return detail::sp_singleton<foo>::get_instance(factory());
}

int foo::data() const
{
  return data_;
}

foo::foo(const instance_guard_type& instance_guard, int data)
  : instance_guard_(instance_guard)
  , data_(data)
{
  EXPECT_EQ(0U, instance_count_);
  ++instance_count_;
}

foo::~foo()
{
  destroy_start_latch.count_down();
  destroy_complete_latch.wait();
  if (!data_)
  {
    detail::this_thread::sleep(boost::posix_time::seconds(1));
  }
  --instance_count_;
  instance_guard_ = boost::none;
}

} // namespace sp_singleton_sync

namespace sp_singleton_sync2 {

class foo;
typedef detail::shared_ptr<foo> foo_ptr;

class foo : private boost::noncopyable
{
private:
  typedef foo this_type;

public:
  typedef ma::limited_int<std::size_t> counter;

  static foo_ptr get_nullable_instance();
  static foo_ptr get_instance();

  counter init_count() const;

protected:
  typedef detail::sp_singleton<this_type>::instance_guard instance_guard_type;

  foo(const instance_guard_type&, const counter&);
  ~foo();

private:
  struct factory;

  static std::size_t instance_count_;

  boost::optional<instance_guard_type> instance_guard_;
  counter init_counter_;
}; // class foo

std::size_t foo::instance_count_   = 0;
const std::size_t iteration_count  = 1;
const std::size_t work_cycle_count = 1;

typedef detail::mt19937 random_generator;
typedef detail::shared_ptr<random_generator> random_generator_ptr;

void work_func(const random_generator_ptr& rng)
{
  detail::uniform_int_distribution<> wait_flag(0, 1);
  for (std::size_t i = 0; i != work_cycle_count; ++i)
  {
    const foo_ptr f = foo::get_instance();
    ASSERT_TRUE(static_cast<bool>(f));
    if (wait_flag(*rng))
    {
      detail::this_thread::sleep(boost::posix_time::milliseconds(1));
    }
    (void) f;
  }
}

void thread_func(detail::barrier& work_barrier, const random_generator_ptr& rng)
{
  work_barrier.count_down_and_wait();
  work_func(rng);
}

TEST(sp_singleton, sync2)
{
  const std::size_t thread_count = 2;

  std::vector<random_generator_ptr> rngs;
  for (std::size_t i = 0; i != thread_count; ++i)
  {
    rngs.push_back(detail::make_shared<random_generator>());
  }

  for (std::size_t n = 0; n != iteration_count; ++n)
  {
    detail::barrier work_barrier(
        static_cast<detail::barrier::counter_type>(thread_count));
    ma::thread_group threads;
    for (std::size_t i = 0; i != thread_count - 1; ++i)
    {
      threads.create_thread(
          detail::bind(thread_func, detail::ref(work_barrier), rngs[i]));
    }
    thread_func(work_barrier, rngs.back());
    threads.join_all();
  }

  const foo_ptr f = foo::get_instance();
  std::cout << "Init counter: " << to_string(f->init_count()) << std::endl;
} // TEST(sp_singleton, sync2)

struct foo::factory
{
  foo_ptr operator()(const instance_guard_type& instance_guard)
  {
    typedef ma::shared_ptr_factory_helper<foo> helper;
    static counter init_counter = 0;
    ++init_counter;
    return detail::make_shared<helper>(instance_guard, init_counter);
  }
}; // struct foo::factory

foo_ptr foo::get_nullable_instance()
{
  return detail::sp_singleton<foo>::get_nullable_instance();
}

foo_ptr foo::get_instance()
{
  return detail::sp_singleton<foo>::get_instance(factory());
}

foo::counter foo::init_count() const
{
  return init_counter_;
}

foo::foo(const instance_guard_type& instance_guard, const counter& init_counter)
  : instance_guard_(instance_guard)
  , init_counter_(init_counter)
{
  EXPECT_EQ(0U, instance_count_);
  ++instance_count_;
}

foo::~foo()
{
  EXPECT_EQ(1U, instance_count_);
  --instance_count_;
  instance_guard_ = boost::none;
}

} // namespace sp_singleton_sync2

} // namespace test
} // namespace ma


text:
#include <gtest/gtest.h>

#include <avahi-client/client.h>
#include <avahi-client/lookup.h>
#include <avahi-client/publish.h>
#include <avahi-common/error.h>
#include <avahi-common/malloc.h>
#include <avahi-common/alternative.h>

#include <boost/lockfree/spsc_queue.hpp>
#include <string>
#include <sstream>
#include <iostream>

#include <boost/asio.hpp>
#include <boost/system/error_code.hpp>
#include "boost/asio/ip/dnssd/impl/avahi_poll.hpp"
using boost::system::error_code;
using std::string;
using std::stringstream;
using std::cout; using std::endl; using std::cerr;

boost::lockfree::spsc_queue<string> found_queue(1024);

/// This is a test with callbacks lifted (almost) directly from the
/// [Avahi examples](http://avahi.org/download/doxygen/examples.html)
/// as a basic test of the implementation of AvahiPoll.

static void create_services(AvahiClient *c);
static AvahiEntryGroup *group = NULL;
static char *name = "Hello World";

static void browse_callback(
    AvahiServiceBrowser *b,
    AvahiIfIndex interface,
    AvahiProtocol protocol,
    AvahiBrowserEvent event,
    const char *name,
    const char *type,
    const char *domain,
    AVAHI_GCC_UNUSED AvahiLookupResultFlags flags,
    void* userdata) {

    auto c = static_cast<AvahiClient *>(userdata);
    assert(b);

    /* Called whenever a new services becomes available on the LAN or is removed from the LAN */
    stringstream found;

    switch (event) {
        case AVAHI_BROWSER_FAILURE:
            fprintf(stderr, "(Browser) %s\n", avahi_strerror(avahi_client_errno(avahi_service_browser_get_client(b))));
            return;

        case AVAHI_BROWSER_NEW:
	    found << name << '.' << type << '.' << domain << '.';
	    found_queue.push(found.str());
	    cerr << found.str() << endl;
            fprintf(stderr, "(Browser) NEW: service '%s' of type '%s' in domain '%s'\n", name, type, domain);
            break;

        case AVAHI_BROWSER_REMOVE:
            fprintf(stderr, "(Browser) REMOVE: service '%s' of type '%s' in domain '%s'\n", name, type, domain);
            break;

        case AVAHI_BROWSER_ALL_FOR_NOW:
        case AVAHI_BROWSER_CACHE_EXHAUSTED:
            fprintf(stderr, "(Browser) %s\n", event == AVAHI_BROWSER_CACHE_EXHAUSTED ? "CACHE_EXHAUSTED" : "ALL_FOR_NOW");
            break;
    }
}

static void entry_group_callback(AvahiEntryGroup *g, AvahiEntryGroupState state, AVAHI_GCC_UNUSED void *userdata) {
    assert(g == group || group == NULL);
    group = g;

    /* Called whenever the entry group state changes */

    switch (state) {
        case AVAHI_ENTRY_GROUP_ESTABLISHED :
            /* The entry group has been established successfully */
            fprintf(stderr, "Service '%s' successfully established.\n", name);
            break;

        case AVAHI_ENTRY_GROUP_COLLISION : {
            char *n;

            /* A service name collision with a remote service
             * happened. Let's pick a new name */
            n = avahi_alternative_service_name(name);
            avahi_free(name);
            name = n;

            fprintf(stderr, "Service name collision, renaming service to '%s'\n", name);

            /* And recreate the services */
            create_services(avahi_entry_group_get_client(g));
            break;
        }

        case AVAHI_ENTRY_GROUP_FAILURE :
            fprintf(stderr, "Entry group failure: %s\n", avahi_strerror(avahi_client_errno(avahi_entry_group_get_client(g))));
            break;

        case AVAHI_ENTRY_GROUP_UNCOMMITED:
        case AVAHI_ENTRY_GROUP_REGISTERING:
            ;
    }
}

static void create_services(AvahiClient *c) {
    char *n, r[128];
    int ret;
    assert(c);

    /* If this is the first time we're called, let's create a new
     * entry group if necessary */

    if (!group)
        if (!(group = avahi_entry_group_new(c, entry_group_callback, NULL))) {
            fprintf(stderr, "avahi_entry_group_new() failed: %s\n", avahi_strerror(avahi_client_errno(c)));
        }

    /* If the group is empty (either because it was just created, or
     * because it was reset previously, add our entries.  */

    if (avahi_entry_group_is_empty(group)) {
        fprintf(stderr, "Adding service '%s'\n", name);

        /* Create some random TXT data */
        snprintf(r, sizeof(r), "random=%i", rand());

        /* Add the service for IPP */
        if ((ret = avahi_entry_group_add_service(group, AVAHI_IF_UNSPEC, AVAHI_PROTO_UNSPEC, (AvahiPublishFlags)0, name, "_http._tcp", NULL, NULL, 651, "test=blah", r, NULL)) < 0) {

            if (ret == AVAHI_ERR_COLLISION)
                goto collision;

            fprintf(stderr, "Failed to add _http._tcp service: %s\n", avahi_strerror(ret));
        }

        /* Tell the server to register the service */
        if ((ret = avahi_entry_group_commit(group)) < 0) {
            fprintf(stderr, "Failed to commit entry group: %s\n", avahi_strerror(ret));
        }
    }

    return;

collision:

    /* A service name collision with a local service happened. Let's
     * pick a new name */
    n = avahi_alternative_service_name(name);
    avahi_free(name);
    name = n;

    fprintf(stderr, "Service name collision, renaming service to '%s'\n", name);

    avahi_entry_group_reset(group);

    create_services(c);
    return;
}

static void client_callback(AvahiClient *c, AvahiClientState state, AVAHI_GCC_UNUSED void * userdata) {
    assert(c);

    /* Called whenever the client or server state changes */

    switch (state) {
        case AVAHI_CLIENT_S_RUNNING:

            /* The server has startup successfully and registered its host
             * name on the network, so it's time to create our services */
            create_services(c);
            break;

        case AVAHI_CLIENT_FAILURE:

            fprintf(stderr, "Client failure: %s\n", avahi_strerror(avahi_client_errno(c)));

            break;

        case AVAHI_CLIENT_S_COLLISION:

            /* Let's drop our registered services. When the server is back
             * in AVAHI_SERVER_RUNNING state we will register them
             * again with the new host name. */

        case AVAHI_CLIENT_S_REGISTERING:

            /* The server records are now being established. This
             * might be caused by a host name change. We need to wait
             * for our own records to register until the host name is
             * properly esatblished. */

            if (group)
                avahi_entry_group_reset(group);

            break;

        case AVAHI_CLIENT_CONNECTING:
            ;
    }
}

TEST (AvahiRegisterBrowse, AvahiTests) {

  boost::asio::io_service io;

  int error;
  AvahiClient *client = avahi_client_new(avahi_io_service_poll_wrap(io),
    (AvahiClientFlags)0, client_callback, 0, &error);

  AvahiServiceBrowser *browser = avahi_service_browser_new(client,
    AVAHI_IF_UNSPEC, AVAHI_PROTO_UNSPEC, "_http._tcp", 0,
    (AvahiLookupFlags)0, browse_callback, client);

  boost::asio::deadline_timer timer(io);
  timer.expires_from_now(boost::posix_time::seconds(2));
  timer.async_wait([&](const error_code& ec){ io.stop(); });

  io.run();

  stringstream ours;
  ours << name << '.' << "_http._tcp" << '.' << "local" << '.';
  string popped;
  while(found_queue.pop(popped)) {
	  if(popped == ours.str()) {
		  SUCCEED() << "We found the service we published ourselves";
		  return;
	  }
  }
  FAIL() << "The service we published was not discovered...";

}

int main(int argc, char *argv[]) {
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}

max_stars_repo_name: bkietz/boost-asio-dnssd

max_issues_repo_name: bkietz/boost-asio-dnssd

max_forks_repo_name: bkietz/boost-asio-dnssd


text:
#include <boost/foreach.hpp>
#include "io_interface.h"

static size_t NCOL = 512;
static size_t NROW = 64;

class compute_callback: public safs::callback {
    public:
        virtual int invoke(safs::io_request *reqs[], int num);
};

static void run_computation(char* buf, size_t bufsize) {
    double* arr = reinterpret_cast<double*>(buf);

    for (size_t i = 0; i < 5; i++)
        std::cout << arr[i] << " ";
    std::cout << "...\n";
}

int compute_callback::invoke(safs::io_request *reqs[], int num) {
    for (int i = 0; i < num; i++) {
        char *buf = reqs[i]->get_buf();
        // io_request has file_id and offset
        // also buf address can be used
        run_computation(buf, reqs[i]->get_size());
        free(buf);
    }
    return 0;
}

// We will use the 50 x 5 matrix
class task {
private:
    size_t ncol;
    size_t nrow;
    size_t taskno;

public:
    task(size_t taskno) {
        this->taskno = taskno;
        ncol = NCOL;
        nrow = NROW;
    }

    size_t get_size() const {
        return sizeof(double)*ncol;
    }

    off_t get_offset() const {
        return taskno*ncol;
    }

    const size_t get_taskno() const {
        return this->taskno;
    }
};

static void test(const std::string conf_file, const std::string &graph_file,
        const std::vector<task> &tasks) {
    config_map::ptr configs = config_map::create(conf_file);
    safs::init_io_system(configs);
    safs::file_io_factory::shared_ptr factory = safs::create_io_factory(graph_file,
            safs::REMOTE_ACCESS);
    safs::io_interface::ptr io = safs::create_io(factory,
            thread::get_curr_thread());
    io->set_callback(safs::callback::ptr(new compute_callback()));

    // How to choose max_ios threshold? Maybe 100 KB. For 4K,8K, 16K io
    //  requests use 64 pending io request. Larger like 1MB - 16 MB we can
    //  use like 8 pending ios.

    int max_ios = 20;
    BOOST_FOREACH(task t, tasks) {

        while (io->num_pending_ios() >= max_ios) {
            printf("\n... Waiting for ios to complete!\n");
            io->wait4complete(1);
        }

        // This is directed I/O. Memory buffer, I/O offset and I/O size
        // all need to be aligned to the I/O block size.
        size_t io_size = ROUNDUP_PAGE(t.get_size());

        printf("Task: %lu, io_size is: %lu\n", t.get_taskno(), io_size);

        safs::data_loc_t loc(factory->get_file_id(), t.get_offset());
        // The buffer will be free'd in the callback function.
        char *buf = (char *) valloc(io_size);
        safs::io_request req(buf, loc, io_size, READ);
        io->access(&req, 1);
    }
    io->wait4complete(io->num_pending_ios());
}

int main(int argc, char* argv[]) {
    std::vector<task> tasks;
    for (size_t i = 0; i < 50; i ++)
        tasks.push_back(task(i));

    test("/home/disa/Sandbox/SAFS/conf/run_graph.txt",
            "mat_64x512.bin", tasks);
    return EXIT_SUCCESS;
}

max_stars_repo_name: disa-mhembere/matree

max_issues_repo_name: disa-mhembere/monya

max_forks_repo_name: disa-mhembere/monya


text:
#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 3, 6> test_long_3_6(const Eigen::Matrix<boost::int64_t, 3, 6>& M) { return M; }
void export_long_3_6() { boost::python::def("test_long_3_6", test_long_3_6); }

max_stars_repo_name: chengfzy/kalibr

max_issues_repo_name: chengfzy/kalibr

max_forks_repo_name: chengfzy/kalibr


text:
/*!
@file
Forward declares `boost::hana::fill`.

@copyright Louis Dionne 2013-2017
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_FWD_FILL_HPP
#define BOOST_HANA_FWD_FILL_HPP

#include <boost/hana/config.hpp>
#include <boost/hana/core/when.hpp>

BOOST_HANA_NAMESPACE_BEGIN
    //! Replace all the elements of a structure with a fixed value.
    //! @ingroup group-Functor
    //!
    //!
    //! Signature
    //! ---------
    //! Given `F` a Functor, the signature is
    //! \f$
    //!     \mathtt{fill} : F(T) \times U \to F(U)
    //! \f$
    //!
    //! @param xs
    //! The structure to fill with a `value`.
    //!
    //! @param value
    //! A value by which every element `x` of the structure is replaced,
    //! unconditionally.
    //!
    //!
    //! Example
    //! -------
    //! @include example/fill.cpp
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto fill = [](auto&& xs, auto&& value) {
        return tag-dispatched;
    };
#else
    template <typename Xs, typename = void>
    struct fill_impl : fill_impl<Xs, when<true>> { };

    struct fill_t {
        template <typename Xs, typename Value>
        constexpr auto operator()(Xs&& xs, Value&& value) const;
    };

    BOOST_HANA_INLINE_VARIABLE constexpr fill_t fill{};
#endif
BOOST_HANA_NAMESPACE_END

#endif // !BOOST_HANA_FWD_FILL_HPP


text: #include <boost/type_traits/is_fundamental.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include <stdint.h>
#include <widget/KittiReader.h>
#include <QtCore/QDir>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include <fstream>
#include <iostream>
#include <sstream>
#include "rv/string_utils.h"

#include <boost/lexical_cast.hpp>

void KittiReader::initialize(const QString& directory) {
  velodyne_filenames_.clear();
  label_filenames_.clear();
  image_filenames_.clear();
  poses_.clear();

  pointsCache_.clear();
  labelCache_.clear();
  tiles_.clear();

  base_dir_ = QDir(directory);
  QDir velodyne_dir(base_dir_.filePath("velodyne"));
  QStringList entries = velodyne_dir.entryList(QDir::Files, QDir::Name);
  for (int32_t i = 0; i < entries.size(); ++i) {
    velodyne_filenames_.push_back(velodyne_dir.filePath(entries.at(i)).toStdString());
  }

  if (!base_dir_.exists("calib.txt"))
    throw std::runtime_error("Missing calibration file: " + base_dir_.filePath("calib.txt").toStdString());

  calib_.initialize(base_dir_.filePath("calib.txt").toStdString());

  readPoses(base_dir_.filePath("poses.txt").toStdString(), poses_);

  // create label dir, etc.
  QDir labels_dir(base_dir_.filePath("labels"));

  // find corresponding label files.
  if (!labels_dir.exists()) base_dir_.mkdir("labels");

  for (uint32_t i = 0; i < velodyne_filenames_.size(); ++i) {
    QString filename = QFileInfo(QString::fromStdString(velodyne_filenames_[i])).baseName() + ".label";
    if (!labels_dir.exists(filename)) {
      std::ifstream in(velodyne_filenames_[i].c_str());
      in.seekg(0, std::ios::end);
      uint32_t num_points = in.tellg() / (4 * sizeof(float));
      in.close();

      std::ofstream out(labels_dir.filePath(filename).toStdString().c_str());

      std::vector<uint32_t> labels(num_points, 0);
      out.write(reinterpret_cast<const char*>(labels.data()), num_points * sizeof(uint32_t));

      out.close();
    }

    label_filenames_.push_back(labels_dir.filePath(filename).toStdString());
  }

  std::string missing_img = QDir::currentPath().toStdString() + "/../assets/missing.png";
  QDir image_dir(base_dir_.filePath("image_2"));
  for (uint32_t i = 0; i < velodyne_filenames_.size(); ++i) {
    QString filename = QFileInfo(QString::fromStdString(velodyne_filenames_[i])).baseName() + ".png";
    if (image_dir.exists(filename)) {
      image_filenames_.push_back(image_dir.filePath(filename).toStdString());
    } else {
      image_filenames_.push_back(missing_img);
    }
  }

  // assumes that (0,0,0) is always the start.
  Eigen::Vector2f min = Eigen::Vector2f::Zero();
  Eigen::Vector2f max = Eigen::Vector2f::Zero();

  for (uint32_t i = 0; i < poses_.size(); ++i) {
    Eigen::Vector4f t = poses_[i].col(3);

    min.x() = std::min(t.x() - maxDistance_, min.x());
    min.y() = std::min(t.y() - maxDistance_, min.y());
    max.x() = std::max(t.x() + maxDistance_, max.x());
    max.y() = std::max(t.y() + maxDistance_, max.y());
  }

  //  std::cout << "tileSize = " << tileSize_ << std::endl;
  //  std::cout << "min = " << min << ", max = " << max << std::endl;

  offset_.x() = std::ceil((std::abs(min.x()) - 0.5 * tileSize_) / tileSize_) * tileSize_ + 0.5 * tileSize_;
  offset_.y() = std::ceil((std::abs(min.y()) - 0.5 * tileSize_) / tileSize_) * tileSize_ + 0.5 * tileSize_;

  //  std::cout << "offset = " << offset_ << std::endl;

  numTiles_.x() = std::ceil((std::abs(min.x()) - 0.5 * tileSize_) / tileSize_) +
                  std::ceil((max.x() - 0.5 * tileSize_) / tileSize_) + 1;
  numTiles_.y() = std::ceil((std::abs(min.y()) - 0.5 * tileSize_) / tileSize_) +
                  std::ceil((max.y() - 0.5 * tileSize_) / tileSize_) + 1;

  //  std::cout << "numTiles = " << numTiles_ << std::endl;

  tiles_.clear();
  tiles_.resize(numTiles_.x() * numTiles_.y());

  Eigen::Vector2f idxRadius(maxDistance_ / tileSize_, maxDistance_ / tileSize_);

  for (uint32_t i = 0; i < uint32_t(numTiles_.x()); ++i) {
    for (uint32_t j = 0; j < uint32_t(numTiles_.y()); ++j) {
      auto& tile = tiles_[tileIdxToOffset(i, j)];

      tile.i = i;
      tile.j = j;
      tile.x = i * tileSize_ - offset_.x() + 0.5 * tileSize_;
      tile.y = j * tileSize_ - offset_.y() + 0.5 * tileSize_;
      tile.size = tileSize_;
    }
  }

  trajectory_.clear();

  Eigen::Vector2f e(0.5 * tileSize_, 0.5 * tileSize_);
  for (uint32_t i = 0; i < poses_.size(); ++i) {
    Eigen::Vector2f t = poses_[i].col(3).head(2);
    Eigen::Vector2f idx((t.x() + offset_.x()) / tileSize_, (t.y() + offset_.y()) / tileSize_);

    trajectory_.push_back(Eigen::Vector2f((t.x() + offset_.x()) / tileSize_, (t.y() + offset_.y()) / tileSize_));

    //    tiles_[tileIdxToOffset(uint32_t(idx.x()), uint32_t(idx.y()))].indexes.push_back(i);
    //    uint32_t u_min = std::max(int32_t(idx.x() - idxRadius.x()), 0);
    //    uint32_t u_max = std::min(int32_t(std::ceil(idx.x() + idxRadius.x())), numTiles_.x());
    //    uint32_t v_min = std::max(int32_t(idx.y() - idxRadius.y()), 0);
    //    uint32_t v_max = std::min(int32_t(std::ceil(idx.y() + idxRadius.y())), numTiles_.y());

    // FIXME: workaround check all tiles.
    for (uint32_t u = 0; u < uint32_t(numTiles_.x()); ++u) {
      for (uint32_t v = 0; v < uint32_t(numTiles_.y()); ++v) {
        auto& tile = tiles_[tileIdxToOffset(u, v)];
        Eigen::Vector2f q = t - Eigen::Vector2f(tile.x, tile.y);
        q[0] = std::abs(q[0]);
        q[1] = std::abs(q[1]);

        // check for exact overlap (see Behley et al., ICRA, 2015)
        if (std::max(q[0], q[1]) > e[0] + maxDistance_) continue;  // definitely outside.
        if (std::min(q[0], q[1]) < e[0] || (q - e).norm() < maxDistance_) {
          tile.indexes.push_back(i);
        }
      }
    }
  }

  // sanity check:

  for (auto& t : tiles_) {
    std::sort(t.indexes.begin(), t.indexes.end());
    //    std::cout << "Tile has " << t.indexes.size() << " tiles associated." << std::endl;
    for (uint32_t i = 1; i < t.indexes.size(); ++i) {
      if (t.indexes[i - 1] == t.indexes[i]) {
        std::cout << "found duplicate!" << std::endl;
      }
    }
  }

  uint32_t tileCount = 0;
  for (uint32_t i = 0; i < uint32_t(numTiles_.x()); ++i) {
    for (uint32_t j = 0; j < uint32_t(numTiles_.y()); ++j) {
      auto& tile = tiles_[tileIdxToOffset(i, j)];

      std::sort(tile.indexes.begin(), tile.indexes.end());
      if (tile.indexes.size() > 0) tileCount += 1;
    }
  }

  std::cout << "#tiles  = " << tileCount << std::endl;

  // meta information for faster loading.
  if (base_dir_.exists("instances.txt")) {
    std::cout << "Reading instances.txt..." << std::flush;

    std::ifstream in(base_dir_.filePath("instances.txt").toStdString());

    while (in.good()) {
      std::string line;
      std::getline(in, line);
      if (line.size() == 0) break;

      std::vector<std::string> tokens = rv::split(line, ":");
      if (tokens.size() != 2) {
        throw std::runtime_error("Invalid instance meta information found!");
      }

      uint32_t label = boost::lexical_cast<uint32_t>(tokens[0]);
      uint32_t maxInstanceId = boost::lexical_cast<uint32_t>(tokens[1]);
      maxInstanceIds_[label] = maxInstanceId;

      in.peek();
    }

    in.close();
    std::cout << "finished." << std::endl;
  } else {
    std::cout << "Generating intances.txt" << std::flush;
    // get the counts from the label files.
    for (const std::string& filename : label_filenames_) {
      std::vector<uint32_t> labels;
      readLabels(filename, labels);

      for (uint32_t instance_label : labels) {
        uint32_t instanceId = (instance_label >> 16) & uint32_t(0xFFFF);
        uint32_t label = instance_label & uint32_t(0xFFFF);
        if (maxInstanceIds_.find(label) == maxInstanceIds_.end())
          maxInstanceIds_[label] = instanceId;
        else
          maxInstanceIds_[label] = std::max(instanceId, maxInstanceIds_[label]);
      }
    }

    // directly update meta information:
    updateMetaInformation(maxInstanceIds_);
  }
}

void KittiReader::updateMetaInformation(const std::map<uint32_t, uint32_t>& maxInstanceIds) {
  std::ofstream out(base_dir_.filePath("instances.txt").toStdString().c_str());
  for (auto it = maxInstanceIds.begin(); it != maxInstanceIds.end(); ++it) {
    out << it->first << ":" << it->second << std::endl;
  }
  out.close();
}

void KittiReader::retrieve(const Eigen::Vector3f& position, std::vector<uint32_t>& indexes,
                           std::vector<PointcloudPtr>& points, std::vector<LabelsPtr>& labels,
                           std::vector<std::string>& images) {
  Eigen::Vector2f idx((position.x() + offset_.x()) / tileSize_, (position.y() + offset_.y()) / tileSize_);

  retrieve(idx.x(), idx.y(), indexes, points, labels, images);
}

void KittiReader::retrieve(uint32_t i, uint32_t j, std::vector<uint32_t>& indexes, std::vector<PointcloudPtr>& points,
                           std::vector<LabelsPtr>& labels, std::vector<std::string>& images) {
  indexes.clear();
  points.clear();
  labels.clear();
  images.clear();

  std::vector<int32_t> indexesBefore;
  for (auto it = pointsCache_.begin(); it != pointsCache_.end(); ++it) indexesBefore.push_back(it->first);
  std::vector<int32_t> indexesAfter;

  uint32_t scansRead = 0;

  indexes = tiles_[tileIdxToOffset(i, j)].indexes;
  std::sort(indexes.begin(), indexes.end());
  for (uint32_t t : indexes) {
    indexesAfter.push_back(t);
    if (pointsCache_.find(t) == pointsCache_.end()) {
      scansRead += 1;

      points.push_back(std::shared_ptr<Laserscan>(new Laserscan));
      readPoints(velodyne_filenames_[t], *points.back());
      pointsCache_[t] = points.back();
      points.back()->pose = poses_[t];

      labels.push_back(std::shared_ptr<std::vector<uint32_t>>(new std::vector<uint32_t>()));
      readLabels(label_filenames_[t], *labels.back());
      labelCache_[t] = labels.back();

      if (points.back()->size() != labels.back()->size()) {
        std::cout << "Filename: " << velodyne_filenames_[t] << std::endl;
        std::cout << "Filename: " << label_filenames_[t] << std::endl;
        std::cout << "num. points = " << points.back()->size() << " vs. num. labels = " << labels.back()->size()
                  << std::endl;
        throw std::runtime_error("Inconsistent number of labels.");
      }

    } else {
      points.push_back(pointsCache_[t]);
      labels.push_back(labelCache_[t]);
    }

    images.push_back(image_filenames_[t]);
  }

  std::cout << scansRead << " point clouds read." << std::endl;

  // FIXME: keep more scans in cache. not only remove unloaded scans.

  std::sort(indexesBefore.begin(), indexesBefore.end());
  std::sort(indexesAfter.begin(), indexesAfter.end());

  std::vector<int32_t> needsDelete(indexesBefore.size());
  std::vector<int32_t>::iterator end = std::set_difference(
      indexesBefore.begin(), indexesBefore.end(), indexesAfter.begin(), indexesAfter.end(), needsDelete.begin());

  for (auto it = needsDelete.begin(); it != end; ++it) {
    pointsCache_.erase(*it);
    labelCache_.erase(*it);
  }
}

const KittiReader::Tile& KittiReader::getTile(const Eigen::Vector3f& position) const {
  Eigen::Vector2f idx((position.x() + offset_.x()) / tileSize_, (position.y() + offset_.y()) / tileSize_);
  return tiles_[tileIdxToOffset(idx.x(), idx.y())];
}
const KittiReader::Tile& KittiReader::getTile(uint32_t i, uint32_t j) const { return tiles_[tileIdxToOffset(i, j)]; }

void KittiReader::setTileSize(float size) { tileSize_ = size; }

void KittiReader::update(const std::vector<uint32_t>& indexes, std::vector<LabelsPtr>& labels) {
  for (uint32_t i = 0; i < indexes.size(); ++i) {
    if (labels[i]->size() == 0) {
      std::cout << "Warning: 0 labels?" << std::endl;
      continue;
    }

    if (pointsCache_.find(indexes[i]) == pointsCache_.end()) {
      std::cout << "Warning: labels of non cached points?" << std::endl;
      continue;
    }

    if (labels[i]->size() != pointsCache_[indexes[i]]->size()) {
      std::cout << "Warning: inconsistent numbers of labels for given point cloud!" << std::endl;

      continue;
    }

    if (label_filenames_.size() < indexes[i]) {
      std::cout << "Warning: wrong index?" << std::endl;

      continue;
    }

    std::ofstream out(label_filenames_[indexes[i]].c_str());
    out.write((const char*)&(*labels[i])[0], labels[i]->size() * sizeof(uint32_t));
    out.close();
  }
}

void KittiReader::readPoints(const std::string& filename, Laserscan& scan) {
  std::ifstream in(filename.c_str(), std::ios::binary);
  if (!in.is_open()) return;

  scan.clear();

  in.seekg(0, std::ios::end);
  uint32_t num_points = in.tellg() / (4 * sizeof(float));
  in.seekg(0, std::ios::beg);

  std::vector<float> values(4 * num_points);
  in.read((char*)&values[0], 4 * num_points * sizeof(float));

  in.close();
  std::vector<Point3f>& points = scan.points;
  std::vector<float>& remissions = scan.remissions;

  points.resize(num_points);
  remissions.resize(num_points);

  for (uint32_t i = 0; i < num_points; ++i) {
    points[i].x = values[4 * i];
    points[i].y = values[4 * i + 1];
    points[i].z = values[4 * i + 2];
    remissions[i] = values[4 * i + 3];
  }
}

void KittiReader::readLabels(const std::string& filename, std::vector<uint32_t>& labels) {
  std::ifstream in(filename.c_str(), std::ios::binary);
  if (!in.is_open()) {
    std::cerr << "Unable to open label file. " << std::endl;
    return;
  }

  labels.clear();

  in.seekg(0, std::ios::end);
  uint32_t num_points = in.tellg() / (sizeof(uint32_t));
  in.seekg(0, std::ios::beg);

  labels.resize(num_points);
  in.read((char*)&labels[0], num_points * sizeof(uint32_t));

  in.close();
}

void KittiReader::readPoses(const std::string& filename, std::vector<Eigen::Matrix4f>& poses) {
  poses = KITTI::Odometry::loadPoses(filename);

  // convert from camera to velodyne coordinate system.
  #if 0
  Eigen::Matrix4f Tr = calib_["Tr"];
  Eigen::Matrix4f Tr_inv = Tr.inverse();
  for (uint32_t i = 0; i < poses.size(); ++i) {
    poses[i] = Tr_inv * poses[i] * Tr;
  }
  #endif
}


text:
/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostBreadthFirstSearchTree.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/
#include "vtkBoostBreadthFirstSearchTree.h"

#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkMath.h"
#include "vtkIdTypeArray.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkPoints.h"
#include "vtkFloatArray.h"
#include "vtkDataArray.h"
#include "vtkSmartPointer.h"
#include "vtkStringArray.h"

#include "vtkBoostGraphAdapter.h"
#include "vtkMutableDirectedGraph.h"
#include "vtkUndirectedGraph.h"
#include "vtkTree.h"

#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/pending/queue.hpp>

using namespace boost;

vtkStandardNewMacro(vtkBoostBreadthFirstSearchTree);

#if BOOST_VERSION >= 104800      // Boost 1.48.x
namespace {
  vtkIdType unwrap_edge_id(vtkEdgeType const &e)
  {
    return e.Id;
  }
  vtkIdType unwrap_edge_id(boost::detail::reverse_graph_edge_descriptor<vtkEdgeType> const &e)
  {
# if BOOST_VERSION == 104800
    return e.underlying_desc.Id;
# else
    return e.underlying_descx.Id;
# endif
  }
}
#endif

// Redefine the bfs visitor, the only visitor we
// are using is the tree_edge visitor.
template <typename IdMap>
class bfs_tree_builder : public default_bfs_visitor
{
public:
  bfs_tree_builder()
  { }

  bfs_tree_builder(IdMap& g2t, IdMap& t2g, vtkGraph* g, vtkMutableDirectedGraph* t, vtkIdType root)
    : graph_to_tree(g2t), tree_to_graph(t2g), tree(t), graph(g)
  {
    double x[3];
    graph->GetPoints()->GetPoint(root, x);
    tree->GetPoints()->InsertNextPoint(x);
    vtkIdType tree_root = t->AddVertex();
    put(graph_to_tree, root, tree_root);
    put(tree_to_graph, tree_root, root);
    tree->GetVertexData()->CopyData(graph->GetVertexData(), root, tree_root);
  }

  template <typename Edge, typename Graph>
  void tree_edge(Edge e, const Graph& g) const
  {
    typename graph_traits<Graph>::vertex_descriptor u, v;
    u = source(e, g);
    v = target(e, g);

    // Get the source vertex id (it has already been visited).
    vtkIdType tree_u = get(graph_to_tree, u);

    // Add the point before the vertex so that
    // points match the number of vertices, so that GetPoints()
    // doesn't reallocate and zero-out points.
    double x[3];
    graph->GetPoints()->GetPoint(v, x);
    tree->GetPoints()->InsertNextPoint(x);

    // Create the target vertex in the tree.
    vtkIdType tree_v = tree->AddVertex();
    vtkEdgeType tree_e = tree->AddEdge(tree_u, tree_v);

    // Store the mapping from graph to tree.
    put(graph_to_tree, v, tree_v);
    put(tree_to_graph, tree_v, v);

    // Copy the vertex and edge data from the graph to the tree.
    tree->GetVertexData()->CopyData(graph->GetVertexData(), v, tree_v);
#if BOOST_VERSION < 104800      // Boost 1.48.x
    tree->GetEdgeData()->CopyData(graph->GetEdgeData(), e.Id, tree_e.Id);
#else
    tree->GetEdgeData()->CopyData(graph->GetEdgeData(),
                                  unwrap_edge_id(e), tree_e.Id);
#endif
  }

private:
  IdMap graph_to_tree;
  IdMap tree_to_graph;
  vtkMutableDirectedGraph *tree;
  vtkGraph *graph;
};

// Constructor/Destructor
vtkBoostBreadthFirstSearchTree::vtkBoostBreadthFirstSearchTree()
{
  // Default values for the origin vertex
  this->OriginVertexIndex = 0;
  this->ArrayName = 0;
  this->SetArrayName("Not Set");
  this->ArrayNameSet = false;
  this->OriginValue = 0;
  this->CreateGraphVertexIdArray = false;
  this->ReverseEdges = false;
}

vtkBoostBreadthFirstSearchTree::~vtkBoostBreadthFirstSearchTree()
{
  this->SetArrayName(NULL);
}

// Description:
// Set the index (into the vertex array) of the
// breadth first search 'origin' vertex.
void vtkBoostBreadthFirstSearchTree::SetOriginVertex(vtkIdType index)
{
  this->OriginVertexIndex = index;
  this->ArrayNameSet = false;
  this->Modified();
}

// Description:
// Set the breadth first search 'origin' vertex.
// This method is basically the same as above
// but allows the application to simply specify
// an array name and value, instead of having to
// know the specific index of the vertex.
void vtkBoostBreadthFirstSearchTree::SetOriginVertex(
  vtkStdString arrayName, vtkVariant value)
{
  this->SetArrayName(arrayName);
  this->ArrayNameSet = true;
  this->OriginValue = value;
  this->Modified();
}

vtkIdType vtkBoostBreadthFirstSearchTree::GetVertexIndex(
  vtkAbstractArray *abstract,vtkVariant value)
{

  // Okay now what type of array is it
  if (abstract->IsNumeric())
    {
    vtkDataArray *dataArray = vtkArrayDownCast<vtkDataArray>(abstract);
    int intValue = value.ToInt();
    for(int i=0; i<dataArray->GetNumberOfTuples(); ++i)
      {
      if (intValue == static_cast<int>(dataArray->GetTuple1(i)))
        {
        return i;
        }
      }
    }
  else
    {
    vtkStringArray *stringArray = vtkArrayDownCast<vtkStringArray>(abstract);
    vtkStdString stringValue(value.ToString());
    for(int i=0; i<stringArray->GetNumberOfTuples(); ++i)
      {
      if (stringValue == stringArray->GetValue(i))
        {
        return i;
        }
      }
    }

  // Failed
  vtkErrorMacro("Did not find a valid vertex index...");
  return 0;
}

int vtkBoostBreadthFirstSearchTree::FillInputPortInformation(
  int vtkNotUsed(port), vtkInformation* info)
{
  info->Set(vtkAlgorithm::INPUT_REQUIRED_DATA_TYPE(), "vtkGraph");
  return 1;
}

int vtkBoostBreadthFirstSearchTree::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkGraph *input = vtkGraph::SafeDownCast(
    inInfo->Get(vtkDataObject::DATA_OBJECT()));

  // Now figure out the origin vertex of the
  // breadth first search
  if (this->ArrayNameSet)
    {
    vtkAbstractArray* abstract = input->GetVertexData()->GetAbstractArray(this->ArrayName);

    // Does the array exist at all?
    if (abstract == NULL)
      {
      vtkErrorMacro("Could not find array named " << this->ArrayName);
      return 0;
      }

    this->OriginVertexIndex = this->GetVertexIndex(abstract,this->OriginValue);
   }

  // Create tree to graph id map array
  vtkIdTypeArray* treeToGraphIdMap = vtkIdTypeArray::New();

  // Create graph to tree id map array
  vtkIdTypeArray* graphToTreeIdMap = vtkIdTypeArray::New();

  // Create a color map (used for marking visited nodes)
  vector_property_map<default_color_type> color;

  // Create a queue to hand off to the BFS
  queue<int> q;

  // Create the mutable graph to build the tree
  vtkSmartPointer<vtkMutableDirectedGraph> temp =
    vtkSmartPointer<vtkMutableDirectedGraph>::New();
  // Initialize copying data into tree
  temp->GetFieldData()->PassData(input->GetFieldData());
  temp->GetVertexData()->CopyAllocate(input->GetVertexData());
  temp->GetEdgeData()->CopyAllocate(input->GetEdgeData());

  // Create the visitor which will build the tree
  bfs_tree_builder<vtkIdTypeArray*> builder(
      graphToTreeIdMap, treeToGraphIdMap, input, temp, this->OriginVertexIndex);

  // Run the algorithm
  if (vtkDirectedGraph::SafeDownCast(input))
    {
    vtkDirectedGraph *g = vtkDirectedGraph::SafeDownCast(input);
    if (this->ReverseEdges)
      {
#if BOOST_VERSION < 104100      // Boost 1.41.x
      vtkErrorMacro("ReverseEdges requires Boost 1.41.x or higher");
      return 0;
#else
      boost::reverse_graph<vtkDirectedGraph*> r(g);
      breadth_first_search(r, this->OriginVertexIndex, q, builder, color);
#endif
      }
    else
      {
      breadth_first_search(g, this->OriginVertexIndex, q, builder, color);
      }
    }
  else
    {
    vtkUndirectedGraph *g = vtkUndirectedGraph::SafeDownCast(input);
    breadth_first_search(g, this->OriginVertexIndex, q, builder, color);
    }

  // If the user wants it, store the mapping back to graph vertices
  if (this->CreateGraphVertexIdArray)
    {
    treeToGraphIdMap->SetName("GraphVertexId");
    temp->GetVertexData()->AddArray(treeToGraphIdMap);
    }

  // Copy the builder graph structure into the output tree
  vtkTree *output = vtkTree::SafeDownCast(
    outInfo->Get(vtkDataObject::DATA_OBJECT()));
  if (!output->CheckedShallowCopy(temp))
    {
    vtkErrorMacro(<<"Invalid tree.");
    return 0;
    }

  // Clean up
  output->Squeeze();
  graphToTreeIdMap->Delete();
  treeToGraphIdMap->Delete();

  return 1;
}

void vtkBoostBreadthFirstSearchTree::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "OriginVertexIndex: " << this->OriginVertexIndex << endl;

  os << indent << "ArrayName: "
     << (this->ArrayName ? this->ArrayName : "(none)") << endl;

  os << indent << "OriginValue: " << this->OriginValue.ToString() << endl;

  os << indent << "ArrayNameSet: "
     << (this->ArrayNameSet ? "true" : "false") << endl;

  os << indent << "CreateGraphVertexIdArray: "
     << (this->CreateGraphVertexIdArray ? "on" : "off") << endl;

  os << indent << "ReverseEdges: "
     << (this->ReverseEdges ? "on" : "off") << endl;
}

max_stars_repo_name: keithroe/vtkoptix

max_issues_repo_name: keithroe/vtkoptix

max_forks_repo_name: keithroe/vtkoptix


text:
/*
 *  Copyright 2015 Aldebaran. All rights reserved.
 *
 */
#include <almath/scenegraph/urdfrigidbodysystemtraverser.h>
#include <almath/scenegraph/urdfeigen.h>
#include <boost/optional.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

namespace Builder = AL::RigidBodySystemBuilder;
typedef Builder::Interface<double>::Pose Pose;
typedef Builder::Interface<double>::BodyMass BodyMass;

namespace AL {

static const urdf::Array3d unit_x = {{1., 0., 0.}};
static const urdf::Array3d unit_y = {{0., 1., 0.}};
static const urdf::Array3d unit_z = {{0., 0., 1.}};

class UrdfRigidBodySystemTraverser : public urdf::RobotTree::JointConstVisitor {
 public:
  UrdfRigidBodySystemTraverser(Builder::Interface<double> &builder,
                               const urdf::RobotTree &parser)
      : builder(builder), parser(parser), removed_root_link(false) {}

  bool _discover(const std::string &parent_link_name,
                 const std::string &link_name, const Pose &origin,
                 Builder::JointType type, const urdf::ptree &link_pt,
                 const boost::optional<std::string> &joint_name);
  bool discover(const urdf::ptree &joint_el);

 private:
  Builder::Interface<double> &builder;
  const urdf::RobotTree &parser;
  boost::optional<std::string> urdf_root_link;
  bool removed_root_link;
};

bool UrdfRigidBodySystemTraverser::_discover(
    const std::string &parent_link_name, const std::string &link_name,
    const Pose &origin, Builder::JointType type, const urdf::ptree &link_pt,
    const boost::optional<std::string> &joint_name =
        boost::optional<std::string>()) {
  urdf::Link link(link_pt);
  if (!link.inertial()) return false;  // stop exploring this branch
  BodyMass mass;
  Math::urdfInertialToEigen(*(link.inertial()), mass.mass, mass.center_of_mass,
                            mass.rotational_inertia);
  if (joint_name)
    builder.add(parent_link_name, link_name, origin, type, mass, *joint_name);
  else
    builder.add(parent_link_name, link_name, origin, type, mass);
  return true;
}

bool UrdfRigidBodySystemTraverser::discover(const urdf::ptree &joint_el) {
  urdf::Joint joint(joint_el);
  Builder::JointType type;
  switch (joint.type()) {
    case urdf::Joint::fixed: {
      builder.add(joint.parent_link(), joint.child_link(),
                  Math::toEigenTransform(joint.origin()), joint.name());
      return true;
    }
    case urdf::Joint::prismatic:
    case urdf::Joint::planar:
      throw std::runtime_error("unsupported joint type");
    case urdf::Joint::revolute:
    case urdf::Joint::continuous: {
      const urdf::Array3d axis = joint.axis();
      if (axis == unit_x) {
        type = Builder::JointType::Rx;
        break;
      }
      if (axis == unit_y) {
        type = Builder::JointType::Ry;
        break;
      }
      if (axis == unit_z) {
        type = Builder::JointType::Rz;
        break;
      }
      throw std::runtime_error("unsupported joint axis");
    }
    case urdf::Joint::floating: {
      type = Builder::JointType::FreeFlyer;
      break;
    }
  }
  _discover(joint.parent_link(), joint.child_link(),
            Math::toEigenTransform(joint.origin()), type,
            parser.link(joint.child_link()), joint.name());
  return true;
}

void buildRigidBodySystemFromUrdf(
    RigidBodySystemBuilder::Interface<double> &builder, urdf::ptree &pt,
    bool remove_root_joint, bool make_continuous_joints_fixed) {
  urdf::RobotTree parser(pt.get_child("robot"));
  if (remove_root_joint) parser.rm_root_joint();
  if (make_continuous_joints_fixed) makeContinuousJointsFixed(parser);
  makeMasslessJointsFixed(parser);
  UrdfRigidBodySystemTraverser visitor(builder, parser);
  visitor._discover("", parser.root_link(), Pose::Identity(),
                    Builder::JointType::FreeFlyer,
                    parser.link(parser.root_link()));
  parser.traverse_joints(visitor);
}

void buildRigidBodySystemFromUrdf(
    RigidBodySystemBuilder::Interface<double> &builder, std::istream &is,
    bool remove_root_joint, bool make_continuous_joints_fixed) {
  AL::urdf::ptree pt;
  boost::property_tree::xml_parser::read_xml(is, pt);
  buildRigidBodySystemFromUrdf(builder, pt, remove_root_joint,
                               make_continuous_joints_fixed);
}
}

max_stars_repo_name: UCCS-Social-Robotics/libalmath

max_issues_repo_name: UCCS-Social-Robotics/libalmath

max_forks_repo_name: UCCS-Social-Robotics/libalmath