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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <mitkImageCast.h>
#include <mitkImage.h>
#include <mitkIOUtil.h>
#include "mitkCommandLineParser.h"
#include <mitkDiffusionPropertyHelper.h>
#include <itksys/SystemTools.hxx>
#include <itkB0ImageExtractionToSeparateImageFilter.h>
#include <mitkIPythonService.h>
#include <mitkImageCast.h>

#include <usGetModuleContext.h>
#include <usModuleContext.h>
#include <usModule.h>
#include <usModuleResource.h>
#include <usModuleResourceStream.h>
#include <BetData.h>
#include <mitkPreferenceListReaderOptionsFunctor.h>

#include <boost/algorithm/string.hpp>

typedef mitk::DiffusionPropertyHelper DPH;
typedef itksys::SystemTools ist;

std::string GetPythonFile(std::string filename, std::string exec_dir)
{
  std::string out = "";
  for (auto dir : mitk::bet::relative_search_dirs)
  {
    if ( ist::FileExists( exec_dir + dir + filename) )
    {
      out = exec_dir + dir + filename;
      return out;
    }
    if ( ist::FileExists( ist::GetCurrentWorkingDirectory() + dir + filename) )
    {
      out = ist::GetCurrentWorkingDirectory() + dir + filename;
      return out;
    }
  }
  for (auto dir : mitk::bet::absolute_search_dirs)
  {
    if ( ist::FileExists( dir + filename) )
    {
      out = dir + filename;
      return out;
    }
  }

  return out;
}

int main(int argc, char* argv[])
{
  mitkCommandLineParser parser;

  parser.setTitle("BrainExtraction");
  parser.setCategory("Preprocessing Tools");
  parser.setDescription("Performs brain extraction using a deep learning model");
  parser.setContributor("MIC");

  parser.setArgumentPrefix("--", "-");
  parser.addArgument("", "i", mitkCommandLineParser::InputFile, "Input:", "input image", us::Any(), false);
  parser.addArgument("", "o", mitkCommandLineParser::OutputFile, "Output:", "output root", us::Any(), false);

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  // mandatory arguments
  std::string i = us::any_cast<std::string>(parsedArgs["i"]);
  std::string o = us::any_cast<std::string>(parsedArgs["o"]);

  std::string exec_dir = ist::GetFilenamePath(argv[0]);
  mitk::PreferenceListReaderOptionsFunctor functor = mitk::PreferenceListReaderOptionsFunctor({"Diffusion Weighted Images"}, {});
  mitk::Image::Pointer mitk_image = mitk::IOUtil::Load<mitk::Image>(i, &functor);

  bool missing_file = false;
  std::string missing_file_string = "";
  if ( GetPythonFile("run_mitk.py", exec_dir).empty() )
  {
    missing_file_string += "Brain extraction script file missing: run_mitk.py\n\n";
    missing_file = true;
  }

  if ( GetPythonFile("model_final.model", exec_dir).empty() )
  {
    missing_file_string += "Brain extraction model file missing: model_final.model\n\n";
    missing_file = true;
  }

  if ( GetPythonFile("basic_config_just_like_braintumor.py", exec_dir).empty() )
  {
    missing_file_string += "Config file missing: basic_config_just_like_braintumor.py\n\n";
    missing_file = true;
  }

  if (missing_file)
  {
    mitkThrow() << missing_file_string;
  }

  us::ModuleContext* context = us::GetModuleContext();
  us::ServiceReference<mitk::IPythonService> m_PythonServiceRef = context->GetServiceReference<mitk::IPythonService>();
  mitk::IPythonService* m_PythonService = dynamic_cast<mitk::IPythonService*> ( context->GetService<mitk::IPythonService>(m_PythonServiceRef) );
  mitk::IPythonService::ForceLoadModule();

  // load essential modules
  m_PythonService->Execute("import SimpleITK as sitk");
  m_PythonService->Execute("import SimpleITK._SimpleITK as _SimpleITK");
  m_PythonService->Execute("import numpy");

  // extend python search path
  std::string pythonpath = "";
  for (auto dir : mitk::bet::relative_search_dirs)
    pythonpath += "','" + ist::GetCurrentWorkingDirectory() + dir;
  for (auto dir : mitk::bet::relative_search_dirs)
    pythonpath += "','" + exec_dir + dir;
  for (auto dir : mitk::bet::absolute_search_dirs)
    pythonpath += "','" + dir;
  m_PythonService->Execute("paths=['"+pythonpath+"']");

  // set input files (model and config)
  m_PythonService->Execute("model_file=\""+GetPythonFile("model_final.model", exec_dir)+"\"");
  m_PythonService->Execute("config_file=\""+GetPythonFile("basic_config_just_like_braintumor.py", exec_dir)+"\"");

  // copy input  image to python
  m_PythonService->CopyToPythonAsSimpleItkImage( mitk_image, "in_image");

  // run segmentation script
  m_PythonService->ExecuteScript( GetPythonFile("run_mitk.py", exec_dir) );

  // clean up after running script (better way than deleting individual variables?)
  if(m_PythonService->DoesVariableExist("in_image"))
    m_PythonService->Execute("del in_image");

  // check for errors
  if(!m_PythonService->GetVariable("error_string").empty())
    mitkThrow() << m_PythonService->GetVariable("error_string");

  // get output images and add to datastorage
  std::string output_variables = m_PythonService->GetVariable("output_variables");
  std::vector<std::string> outputs;
  boost::split(outputs, output_variables, boost::is_any_of(","));

  std::string output_types = m_PythonService->GetVariable("output_types");
  std::vector<std::string> types;
  boost::split(types, output_types, boost::is_any_of(","));

  for (unsigned int i=0; i<outputs.size(); ++i)
  {
    if (m_PythonService->DoesVariableExist(outputs.at(i)))
    {
      mitk::Image::Pointer image = m_PythonService->CopySimpleItkImageFromPython(outputs.at(i));

      if(types.at(i)=="input" && mitk::DiffusionPropertyHelper::IsDiffusionWeightedImage(mitk_image))
      {
        mitk::DiffusionPropertyHelper::CopyProperties(mitk_image, image, true);
        mitk::DiffusionPropertyHelper::InitializeImage(image);
      }

      mitk::DataNode::Pointer corrected_node = mitk::DataNode::New();
      corrected_node->SetData( image );
      std::string name = o + "_";
      name += outputs.at(i);

      if(types.at(i)=="input" && mitk::DiffusionPropertyHelper::IsDiffusionWeightedImage(mitk_image))
        mitk::IOUtil::Save(image, "application/vnd.mitk.nii.gz", name+".nii.gz");
      else
        mitk::IOUtil::Save(image, name+".nii.gz");
    }
  }

  MITK_INFO << "Finished brain extraction";
  return EXIT_SUCCESS;
}

ext: cpp

max_stars_repo_name: wyyrepo/MITK

max_issues_repo_name: wyyrepo/MITK

max_forks_repo_name: wyyrepo/MITK


text:
#ifndef DAQI_REDIS_VALUE_HPP
#define DAQI_REDIS_VALUE_HPP

#include <string>
#include <stack>
#include <vector>
#include <utility>

#include <boost/variant.hpp>

namespace da4qi4
{

class RedisValue
{
public:
    struct ErrorTag {};

    RedisValue();
    RedisValue(RedisValue&& other);
    RedisValue(int64_t i);
    RedisValue(const char* s);
    RedisValue(const std::string& s);
    RedisValue(std::vector<char> buf);
    RedisValue(std::vector<char> buf, struct ErrorTag);
    RedisValue(std::string const& custom_error, struct ErrorTag);
    RedisValue(std::vector<RedisValue> array);

    RedisValue(const RedisValue&) = default;
    RedisValue& operator = (const RedisValue&) = default;
    RedisValue& operator = (RedisValue&&) = default;

    // Return the value as a std::string if
    // type is a byte string; otherwise returns an empty std::string.
    std::string ToString() const;

    // Return the value as a std::vector<char> if
    // type is a byte string; otherwise returns an empty std::vector<char>.
    std::vector<char> ToByteArray() const;

    // Return the value as a std::vector<RedisValue> if
    // type is an int; otherwise returns 0.
    int64_t ToInt() const;

    int ToInt32() const
    {
        return static_cast<int>(ToInt());
    }

    // Return the value as an array if type is an array;
    // otherwise returns an empty array.
    std::vector<RedisValue> ToArray() const;

    // Return the string representation of the value. Use
    // for dump content of the value.
    std::string Inspect() const;

    // Return true if value not a error
    bool IsOk() const;
    // Return true if value is a error
    bool IsError() const;

    // Return true if this is a null.
    bool IsNull() const;
    // Return true if type is an int
    bool IsInt() const;
    // Return true if type is an array
    bool IsArray() const;
    // Return true if type is a string/byte array. Alias for IsString();
    bool IsByteArray() const;
    // Return true if type is a string/byte array. Alias for IsByteArray().
    bool IsString() const;

    // Methods for increasing perfomance
    // Throws: boost::bad_get if the type does not match
    std::vector<char>& GetByteArray();
    const std::vector<char>& GetByteArray() const;
    std::vector<RedisValue>& GetArray();
    const std::vector<RedisValue>& GetArray() const;

    bool operator == (const RedisValue& rhs) const;
    bool operator != (const RedisValue& rhs) const;

protected:
    template<typename T>
    T cast_to() const;

    template<typename T>
    bool type_eq() const;

private:
    struct NullTag
    {
        inline bool operator == (const NullTag&) const
        {
            return true;
        }
    };

    boost::variant<NullTag, int64_t, std::vector<char>, std::vector<RedisValue>> _value;
    bool _error;
};

template<typename T>
T RedisValue::cast_to() const
{
    return (_value.type() == typeid(T)) ? boost::get<T>(_value) : T();
}

template<typename T>
bool RedisValue::type_eq() const
{
    return (_value.type() == typeid(T));
}

} // namespace da4qi4

#endif // DAQI_REDIS_VALUE_HPP

ext: hpp


text:
/*
 * running.cpp
 *
 *  Created on: May 15, 2018
 *      Author: jasonr
 */

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

#include <iostream>
#include <boost/filesystem/operations.hpp>

#include "core/global.h"
#include "core/state.h"
#include "core/signals.h"

#include "gdb/result.h"
#include "gdb/controller.h"

namespace Gdb
{

namespace Handlers
{

Controller::HandlerReturn
running(const Result &result, int token, boost::any data)
{
    auto match = result.message.type == Message::Type::STRING &&
                 result.message.string.data == "running" &&
                 result.payload.type == Payload::Type::NONE &&
                 result.stream == Stream::STDOUT &&
                 result.type == Type::RESULT;

    /*
    {'message': u'running',
     'payload': None,
     'stream': 'stdout',
     'token': 6,
     'type': 'result'}
    */

    if (match)
    {
        Core::state()->setDebuggerState(Core::State::Debugger::RUNNING);
    }

    return {"running", match, Controller::MatchType::TOKEN};
}

}

}

max_stars_repo_name: etiam/db

max_issues_repo_name: etiam/db

max_forks_repo_name: etiam/db


text:
/** @file   mpi_search.cc
 *  @brief  Search with suppport for MPI
 *
 *  @author Kaloyan Petrov (kaloyan_petrov@mail.bg)
 *  @date   11 May 2019
 */

#define RR_MPI

#include <iostream>
#include <string>
#include <map>
#include <thread>
#include <unistd.h>

#define BOOST_LOG_DYN_LINK 1
#include <boost/log/trivial.hpp>
#include <boost/filesystem.hpp>
//#include <boost/serialization/string.hpp>
//#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/mpi.hpp>

#include "vise/relja_retrival.h"
#include "vise/util.h"

//#include "mpi_queue.h"
using namespace std;

enum tags{ TAG_RESULT=0, TAG_QUIT};

struct result{  //search result
  unsigned int row_id;
  unsigned int file_id;
  float score;
  double H[9];
  char filename[100];
  char metadata[100];

  friend class boost::serialization::access;

  template<class Archive>
  void serialize(Archive & ar, const unsigned int version)
  {
    ar & row_id;
    ar & file_id;
    ar & score;
    ar & H;
    ar & filename;
    ar & metadata;
  }
};
BOOST_IS_MPI_DATATYPE (result);

struct region_query{
  unsigned int file_id;
  unsigned int x,y,width,height;
};

static map<string, string> cfg;
static vector<struct region_query> queries;

static vise::relja_retrival * relja = NULL;

static bool se_qeury(const string& se_id, unsigned int ri, const float score_threshold, vector<struct result>& results) {

  struct region_query * r = &queries[ri];

  //result vectors
  std::vector<unsigned int> result_file_id;
  std::vector<float> result_score;
  std::vector< std::array<double, 9> > result_H;
  std::vector<std::string> result_filename;
  std::vector<std::string> result_metadata;

  //make the query
  relja->query_using_file_region(r->file_id, r->x, r->y, r->width, r->height, score_threshold,
    result_file_id, result_filename, result_metadata, result_score, result_H);

  //save result to row
  for ( std::size_t i=0; i < result_score.size(); ++i ) {

    if(result_file_id[i] == r->file_id){  //if result and row match, its the same file
      continue; //don't put it in results
    }

    struct result result;
    result.row_id   = ri;
    result.file_id  = result_file_id[i];

    strncpy(result.filename, result_filename[i].c_str(), 100);
    strncpy(result.metadata, result_metadata[i].c_str(), 100);
    result.filename[100] = 0;
    result.metadata[100] = 0;

    result.score    = result_score[i];
    memcpy(result.H, result_H[i].data(), 9*sizeof(double));

    results.push_back(result);
  }

  return true;
}

static bool load_config(map<string,string>& cfg) {

  char * config_filename = getenv("CONFIG_FILENAME");
  if(config_filename == NULL){
    cerr << "Error: CONFIG_FILENAME not set" << endl;
    return false;
  }

  ifstream ifs = ifstream(config_filename, std::ifstream::in);
  if(!ifs.is_open()){
    cerr << "Error: Can't open config %s" << config_filename << endl;
    return false;
  }

  string line;
  while (getline(ifs, line)) {  //read each line from config
    if(line[0] == '['){
      cfg[ "se_id" ] = line.substr(1, line.size()-2);
    }else{
      size_t pos = line.find('=');
      if(pos == string::npos){
        ifs.close();
        return false;
      }
      string key = line.substr(0, pos);
      string val = line.substr(pos+1);

      cfg[key] = val;
    }
  }
  ifs.close();

  if (cfg.find("data_dir") == cfg.end()){
    cerr << "Error: data_dir not defined" << endl;
    return false;
  }

  return true;
}

static bool load_queries(const char * image_coordinates_fn, vector<struct region_query>& queries){

  ifstream ifs = ifstream(image_coordinates_fn, std::ifstream::in);
  if(!ifs.is_open()){
    cerr << "Error: Can't open config %s" << image_coordinates_fn << endl;
    return false;
  }

  string line;
  while (getline(ifs, line)){  //read each line from config

    if(line.back() = '\r'){
      line.pop_back();
    }

    vector<string> columns = vise::util::split(line, ',');

    if(columns[0].compare("Filename") != 0){  //if we have a header

      struct region_query q;
      q.file_id   = relja->get_file_id(columns[0]);
      q.x         = stoul(columns[1]);
      q.y         = stoul(columns[2]);
      q.width     = stoul(columns[3]);
      q.height    = stoul(columns[4]);

      queries.push_back(q);
    }
  }
  ifs.close();

  return true;
}

static void print_row(ofstream& ofs, struct result* result){
  struct region_query * query  = &queries[result->row_id];
  ofs << relja->get_filename(query->file_id) << "," << query->x << "," << query->y << "," << query->width << "," << query->height << ","
      << result->file_id << "," << result->filename << "," << result->metadata << "," << result->score << ","
      << result->H[0] << "," << result->H[1] << "," << result->H[2] << ","
      << result->H[3] << "," << result->H[4] << "," << result->H[5] << ","
      << result->H[6] << "," << result->H[7] << "," << result->H[8]
      << endl;
}

static bool rejla_load_timeout(int tout){

  while(!relja->is_loaded() && (tout-- > 0)){
    sleep(1);
  }
  return relja->is_loaded();
}

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

  MPI_INIT_ENV
  MPI_GLOBAL_ALL

  if(argc != 3){
    if(rank == 0){
      cerr << "Usage: " << argv[0] << " ImageCoordinates.csv 50.0" << endl;
    }
    boost::mpi::environment::abort(1);
  }
  int buf[2];
  const int root = (numProc - 1);

  if(numProc < 2){
    cerr << "Error: At least two nodes are required!" << endl;
    boost::mpi::environment::abort(1);
  }

  const char * image_coordinates_fn = argv[1];
  const float threshold = atof(argv[2]);

  if( !load_config(cfg)){
    if(rank == root){
      cerr << "Error: Failed to load config" << endl;
    }
    boost::mpi::environment::abort(2);
  }

  boost::filesystem::path data_path{cfg["data_dir"]};
  boost::filesystem::path asset_path{cfg["asset_dir"]};
  boost::filesystem::path temp_path{cfg["temp_dir"]};

  relja = new vise::relja_retrival(cfg["se_id"], data_path, asset_path, temp_path);
  if(relja == NULL){
    if(rank == root){
      cerr << "Error: relja_retrival new failed" << endl;
    }
    boost::mpi::environment::abort(3);
  }

  if(!rejla_load_timeout(15)){
    if(rank == root){
      cerr << "Error: relja load failed" << endl;
    }
    boost::mpi::environment::abort(5);
  }

  if(!load_queries(image_coordinates_fn, queries)){
    return EXIT_FAILURE;
  }
  if(rank == root){
    cout << "Found " << queries.size() << " queries."<< endl;
  }

  unsigned int chunk_size = queries.size() / (numProc - 1);  //-1 because root only saves results

  //query start,end index
  unsigned int start = rank * chunk_size, end = start + chunk_size;
  if(rank == (root-1)){
    end += queries.size() % (root-1);
  }

  if(rank == root){  //only root saves results

    //append the threshold parameter to filename
    const string output_filename = "output_" + to_string(threshold) + ".csv";
    ofstream ofs = ofstream(output_filename, ofstream::out);
    if(!ofs.is_open()){
      cerr << "Error: Failed to open " << output_filename << endl;
      boost::mpi::environment::abort(4);
    }
    //show the column labels
    ofs << "#filename,x,y,width,height,file_id_result,filename2,metadata,score,H0,H1,H2,H3,H4,H5,H6,H7,H8" << endl;

    int num_searching = numProc - 1;  //count of searching nodes

    struct result result;
    while(num_searching > 0){

      boost::mpi::status st = comm.recv(boost::mpi::any_source, boost::mpi::any_tag, result);

      if(st.tag() == TAG_QUIT){  //worked sent exit marker
        num_searching --;
        cout << "Node " << st.source() << " finished" << endl;
        continue;
      }

      //cout << "Node " << st.source() << " found " << results.size() << " results" << endl;
      print_row(ofs, &result); //save result to output file
      result.clear();
    }
    ofs.close();

  }else{  //workers do the search

  cout << "Node " << rank << " queries[" << start << "," << end << "]" << endl;

#pragma omp parallel
{
  vector<struct result> results;

#pragma omp for schedule(dynamic)
  for(unsigned int i=start; i < end; ++i){
    se_qeury(cfg["se_id"], i, threshold, results); //search

    if(results.size() > 0){
      #pragma omp critical
      {
        for(unsigned int j=0; j < results.size(); j++){
          comm.send(root, TAG_RESULT, results[j]);
        }
      }
      results.clear();
    }
  }
}

  struct result result;
  comm.send(root, TAG_QUIT, result);

} //end else

  delete relja;

  return EXIT_SUCCESS;
}

ext: cc


text:
//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2008-2012. Distributed under the Boost
// Software License, Version 1.0. (See accompanying file
// LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/interprocess for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_INTERPROCESS_DETAIL_VARIADIC_TEMPLATES_TOOLS_HPP
#define BOOST_INTERPROCESS_DETAIL_VARIADIC_TEMPLATES_TOOLS_HPP

#ifndef BOOST_CONFIG_HPP
#  include <boost/config.hpp>
#endif
#
#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

#include <boost/container/detail/variadic_templates_tools.hpp>

namespace boost {
namespace interprocess {
namespace ipcdetail {

using boost::container::container_detail::tuple;
using boost::container::container_detail::build_number_seq;
using boost::container::container_detail::index_tuple;
using boost::container::container_detail::get;

}}}   //namespace boost { namespace interprocess { namespace ipcdetail {

#endif   //#ifndef BOOST_INTERPROCESS_DETAIL_VARIADIC_TEMPLATES_TOOLS_HPP

max_stars_repo_name: rudylee/expo

max_issues_repo_name: rudylee/expo

max_forks_repo_name: rudylee/expo


text:
/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */

#define BOOST_TEST_MODULE FCL_DISTANCE
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <boost/timer.hpp>
#include <boost/filesystem.hpp>

#include <hpp/fcl/internal/traversal_node_bvhs.h>
#include <hpp/fcl/internal/traversal_node_setup.h>
#include "../src/collision_node.h"
#include <hpp/fcl/internal/BV_splitter.h>

#include "utility.h"
#include "fcl_resources/config.h"

using namespace hpp::fcl;
namespace utf = boost::unit_test::framework;

bool verbose = false;
FCL_REAL DELTA = 0.001;

template<typename BV>
void distance_Test(const Transform3f& tf,
                   const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                   const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
                   int qsize,
                   DistanceRes& distance_result,
                   bool verbose = true);

bool collide_Test_OBB(const Transform3f& tf,
                      const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                      const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose);

template<typename BV, typename TraversalNode>
void distance_Test_Oriented(const Transform3f& tf,
                            const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                            const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
                            int qsize,
                            DistanceRes& distance_result,
                            bool verbose = true);

inline bool nearlyEqual(const Vec3f& a, const Vec3f& b)
{
  if(fabs(a[0] - b[0]) > DELTA) return false;
  if(fabs(a[1] - b[1]) > DELTA) return false;
  if(fabs(a[2] - b[2]) > DELTA) return false;
  return true;
}

BOOST_AUTO_TEST_CASE(mesh_distance)
{
  std::vector<Vec3f> p1, p2;
  std::vector<Triangle> t1, t2;
  boost::filesystem::path path(TEST_RESOURCES_DIR);
  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);

  std::vector<Transform3f> transforms; // t0
  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
#ifndef NDEBUG // if debug mode
  std::size_t n = 1;
#else
  std::size_t n = 10;
#endif
  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);

  generateRandomTransforms(extents, transforms, n);

  double dis_time = 0;
  double col_time = 0;

  DistanceRes res, res_now;
  for(std::size_t i = 0; i < transforms.size(); ++i)
  {
    boost::timer timer_col;
    collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
    col_time += timer_col.elapsed();

    boost::timer timer_dist;
    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose);
    dis_time += timer_dist.elapsed();

    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);

    BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
    BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));

  }

  BOOST_TEST_MESSAGE("distance timing: " << dis_time << " sec");
  BOOST_TEST_MESSAGE("collision timing: " << col_time << " sec");
}

template<typename BV, typename TraversalNode>
void distance_Test_Oriented(const Transform3f& tf,
                            const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                            const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
                            int qsize,
                            DistanceRes& distance_result,
                            bool verbose)
{
  BVHModel<BV> m1;
  BVHModel<BV> m2;
  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));

  m1.beginModel();
  m1.addSubModel(vertices1, triangles1);
  m1.endModel();

  m2.beginModel();
  m2.addSubModel(vertices2, triangles2);
  m2.endModel();

  DistanceResult local_result;
  TraversalNode node;
  if(!initialize(node, (const BVHModel<BV>&)m1, tf, (const BVHModel<BV>&)m2, Transform3f(), DistanceRequest(true), local_result))
    std::cout << "initialize error" << std::endl;

  node.enable_statistics = verbose;

  distance(&node, NULL, qsize);

  // points are in local coordinate, to global coordinate
  Vec3f p1 = local_result.nearest_points[0];
  Vec3f p2 = local_result.nearest_points[1];

  distance_result.distance = local_result.min_distance;
  distance_result.p1 = p1;
  distance_result.p2 = p2;

  if(verbose)
  {
    std::cout << "distance " << local_result.min_distance << std::endl;

    std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl;
    std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl;
    std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
  }
}

template<typename BV>
void distance_Test(const Transform3f& tf,
                   const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                   const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
                   int qsize,
                   DistanceRes& distance_result,
                   bool verbose)
{
  BVHModel<BV> m1;
  BVHModel<BV> m2;
  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));

  m1.beginModel();
  m1.addSubModel(vertices1, triangles1);
  m1.endModel();

  m2.beginModel();
  m2.addSubModel(vertices2, triangles2);
  m2.endModel();

  Transform3f pose1(tf), pose2;

  DistanceResult local_result;
  MeshDistanceTraversalNode<BV> node;

  if(!initialize<BV>(node, m1, pose1, m2, pose2, DistanceRequest(true), local_result))
    std::cout << "initialize error" << std::endl;

  node.enable_statistics = verbose;

  distance(&node, NULL, qsize);

  distance_result.distance = local_result.min_distance;
  distance_result.p1 = local_result.nearest_points[0];
  distance_result.p2 = local_result.nearest_points[1];

  if(verbose)
  {
    std::cout << "distance " << local_result.min_distance << std::endl;

    std::cout << local_result.nearest_points[0][0] << " " << local_result.nearest_points[0][1] << " " << local_result.nearest_points[0][2] << std::endl;
    std::cout << local_result.nearest_points[1][0] << " " << local_result.nearest_points[1][1] << " " << local_result.nearest_points[1][2] << std::endl;
    std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
  }
}

bool collide_Test_OBB(const Transform3f& tf,
                      const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                      const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
{
  BVHModel<OBB> m1;
  BVHModel<OBB> m2;
  m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
  m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));

  m1.beginModel();
  m1.addSubModel(vertices1, triangles1);
  m1.endModel();

  m2.beginModel();
  m2.addSubModel(vertices2, triangles2);
  m2.endModel();

  CollisionResult local_result;
  CollisionRequest request (CONTACT | DISTANCE_LOWER_BOUND, 1);
  MeshCollisionTraversalNodeOBB node (request);
  bool success (initialize(node, (const BVHModel<OBB>&)m1, tf,
                           (const BVHModel<OBB>&)m2, Transform3f(),
                           local_result));
  BOOST_REQUIRE (success);

  node.enable_statistics = verbose;

  collide(&node, request, local_result);

  if(local_result.numContacts() > 0)
    return true;
  else
    return false;
}

max_stars_repo_name: rstrudel/hpp-fcl

max_issues_repo_name: rstrudel/hpp-fcl

max_forks_repo_name: rstrudel/hpp-fcl


text:
//  (C) Copyright Boost.org 1999. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

// Test file for config setup
// This file should compile, if it does not then
// one or more macros need to be defined.
// see boost_*.cxx for more details

// Do not edit this file, it was generated automatically by
// ../tools/generate from boost_*.cxx on
// Thu Feb 13 12:10:01 GMTST 2003

#include <boost/config.hpp>
#define BOOST_INCLUDE_MAIN
#include <boost/test/test_tools.hpp>
#include "test.hpp"

#ifndef BOOST_NO_ARGUMENT_DEPENDENT_LOOKUP
#include "boost_no_arg_dep_lookup.cxx"
#else
namespace boost_no_argument_dependent_lookup = empty_boost;
#endif
#ifndef BOOST_NO_AUTO_PTR
#include "boost_no_auto_ptr.cxx"
#else
namespace boost_no_auto_ptr = empty_boost;
#endif
#ifndef BOOST_BCB_PARTIAL_SPECIALIZATION_BUG
#include "boost_no_bcb_partial_spec.cxx"
#else
namespace boost_bcb_partial_specialization_bug = empty_boost;
#endif
#ifndef BOOST_NO_CTYPE_FUNCTIONS
#include "boost_no_ctype_functions.cxx"
#else
namespace boost_no_ctype_functions = empty_boost;
#endif
#ifndef BOOST_NO_CV_SPECIALIZATIONS
#include "boost_no_cv_spec.cxx"
#else
namespace boost_no_cv_specializations = empty_boost;
#endif
#ifndef BOOST_NO_CV_VOID_SPECIALIZATIONS
#include "boost_no_cv_void_spec.cxx"
#else
namespace boost_no_cv_void_specializations = empty_boost;
#endif
#ifndef BOOST_NO_CWCHAR
#include "boost_no_cwchar.cxx"
#else
namespace boost_no_cwchar = empty_boost;
#endif
#ifndef BOOST_NO_CWCTYPE
#include "boost_no_cwctype.cxx"
#else
namespace boost_no_cwctype = empty_boost;
#endif
#ifndef BOOST_DEDUCED_TYPENAME
#include "boost_no_ded_typename.cxx"
#else
namespace boost_deduced_typename = empty_boost;
#endif
#ifndef BOOST_NO_DEPENDENT_NESTED_DERIVATIONS
#include "boost_no_dep_nested_class.cxx"
#else
namespace boost_no_dependent_nested_derivations = empty_boost;
#endif
#ifndef BOOST_NO_DEPENDENT_TYPES_IN_TEMPLATE_VALUE_PARAMETERS
#include "boost_no_dep_val_param.cxx"
#else
namespace boost_no_dependent_types_in_template_value_parameters = empty_boost;
#endif
#ifndef BOOST_NO_EXCEPTION_STD_NAMESPACE
#include "boost_no_excep_std.cxx"
#else
namespace boost_no_exception_std_namespace = empty_boost;
#endif
#ifndef BOOST_NO_EXCEPTIONS
#include "boost_no_exceptions.cxx"
#else
namespace boost_no_exceptions = empty_boost;
#endif
#ifndef BOOST_NO_EXPLICIT_FUNCTION_TEMPLATE_ARGUMENTS
#include "boost_no_exp_func_tem_arg.cxx"
#else
namespace boost_no_explicit_function_template_arguments = empty_boost;
#endif
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
#include "boost_no_func_tmp_order.cxx"
#else
namespace boost_no_function_template_ordering = empty_boost;
#endif
#ifndef BOOST_NO_MS_INT64_NUMERIC_LIMITS
#include "boost_no_i64_limits.cxx"
#else
namespace boost_no_ms_int64_numeric_limits = empty_boost;
#endif
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
#include "boost_no_inline_memb_init.cxx"
#else
namespace boost_no_inclass_member_initialization = empty_boost;
#endif
#ifndef BOOST_NO_INTEGRAL_INT64_T
#include "boost_no_integral_int64_t.cxx"
#else
namespace boost_no_integral_int64_t = empty_boost;
#endif
#ifndef BOOST_NO_TEMPLATED_ITERATOR_CONSTRUCTORS
#include "boost_no_iter_construct.cxx"
#else
namespace boost_no_templated_iterator_constructors = empty_boost;
#endif
#ifndef BOOST_NO_LIMITS
#include "boost_no_limits.cxx"
#else
namespace boost_no_limits = empty_boost;
#endif
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#include "boost_no_limits_const_exp.cxx"
#else
namespace boost_no_limits_compile_time_constants = empty_boost;
#endif
#ifndef BOOST_NO_LONG_LONG_NUMERIC_LIMITS
#include "boost_no_ll_limits.cxx"
#else
namespace boost_no_long_long_numeric_limits = empty_boost;
#endif
#ifndef BOOST_NO_MEMBER_FUNCTION_SPECIALIZATIONS
#include "boost_no_mem_func_spec.cxx"
#else
namespace boost_no_member_function_specializations = empty_boost;
#endif
#ifndef BOOST_NO_MEMBER_TEMPLATE_KEYWORD
#include "boost_no_mem_tem_keyword.cxx"
#else
namespace boost_no_member_template_keyword = empty_boost;
#endif
#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
#include "boost_no_mem_templ_frnds.cxx"
#else
namespace boost_no_member_template_friends = empty_boost;
#endif
#ifndef BOOST_NO_MEMBER_TEMPLATES
#include "boost_no_mem_templates.cxx"
#else
namespace boost_no_member_templates = empty_boost;
#endif
#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
#include "boost_no_ops_in_namespace.cxx"
#else
namespace boost_no_operators_in_namespace = empty_boost;
#endif
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
#include "boost_no_partial_spec.cxx"
#else
namespace boost_no_template_partial_specialization = empty_boost;
#endif
#ifndef BOOST_NO_PRIVATE_IN_AGGREGATE
#include "boost_no_priv_aggregate.cxx"
#else
namespace boost_no_private_in_aggregate = empty_boost;
#endif
#ifndef BOOST_NO_POINTER_TO_MEMBER_CONST
#include "boost_no_ptr_mem_const.cxx"
#else
namespace boost_no_pointer_to_member_const = empty_boost;
#endif
#ifndef BOOST_NO_UNREACHABLE_RETURN_DETECTION
#include "boost_no_ret_det.cxx"
#else
namespace boost_no_unreachable_return_detection = empty_boost;
#endif
#ifndef BOOST_NO_STRINGSTREAM
#include "boost_no_sstream.cxx"
#else
namespace boost_no_stringstream = empty_boost;
#endif
#ifndef BOOST_NO_STD_ALLOCATOR
#include "boost_no_std_allocator.cxx"
#else
namespace boost_no_std_allocator = empty_boost;
#endif
#ifndef BOOST_NO_STD_DISTANCE
#include "boost_no_std_distance.cxx"
#else
namespace boost_no_std_distance = empty_boost;
#endif
#ifndef BOOST_NO_STD_ITERATOR_TRAITS
#include "boost_no_std_iter_traits.cxx"
#else
namespace boost_no_std_iterator_traits = empty_boost;
#endif
#ifndef BOOST_NO_STD_ITERATOR
#include "boost_no_std_iterator.cxx"
#else
namespace boost_no_std_iterator = empty_boost;
#endif
#ifndef BOOST_NO_STD_LOCALE
#include "boost_no_std_locale.cxx"
#else
namespace boost_no_std_locale = empty_boost;
#endif
#ifndef BOOST_NO_STD_MESSAGES
#include "boost_no_std_messages.cxx"
#else
namespace boost_no_std_messages = empty_boost;
#endif
#ifndef BOOST_NO_STD_MIN_MAX
#include "boost_no_std_min_max.cxx"
#else
namespace boost_no_std_min_max = empty_boost;
#endif
#ifndef BOOST_NO_STD_OUTPUT_ITERATOR_ASSIGN
#include "boost_no_std_oi_assign.cxx"
#else
namespace boost_no_std_output_iterator_assign = empty_boost;
#endif
#ifndef BOOST_NO_STD_USE_FACET
#include "boost_no_std_use_facet.cxx"
#else
namespace boost_no_std_use_facet = empty_boost;
#endif
#ifndef BOOST_NO_STD_WSTREAMBUF
#include "boost_no_std_wstreambuf.cxx"
#else
namespace boost_no_std_wstreambuf = empty_boost;
#endif
#ifndef BOOST_NO_STD_WSTRING
#include "boost_no_std_wstring.cxx"
#else
namespace boost_no_std_wstring = empty_boost;
#endif
#ifndef BOOST_NO_STDC_NAMESPACE
#include "boost_no_stdc_namespace.cxx"
#else
namespace boost_no_stdc_namespace = empty_boost;
#endif
#ifndef BOOST_NO_SWPRINTF
#include "boost_no_swprintf.cxx"
#else
namespace boost_no_swprintf = empty_boost;
#endif
#ifndef BOOST_NO_TEMPLATE_TEMPLATES
#include "boost_no_template_template.cxx"
#else
namespace boost_no_template_templates = empty_boost;
#endif
#ifndef BOOST_NO_USING_TEMPLATE
#include "boost_no_using_template.cxx"
#else
namespace boost_no_using_template = empty_boost;
#endif
#ifndef BOOST_NO_VOID_RETURNS
#include "boost_no_void_returns.cxx"
#else
namespace boost_no_void_returns = empty_boost;
#endif
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
#include "boost_no_wchar_t.cxx"
#else
namespace boost_no_intrinsic_wchar_t = empty_boost;
#endif
#ifdef BOOST_HAS_TWO_ARG_USE_FACET
#include "boost_has_2arg_use_facet.cxx"
#else
namespace boost_has_two_arg_use_facet = empty_boost;
#endif
#ifdef BOOST_HAS_BETHREADS
#include "boost_has_bethreads.cxx"
#else
namespace boost_has_bethreads = empty_boost;
#endif
#ifdef BOOST_HAS_CLOCK_GETTIME
#include "boost_has_clock_gettime.cxx"
#else
namespace boost_has_clock_gettime = empty_boost;
#endif
#ifdef BOOST_HAS_DIRENT_H
#include "boost_has_dirent_h.cxx"
#else
namespace boost_has_dirent_h = empty_boost;
#endif
#ifdef BOOST_HAS_FTIME
#include "boost_has_ftime.cxx"
#else
namespace boost_has_ftime = empty_boost;
#endif
#ifdef BOOST_HAS_GETTIMEOFDAY
#include "boost_has_gettimeofday.cxx"
#else
namespace boost_has_gettimeofday = empty_boost;
#endif
#ifdef BOOST_HAS_HASH
#include "boost_has_hash.cxx"
#else
namespace boost_has_hash = empty_boost;
#endif
#ifdef BOOST_HAS_LONG_LONG
#include "boost_has_long_long.cxx"
#else
namespace boost_has_long_long = empty_boost;
#endif
#ifdef BOOST_HAS_MACRO_USE_FACET
#include "boost_has_macro_use_facet.cxx"
#else
namespace boost_has_macro_use_facet = empty_boost;
#endif
#ifdef BOOST_HAS_MS_INT64
#include "boost_has_ms_int64.cxx"
#else
namespace boost_has_ms_int64 = empty_boost;
#endif
#ifdef BOOST_HAS_NANOSLEEP
#include "boost_has_nanosleep.cxx"
#else
namespace boost_has_nanosleep = empty_boost;
#endif
#ifdef BOOST_HAS_NL_TYPES_H
#include "boost_has_nl_types_h.cxx"
#else
namespace boost_has_nl_types_h = empty_boost;
#endif
#ifdef BOOST_HAS_NRVO
#include "boost_has_nrvo.cxx"
#else
namespace boost_has_nrvo = empty_boost;
#endif
#ifdef BOOST_HAS_PARTIAL_STD_ALLOCATOR
#include "boost_has_part_alloc.cxx"
#else
namespace boost_has_partial_std_allocator = empty_boost;
#endif
#ifdef BOOST_HAS_PTHREAD_DELAY_NP
#include "boost_has_pthread_delay_np.cxx"
#else
namespace boost_has_pthread_delay_np = empty_boost;
#endif
#ifdef BOOST_HAS_PTHREAD_MUTEXATTR_SETTYPE
#include "boost_has_pthread_ma_st.cxx"
#else
namespace boost_has_pthread_mutexattr_settype = empty_boost;
#endif
#ifdef BOOST_HAS_PTHREAD_YIELD
#include "boost_has_pthread_yield.cxx"
#else
namespace boost_has_pthread_yield = empty_boost;
#endif
#ifdef BOOST_HAS_PTHREADS
#include "boost_has_pthreads.cxx"
#else
namespace boost_has_pthreads = empty_boost;
#endif
#ifdef BOOST_HAS_SCHED_YIELD
#include "boost_has_sched_yield.cxx"
#else
namespace boost_has_sched_yield = empty_boost;
#endif
#ifdef BOOST_HAS_SGI_TYPE_TRAITS
#include "boost_has_sgi_type_traits.cxx"
#else
namespace boost_has_sgi_type_traits = empty_boost;
#endif
#ifdef BOOST_HAS_SIGACTION
#include "boost_has_sigaction.cxx"
#else
namespace boost_has_sigaction = empty_boost;
#endif
#ifdef BOOST_HAS_SLIST
#include "boost_has_slist.cxx"
#else
namespace boost_has_slist = empty_boost;
#endif
#ifdef BOOST_HAS_STDINT_H
#include "boost_has_stdint_h.cxx"
#else
namespace boost_has_stdint_h = empty_boost;
#endif
#ifdef BOOST_HAS_STLP_USE_FACET
#include "boost_has_stlp_use_facet.cxx"
#else
namespace boost_has_stlp_use_facet = empty_boost;
#endif
#ifdef BOOST_HAS_UNISTD_H
#include "boost_has_unistd_h.cxx"
#else
namespace boost_has_unistd_h = empty_boost;
#endif
#ifdef BOOST_MSVC6_MEMBER_TEMPLATES
#include "boost_has_vc6_mem_templ.cxx"
#else
namespace boost_msvc6_member_templates = empty_boost;
#endif
#ifdef BOOST_MSVC_STD_ITERATOR
#include "boost_has_vc_iterator.cxx"
#else
namespace boost_msvc_std_iterator = empty_boost;
#endif
#ifdef BOOST_HAS_WINTHREADS
#include "boost_has_winthreads.cxx"
#else
namespace boost_has_winthreads = empty_boost;
#endif

int test_main( int, char *[] )
{
   BOOST_TEST(0 == boost_no_intrinsic_wchar_t::test());
   BOOST_TEST(0 == boost_no_void_returns::test());
   BOOST_TEST(0 == boost_no_using_template::test());
   BOOST_TEST(0 == boost_no_template_templates::test());
   BOOST_TEST(0 == boost_no_swprintf::test());
   BOOST_TEST(0 == boost_no_stdc_namespace::test());
   BOOST_TEST(0 == boost_no_std_wstring::test());
   BOOST_TEST(0 == boost_no_std_wstreambuf::test());
   BOOST_TEST(0 == boost_no_std_use_facet::test());
   BOOST_TEST(0 == boost_no_std_output_iterator_assign::test());
   BOOST_TEST(0 == boost_no_std_min_max::test());
   BOOST_TEST(0 == boost_no_std_messages::test());
   BOOST_TEST(0 == boost_no_std_locale::test());
   BOOST_TEST(0 == boost_no_std_iterator::test());
   BOOST_TEST(0 == boost_no_std_iterator_traits::test());
   BOOST_TEST(0 == boost_no_std_distance::test());
   BOOST_TEST(0 == boost_no_std_allocator::test());
   BOOST_TEST(0 == boost_no_stringstream::test());
   BOOST_TEST(0 == boost_no_unreachable_return_detection::test());
   BOOST_TEST(0 == boost_no_pointer_to_member_const::test());
   BOOST_TEST(0 == boost_no_private_in_aggregate::test());
   BOOST_TEST(0 == boost_no_template_partial_specialization::test());
   BOOST_TEST(0 == boost_no_operators_in_namespace::test());
   BOOST_TEST(0 == boost_no_member_templates::test());
   BOOST_TEST(0 == boost_no_member_template_friends::test());
   BOOST_TEST(0 == boost_no_member_template_keyword::test());
   BOOST_TEST(0 == boost_no_member_function_specializations::test());
   BOOST_TEST(0 == boost_no_long_long_numeric_limits::test());
   BOOST_TEST(0 == boost_no_limits_compile_time_constants::test());
   BOOST_TEST(0 == boost_no_limits::test());
   BOOST_TEST(0 == boost_no_templated_iterator_constructors::test());
   BOOST_TEST(0 == boost_no_integral_int64_t::test());
   BOOST_TEST(0 == boost_no_inclass_member_initialization::test());
   BOOST_TEST(0 == boost_no_ms_int64_numeric_limits::test());
   BOOST_TEST(0 == boost_no_function_template_ordering::test());
   BOOST_TEST(0 == boost_no_explicit_function_template_arguments::test());
   BOOST_TEST(0 == boost_no_exceptions::test());
   BOOST_TEST(0 == boost_no_exception_std_namespace::test());
   BOOST_TEST(0 == boost_no_dependent_types_in_template_value_parameters::test());
   BOOST_TEST(0 == boost_no_dependent_nested_derivations::test());
   BOOST_TEST(0 == boost_deduced_typename::test());
   BOOST_TEST(0 == boost_no_cwctype::test());
   BOOST_TEST(0 == boost_no_cwchar::test());
   BOOST_TEST(0 == boost_no_cv_void_specializations::test());
   BOOST_TEST(0 == boost_no_cv_specializations::test());
   BOOST_TEST(0 == boost_no_ctype_functions::test());
   BOOST_TEST(0 == boost_bcb_partial_specialization_bug::test());
   BOOST_TEST(0 == boost_no_auto_ptr::test());
   BOOST_TEST(0 == boost_no_argument_dependent_lookup::test());
   BOOST_TEST(0 == boost_has_winthreads::test());
   BOOST_TEST(0 == boost_msvc_std_iterator::test());
   BOOST_TEST(0 == boost_msvc6_member_templates::test());
   BOOST_TEST(0 == boost_has_unistd_h::test());
   BOOST_TEST(0 == boost_has_stlp_use_facet::test());
   BOOST_TEST(0 == boost_has_stdint_h::test());
   BOOST_TEST(0 == boost_has_slist::test());
   BOOST_TEST(0 == boost_has_sigaction::test());
   BOOST_TEST(0 == boost_has_sgi_type_traits::test());
   BOOST_TEST(0 == boost_has_sched_yield::test());
   BOOST_TEST(0 == boost_has_pthreads::test());
   BOOST_TEST(0 == boost_has_pthread_yield::test());
   BOOST_TEST(0 == boost_has_pthread_mutexattr_settype::test());
   BOOST_TEST(0 == boost_has_pthread_delay_np::test());
   BOOST_TEST(0 == boost_has_partial_std_allocator::test());
   BOOST_TEST(0 == boost_has_nrvo::test());
   BOOST_TEST(0 == boost_has_nl_types_h::test());
   BOOST_TEST(0 == boost_has_nanosleep::test());
   BOOST_TEST(0 == boost_has_ms_int64::test());
   BOOST_TEST(0 == boost_has_macro_use_facet::test());
   BOOST_TEST(0 == boost_has_long_long::test());
   BOOST_TEST(0 == boost_has_hash::test());
   BOOST_TEST(0 == boost_has_gettimeofday::test());
   BOOST_TEST(0 == boost_has_ftime::test());
   BOOST_TEST(0 == boost_has_dirent_h::test());
   BOOST_TEST(0 == boost_has_clock_gettime::test());
   BOOST_TEST(0 == boost_has_bethreads::test());
   BOOST_TEST(0 == boost_has_two_arg_use_facet::test());
   return 0;
}


text:
/**
 *  @file
 *  @copyright defined in eos/LICENSE.txt
 */
#include <eos/chain/BlockchainConfiguration.hpp>

#include <boost/range/algorithm/nth_element.hpp>

#include <fc/io/json.hpp>

namespace eos { namespace chain {

template <typename T>
struct assign_visitor {
   T& a;
   const T& b;

   template <typename Member, class Class, Member (Class::*member)>
   void operator() (const char*) const {
      a.*member = b.*member;
   }
};

BlockchainConfiguration& BlockchainConfiguration::operator=(const types::BlockchainConfiguration& other) {
   if (&other != this)
      fc::reflector<BlockchainConfiguration>::visit(assign_visitor<types::BlockchainConfiguration>{*this, other});
   return *this;
}

template <typename T, typename Range>
struct properties_median_calculator_visitor {
   properties_median_calculator_visitor (T& medians, Range votes)
      : medians(medians), votes(votes)
   {}

   template <typename Member, class Class, Member (Class::*member)>
   void operator() (const char*) const {
      auto median_itr = boost::begin(votes) + boost::distance(votes)/2;
      boost::nth_element(votes, median_itr, [](const T& a, const T& b) { return a.*member < b.*member; });
      medians.*member = (*median_itr).*member;
   }

   T& medians;
   mutable Range votes;
};
template <typename T, typename Range>
properties_median_calculator_visitor<T, Range> get_median_properties_calculator(T& medians, Range&& votes) {
   return properties_median_calculator_visitor<T, Range>(medians, std::move(votes));
}

BlockchainConfiguration BlockchainConfiguration::get_median_values(
      std::vector<BlockchainConfiguration> votes) {
   BlockchainConfiguration results;
   fc::reflector<BlockchainConfiguration>::visit(get_median_properties_calculator(results, std::move(votes)));
   return results;
}

std::ostream& operator<< (std::ostream& s, const BlockchainConfiguration& p) {
   return s << fc::json::to_string(p);
}

template <typename T>
struct comparison_visitor {
   const T& a;
   const T& b;

   template <typename Member, class Class, Member (Class::*member)>
   void operator() (const char*) const {
      if (a.*member != b.*member)
         // Throw to stop comparing fields: the structs are unequal
         throw false;
   }
};

bool operator==(const types::BlockchainConfiguration& a, const types::BlockchainConfiguration& b) {
   // Yes, it's gross, I'm using a boolean exception to direct normal control flow... that's why it's buried deep in an
   // implementation detail file. I think it's worth it for the generalization, though: this code keeps working no
   // matter what updates happen to BlockchainConfiguration
   if (&a != &b) {
      try {
         fc::reflector<BlockchainConfiguration>::visit(comparison_visitor<types::BlockchainConfiguration>{a, b});
      } catch (bool) {
         return false;
      }
   }
   return true;
}

} } // namespace eos::chain


text:
// -----------------------------------------------------------------------------
// simple_server_application_with_auto_handler.cpp : examples that show how use
// Boost.Application to make a simplest interactive (terminal) application
//
// Note 1: The Boost.Application (Aspects v4) and this sample are in
//         development process.
// -----------------------------------------------------------------------------

// Copyright 2011-2013 Renato Tegon Forti
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

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

//
// *****************************************************************************
// note that this code uses experimental module auto_handler
// *****************************************************************************
//

// To call "stop" on POSIX, use:
//
// kill -2 <PID>
// kill -INT <PID>

// output sample on windows service (log.txt)
//
// Start Log...
// -----------------------------
// ---------- Arg List ---------
// -----------------------------
// simple_server_application
// -----------------------------
// running...
// running...
// running...
// [...]
// Pause my application...
// paused...
// paused...
// paused...
// [...]
// Resume my application...
// running...
// running...
// running...
// [...]
// Stoping my application...
//

#define BOOST_ALL_DYN_LINK
#define BOOST_LIB_DIAGNOSTIC

#define BOOST_APPLICATION_FEATURE_NS_SELECT_BOOST

#include <iostream>
#include <fstream>
#include <boost/program_options.hpp>
#include <boost/application.hpp>
#include <boost/application/auto_handler.hpp>

// provide setup example for windows service
#if defined(BOOST_WINDOWS_API)
#   include "setup/windows/setup/service_setup.hpp"
#endif

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

// my application code

class myapp
{
public:

   myapp(application::context& context)
      : context_(context)
   {
   }

   void worker()
   {
      // my application behaviour

      // dump args

      std::vector<std::string> arg_vector =
         context_.find<application::args>()->arg_vector();

      my_log_file_ << "-----------------------------" << std::endl;
      my_log_file_ << "---------- Arg List ---------" << std::endl;
      my_log_file_ << "-----------------------------" << std::endl;

      // only print args on screen
      for(std::vector<std::string>::iterator it = arg_vector.begin();
         it != arg_vector.end(); ++it) {
         my_log_file_ << *it << std::endl;
      }

      my_log_file_ << "-----------------------------" << std::endl;

      boost::shared_ptr<application::process_id> pid
         = context_.find<application::process_id>();

      my_log_file_ << "PID: " << pid->pid() << std::endl;

      my_log_file_ << "-----------------------------" << std::endl;

      // run logic

      boost::shared_ptr<application::status> st =
         context_.find<application::status>();

      int count = 0;
      while(st->state() != application::status::stopped)
      {
         boost::this_thread::sleep(boost::posix_time::seconds(1));

         if(st->state() == application::status::paused)
            my_log_file_ << count++ << ", paused..." << std::endl;
         else
            my_log_file_ << count++ << ", running..." << std::endl;
      }
   }

   // param
   int operator()()
   {
      std::string logfile
         = context_.find<application::path>()->executable_path().string() + "/log.txt";

      my_log_file_.open(logfile.c_str());
      my_log_file_ << "Start Log..." << std::endl;

      // launch a work thread
      boost::thread thread(&myapp::worker, this);

      context_.find<application::wait_for_termination_request>()->wait();

      // to run direct
      // worker(&context);

      return 0;
   }

   // windows/posix

   bool stop()
   {
      my_log_file_ << "Stoping my application..." << std::endl;
      my_log_file_.close();

      return true; // return true to stop, false to ignore
   }

   // windows specific (ignored on posix)

   bool pause()
   {
      my_log_file_ << "Pause my application..." << std::endl;
      return true; // return true to pause, false to ignore
   }

   bool resume()
   {
      my_log_file_ << "Resume my application..." << std::endl;
      return true; // return true to resume, false to ignore
   }

private:

   std::ofstream my_log_file_;
   application::context& context_;

};

// my setup code for windows service

bool setup(application::context& context)
{
   strict_lock<application::aspect_map> guard(context);

   boost::shared_ptr<application::args> myargs
      = context.find<application::args>(guard);

   boost::shared_ptr<application::path> mypath
      = context.find<application::path>(guard);

// provide setup for windows service
#if defined(BOOST_WINDOWS_API)
#if !defined(__MINGW32__)

   // get our executable path name
   boost::filesystem::path executable_path_name = mypath->executable_path_name();

   // define our simple installation schema options
   po::options_description install("service options");
   install.add_options()
      ("help", "produce a help message")
      (",i", "install service")
      (",u", "unistall service")
      ("name", po::value<std::string>()->default_value(mypath->executable_name().stem().string()), "service name")
      ("display", po::value<std::string>()->default_value(""), "service display name (optional, installation only)")
      ("description", po::value<std::string>()->default_value(""), "service description (optional, installation only)")
      ;

      po::variables_map vm;
      po::store(po::parse_command_line(myargs->argc(), myargs->argv(), install), vm);
      boost::system::error_code ec;

      if (vm.count("help"))
      {
         std::cout << install << std::cout;
         return true;
      }

      if (vm.count("-i"))
      {
         application::example::install_windows_service(
         application::setup_arg(vm["name"].as<std::string>()),
         application::setup_arg(vm["display"].as<std::string>()),
         application::setup_arg(vm["description"].as<std::string>()),
         application::setup_arg(executable_path_name)).install(ec);

         std::cout << ec.message() << std::endl;

         return true;
      }

      if (vm.count("-u"))
      {
         application::example::uninstall_windows_service(
            application::setup_arg(vm["name"].as<std::string>()),
            application::setup_arg(executable_path_name)).uninstall(ec);

         std::cout << ec.message() << std::endl;

         return true;
      }

#endif
#endif

   return false;
}
// main

int main(int argc, char *argv[])
{
   application::context app_context;

   // auto_handler will automatically add termination, pause and resume (windows) handlers
   application::auto_handler<myapp> app(app_context);
   // application::detail::handler_auto_set<myapp> app(app_context);

   // my server aspects

   app_context.insert<application::path>(
      boost::make_shared<application::path>());

   app_context.insert<application::args>(
      boost::make_shared<application::args>(argc, argv));

   // check if we need setup

   if(setup(app_context))
   {
      std::cout << "[I] Setup changed the current configuration." << std::endl;
      return 0;
   }

   // my server instantiation

   boost::system::error_code ec;
   int result = application::launch<application::server>(app, app_context, ec);

   if(ec)
   {
      std::cout << "[E] " << ec.message()
         << " <" << ec.value() << "> " << std::cout;
   }

   return result;
}

max_stars_repo_name: hbccdf/network-core

max_issues_repo_name: hbccdf/network-core

max_forks_repo_name: hbccdf/network-core


text: #include <boost/process/locale.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
//
// KinectAzureUtils.cpp
// Custom Utilities dealing with the Kinect Azure SDK
//
// Created by: Jeremy Keller
// 2/03/2020
//

#include "KinectAzureProcessor.h"
#include "BodyTrackingUtils.h"
#include <regex>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/filesystem.hpp>

using boost::property_tree::ptree;

void KinectAzureProcessor::print_capture_info(recording_t* file)
{
	k4a_image_t image = k4a_capture_get_depth_image(file->capture);

	printf("%-32s", file->filename.c_str());

	if (image != NULL)
	{
		uint64_t timestamp = k4a_image_get_device_timestamp_usec(image);
		printf("  %7ju usec", timestamp);
		k4a_image_release(image);
		image = NULL;
	}
	else
	{
		printf("  %12s", "");
	}
	printf("\n");
}

uint64_t KinectAzureProcessor::first_capture_timestamp(k4a_capture_t capture)
{
	uint64_t timestamp = k4a_image_get_device_timestamp_usec(k4a_capture_get_depth_image(capture));
	k4a_image_release(k4a_capture_get_depth_image(capture));
	return timestamp;
}

std::string KinectAzureProcessor::getStartRecording(recording_t* files, std::vector<CalibrationInfo> frameInfo, int fileCount) {
	std::unordered_map<uint64_t, TimestampCounter> timestampDifferenceCounts;

	// Count the occurences of each timestamp difference
	for (int i = 0 + 1; i < frameInfo.size(); i++) {
		// Incremement timestampDifferenceCounts
		uint64_t timestampDiff = frameInfo.at(i).timestampDiff;
		std::string fileName = frameInfo.at(i).fileName;
		if (timestampDifferenceCounts.count(timestampDiff) > 0) {
			// Counter for frame timestamp diff is the same, increment
			TimestampCounter count = timestampDifferenceCounts.at(timestampDiff);
			count.timestampCount++;
			timestampDifferenceCounts.erase(timestampDiff);
			timestampDifferenceCounts.insert({ timestampDiff, count });
		}
		else {
			// Counter for frame timestamp is new, insert with count of 1
			timestampDifferenceCounts.insert({ timestampDiff, {fileName, 1} });
		}
	}

	// Determine the frame with the max usec timestamp difference before it, this will be the true starting capture
	uint64_t maxTimestampDiff = 0;
	for (auto& x : timestampDifferenceCounts) {
		uint64_t timestampDiff = x.first;
		TimestampCounter timestampCounter = x.second;
		std::string fileName = timestampCounter.fileName;
		int count = timestampCounter.timestampCount;

		// Only process timestamp info with a count of > 3 and if the timestamp difference is greater than the max tracked timestamp so far
		if (count > 3 && timestampDiff > maxTimestampDiff) {
			maxTimestampDiff = timestampDiff;
		}
	}

	// If no timestamp could be evaluated, return empty name
	if (maxTimestampDiff == 0) {
		return "";
	}

	//CalibrationInfo startFrameInfo = { timestampDifferenceCounts.at(maxTimestampDiff).fileName, maxTimestampDiff };
	return timestampDifferenceCounts.at(maxTimestampDiff).fileName;
}

KinectAzureProcessor::FrameInfo KinectAzureProcessor::getNextFrame(int fileCount, recording_t* files) {
	uint64_t minTimestamp = (uint64_t)-1;
	KinectAzureProcessor::recording_t* nextFile = NULL;

	int index = 0;
	// Find the lowest timestamp out of each of the current captures.
	for (size_t i = 0; i < fileCount; i++)
	{
		if (files[i].capture != NULL)
		{
			uint64_t timestamp = first_capture_timestamp(files[i].capture);
			if (timestamp < minTimestamp)
			{
				index = i;
				minTimestamp = timestamp;
				nextFile = &files[i];
			}
		}
	}

	return {index, minTimestamp, nextFile};
}

Ply KinectAzureProcessor::generatePointCloud(KinectAzureProcessor::FrameInfo frameInfo, k4a_calibration_t* calibrations) {
	// Generate xy table
	k4a_image_t xyTable = NULL;

	k4a_result_t result1 = k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
		calibrations[frameInfo.index].depth_camera_calibration.resolution_width,
		calibrations[frameInfo.index].depth_camera_calibration.resolution_height,
		calibrations[frameInfo.index].depth_camera_calibration.resolution_width * (int)sizeof(k4a_float2_t),
		&xyTable);

	// Create xy table
	k4a_float2_t* table_data = (k4a_float2_t*)(void*)k4a_image_get_buffer(xyTable);

	int width = calibrations[frameInfo.index].depth_camera_calibration.resolution_width;
	int height = calibrations[frameInfo.index].depth_camera_calibration.resolution_height;

	k4a_float2_t p;
	k4a_float3_t ray;
	int valid;

	for (int y = 0, idx = 0; y < height; y++)
	{
		p.xy.y = (float)y;
		for (int x = 0; x < width; x++, idx++)
		{
			p.xy.x = (float)x;

			k4a_calibration_2d_to_3d(
				&calibrations[frameInfo.index], &p, 1.f, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_DEPTH, &ray, &valid);

			if (valid)
			{
				table_data[idx].xy.x = ray.xyz.x;
				table_data[idx].xy.y = ray.xyz.y;
			}
			else
			{
				table_data[idx].xy.x = nanf("");
				table_data[idx].xy.y = nanf("");
			}
		}
	}

	// Generate point cloud
	const k4a_image_t depthImage = k4a_capture_get_depth_image(frameInfo.file->capture);
	int depthWidth = k4a_image_get_width_pixels(depthImage);
	int depthHeight = k4a_image_get_height_pixels(depthImage);

	uint16_t* depth_data = (uint16_t*)(void*)k4a_image_get_buffer(depthImage);
	k4a_float2_t* xy_table_data = (k4a_float2_t*)(void*)k4a_image_get_buffer(xyTable);

	Ply ply = Ply();
	ply.setFileName(frameInfo.file->filename);
	for (int i = 0; i < depthWidth * depthHeight; i++)
	{
		if (depth_data[i] != 0 && !isnan(xy_table_data[i].xy.x) && !isnan(xy_table_data[i].xy.y))
		{
			Eigen::RowVector3d point;
			point(0) = xy_table_data[i].xy.x * (float)depth_data[i];
			point(1) = xy_table_data[i].xy.y * (float)depth_data[i];
			point(2) = (float)depth_data[i];
			ply.addPoint(point);
		}
	}

	return ply;

}

bool KinectAzureProcessor::checkSubjectWithinCaptureSpace(std::vector<Eigen::RowVector3d> jointPositions, BodyTrackingUtils::BoundingBox captureSpaceBounds) {
	// Check if all joint locations are contained within the capture space bounds. If not, then the frame should not be processed.
	for (Eigen::RowVector3d jointPosition : jointPositions) {
		if (!BodyTrackingUtils::withinBounds(jointPosition, captureSpaceBounds)) {
			return false;
		}
	}

	return true;
}

void KinectAzureProcessor::outputPointCloudGroup(std::vector<Ply> plys, uint64_t groupCount, std::unordered_map<std::string, Eigen::Matrix4Xd> transformations,
	std::vector<Eigen::RowVector3d> jointPositions, BodyTrackingUtils::BoundingBox captureSpaceBounds, std::string bodyTrackingFileSuffix) {

	// Calculate different behavior settings:
	bool individualOutputUnfiltered = debugMode || (calibrationMode && transformations.size() == 0);
	bool individualOutputFiltered = debugMode || (calibrationMode && transformations.size() > 0);
	bool unprocessedMergedOutput = debugMode;
	bool filterAndMeshing = !calibrationMode;

	std::stringstream outputFileName;
	outputFileName << "Group" << groupCount << "_";

	// Apply transformations to move all point clouds into master coordinate system
	std::vector<Ply> transformedPlys = MatrixUtils::applyTransforms(plys, transformations);

	BodyTrackingUtils::BoundingBox boundingBox;
	// If there are joint positions, create a bounding box based off of them
	if (jointPositions.size() > 0) {
		// Grab transformation for body tracking file to apply to jointPositions
		bool jointTransformFound = false;
		Eigen::Matrix4Xd jointPositionTransformation;
		for (auto it = transformations.begin(); it != transformations.end(); it++) {
			// For each pc, loop through transform and find matches
			if (it->first.compare(bodyTrackingFileSuffix) == 0) {
				jointPositionTransformation = it->second;
				jointTransformFound = true;
				break;
			}
		}

		if (debugMode) {
			// Output individual json for frame
			std::stringstream jointOutputName;
			jointOutputName << captureDirectory << "\\Joints_" << groupCount << ".txt";
			ofstream out;
			out.open(jointOutputName.str(), ios::out);
			for (Eigen::RowVector3d jointXYZ : jointPositions) {
				out << jointXYZ(0) << " " << jointXYZ(1) << " " << jointXYZ(2) << endl;
			}
			out.close();
		}

		// Apply transform to joint locations
		// Check capture space bounding box
		//	If there are any joints located outside of the capture space, the output will be skipped
		// Create Bounding box
		if (jointTransformFound) {
			std::vector<Eigen::RowVector3d> jointPositionsTransformed = MatrixUtils::applyJointTrackingTransform(jointPositions, jointPositionTransformation);
			if (!checkSubjectWithinCaptureSpace(jointPositionsTransformed, captureSpaceBounds)) {
				std::cerr << "Subject is outside of capture space. Skipping output." << endl;
				return;
			}
			boundingBox = BodyTrackingUtils::createBoundingBox(jointPositionsTransformed);

		}
		else {
			if (!checkSubjectWithinCaptureSpace(jointPositions, captureSpaceBounds)) {
				std::cerr << "Subject is outside of capture space. Skipping output." << endl;
				return;
			}
			boundingBox = BodyTrackingUtils::createBoundingBox(jointPositions);
		}

		// Add 'f' modifier to output file name
		outputFileName << "f";
	}
	else {
		// Case when there is no joint locations

		// Bounding box is just going to be the entire capture space bounds, since we do not have joint locations to isolate a subject
		boundingBox = captureSpaceBounds;

		// Skip filtering and meshing since the capture will not be isolated
		filterAndMeshing = false;

		// Output unprocessed merged point cloud output, since additonal meshing and processing will not occur
		unprocessedMergedOutput = true;

		// Add 'a' modifier to output file name
		outputFileName << "a";
	}

	// Apply bounds, convert to pcd file, and filter
	pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloudCombined(new pcl::PointCloud<pcl::PointXYZ>);
	int index = 0;
	for (Ply pc : transformedPlys) {
		pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloudSingle(new pcl::PointCloud<pcl::PointXYZ>);
		for (Eigen::RowVector3d point : pc.getPoints()) {
			if (BodyTrackingUtils::withinBounds(point, boundingBox)) {
				pclCloudSingle->points.push_back({ (float)point(0), (float)point(1), (float)point(2) });
			}
		}

		// Calculate individual file name
		std::string individualFileNameIdentifier;
		// Only calculate if any individual file output is specified
		if (individualOutputUnfiltered || individualOutputFiltered) {
			individualFileNameIdentifier = IOUtils::extractFileName(pc.getFileName());
		}

		// In calibration mode, output point cloud before statistical outlier filtering is applied
		if (pclCloudSingle->size() > 0 && individualOutputUnfiltered) {
			pclCloudSingle->resize(pclCloudSingle->size());
			std::stringstream singleFileName;
			singleFileName << captureDirectory << "\\Unfiltered_IndividualPc_" << groupCount << "_" << individualFileNameIdentifier << ".pcd";
			PclUtils::outputToFile(pclCloudSingle, singleFileName.str());
		}

		// Apply statistical outlier filter
		pcl::PointCloud<pcl::PointXYZ>::Ptr filteredPc(new pcl::PointCloud<pcl::PointXYZ>);
		PclUtils::applyStatisticalOutlierFilter(pclCloudSingle, filteredPc);

		// For single frame output
		if (filteredPc->size() > 0 && individualOutputFiltered) {
			std::stringstream filteredFileName;
			filteredFileName << captureDirectory << "\\IndividualPc_" << groupCount << "_" << individualFileNameIdentifier << ".pcd";
			PclUtils::outputToFile(filteredPc, filteredFileName.str());
		}

		*pclCloudCombined += *filteredPc;
		index++;
	}

	// For merged point cloud output before downsampling and smoothing
	if (unprocessedMergedOutput) {
		std::string pcFileName = std::string(outputFileName.str());
		pcFileName += "_full.pcd";
		std::string fullFilePath = std::string(captureDirectory);
		fullFilePath += "\\";
		fullFilePath += pcFileName;
		PclUtils::outputToFile(pclCloudCombined, fullFilePath);
	}

	// Meshing
	if (filterAndMeshing) {
		std::string meshFileName = std::string(outputFileName.str());
		std::string fullMeshPath = std::string(captureDirectory);
		fullMeshPath += "\\";
		fullMeshPath += meshFileName;
		PclUtils::resampleAndMesh(pclCloudCombined, fullMeshPath, disableMeshOutput);
	}
}

bool KinectAzureProcessor::openFiles(KinectAzureProcessor::recording_t** filess, k4a_calibration_t** calibrationss, k4abt_tracker_t &tracker, std::vector<std::string> mkvFiles, std::string btFileSuffix) {
	if (mkvFiles.size() < 1) {
		printf("At least one mkv video must be found for processing");
		return false;
	}

	KinectAzureProcessor::recording_t* files = *filess;
	k4a_calibration_t* calibrations = *calibrationss;

	size_t file_count = (size_t)(mkvFiles.size());
	bool master_found = false;
	k4a_result_t result = K4A_RESULT_SUCCEEDED;

	// Open each recording file and validate they were recorded in master/subordinate mode.
	for (int i = 0; i < file_count; i++)
	{
		char* filename = const_cast<char*>(mkvFiles.at(i).c_str());
		files[i].filename = mkvFiles.at(i);
		result = k4a_playback_open(filename, &files[i].handle);

		if (result != K4A_RESULT_SUCCEEDED)
		{
			printf("Failed to open file: %s\n", filename);
			break;
		}

		result = k4a_playback_get_record_configuration(files[i].handle, &files[i].record_config);
		if (result != K4A_RESULT_SUCCEEDED)
		{
			printf("Failed to get record configuration for file: %s\n", filename);
			break;
		}

		if (files[i].record_config.wired_sync_mode == K4A_WIRED_SYNC_MODE_MASTER)
		{
			printf("Opened master recording file: %s\n", filename);
			if (master_found)
			{
				printf("ERROR: Multiple master recordings listed!\n");
				result = K4A_RESULT_FAILED;
				break;
			}
			else
			{
				master_found = true;
			}
		}
		else if (files[i].record_config.wired_sync_mode == K4A_WIRED_SYNC_MODE_SUBORDINATE)
		{
			printf("Opened subordinate recording file: %s\n", filename);
		}
		else
		{
			printf("ERROR: Recording file was not recorded in master/sub mode: %s\n", filename);
			result = K4A_RESULT_FAILED;
			break;
		}

		k4a_result_t calibrationResult = k4a_playback_get_calibration(files[i].handle, &calibrations[i]);
		if (calibrationResult != K4A_RESULT_SUCCEEDED) {
			printf("Error getting calibration for: %d\n", i);
			return false;
		}

		// Set up body tracking if body tracking file suffix is defined and the capture file names ends with that suffix
		if (btFileSuffix.compare("") != 0 && IOUtils::endsWith(filename, btFileSuffix)) {
			k4abt_tracker_configuration_t tracker_config = K4ABT_TRACKER_CONFIG_DEFAULT;
			result = k4abt_tracker_create(&calibrations[i], tracker_config, &tracker);
			if (result != K4A_RESULT_SUCCEEDED)
			{
				std::cerr << "Body tracker initialization failed!" << std::endl;
				result = K4A_RESULT_FAILED;
				break;
			}
		}

		// Read the first capture of each recording into memory.
		k4a_stream_result_t stream_result = k4a_playback_get_next_capture(files[i].handle, &files[i].capture);
		if (stream_result == K4A_STREAM_RESULT_EOF)
		{
			printf("ERROR: Recording file is empty: %s\n", filename);
			result = K4A_RESULT_FAILED;
			break;
		}
		else if (stream_result == K4A_STREAM_RESULT_FAILED)
		{
			printf("ERROR: Failed to read first capture from file: %s\n", filename);
			result = K4A_RESULT_FAILED;
			break;
		}
	}

	if (tracker == NULL) {
		printf("Warning: Capture for body tracking was not found. Body tracking will not be processed.\n");
	}

	return result == K4A_RESULT_SUCCEEDED;
}

uint64_t KinectAzureProcessor::getMaxTimestampDiff(k4a_calibration_t** calibrations, int fileCount) {
	for (int i = 0; i < fileCount; i++) {
		// If any capture is running in max resolution, assume 15fps
		if (calibrations[0]->depth_camera_calibration.resolution_height == 1024) {
			return timestampDiff15;
		}
	}

	// Otherwise assume 30fps
	return timestampDiff30;
}

int KinectAzureProcessor::outputRecordingsToPlyFiles(std::unordered_map<std::string, Eigen::Matrix4Xd> transformations, std::string btFileSuffix, BodyTrackingUtils::BoundingBox captureSpaceBounds) {
	// Grab filenames to read in
	std::vector<std::string> mkvFiles = IOUtils::obtainMkvFilesFromDirectory(captureDirectory);

	int fileCount = mkvFiles.size();
	std::string masterFileSuffix = "Master.mkv";

	KinectAzureProcessor::recording_t* files = (KinectAzureProcessor::recording_t*)malloc(sizeof(KinectAzureProcessor::recording_t) * fileCount);
	k4a_calibration_t* calibrations = (k4a_calibration_t*)malloc(sizeof(k4a_calibration_t) * fileCount);
	// Allocate memory to store the state of N recordings.
	if (files == NULL)
	{
		printf("Failed to allocate memory for playback (%zu bytes)\n", sizeof(KinectAzureProcessor::recording_t) * fileCount);
		return 1;
	}
	memset(files, 0, sizeof(KinectAzureProcessor::recording_t) * fileCount);
	memset(calibrations, 0, sizeof(k4a_calibration_t) * fileCount);
	k4abt_tracker_t tracker = NULL;

	// Open files
	bool openRecordingSuccess = openFiles(&files, &calibrations, tracker, mkvFiles, btFileSuffix);
	if (!openRecordingSuccess) {
		return 1;
	}

	if (bodyTrackingOutputOnly && tracker == NULL) {
		cerr << "Error: Body tracking only argument was specified, but not body tracking file was specified in the transformation file. Ending processing." << endl;
		return 1;
	}

	k4a_result_t result = K4A_RESULT_SUCCEEDED;

	// Track frame indices
	std::map<std::string, uint64_t> fileIndexCounter;
	for (int i = 0; i < fileCount; i++) {
		fileIndexCounter.insert({mkvFiles.at(i), 0});
	}

	// Calibration variables
	uint64_t previousTimestamp = (uint64_t)0;
	int discardFrameEnd = fileCount * 15;
	int orderingFrameEnd = fileCount * 15 * 2;
	std::vector<CalibrationInfo> calibrationInfo;

	// Processing variables
	int captureCount = 0;
	int groupCount = 0;
	int previousGroupCount = 0;
	uint64_t startGroupTimestamp = 0;
	std::string startFileName;
	std::string previousFileName;
	// Assume all files are run with the same frame rate
	uint64_t maxTimestampDiff = getMaxTimestampDiff(&calibrations, fileCount);
	std::vector<Ply> groupFrames;

	// Joint tracking variables
	bool trackerCaptureFound = (tracker != NULL);
	bool jointsObtained = false;
	std::vector<Eigen::RowVector3d> sub2Joints;
	ptree jointOutputJson, framesJson;
	std::vector<Eigen::RowVector3d> joints;
	double jointFrameModifier = 0.0;

	// Loop variables
	int endThreshold = 3; // Number of consecutive frames without a master capture before we decide to end the processing
	int missingFrameCount = 0; // Track the number of consecutive frames without all the captures
	uint64_t frame = 0;
	bool endProcessing = false;
	bool individualFrameProcessed = false;

	// Get frames in order
	while(true)
	{
		// First check to see if master frame has not been found for a consecutive number of frames
		// If individual frame is specified and has already been output, end processing
		if (endProcessing || missingFrameCount >= endThreshold) {
			// If so, stop processing and exit

			// Output joints json if tracker capture is in use
			if (trackerCaptureFound) {
				// Create an output stream
				std::ofstream outfile;

				// Remove Master.mkv
				// Add .json
				std::string outfileName = std::string(captureDirectory);
				outfileName = outfileName.append("\\joints.json");

				// Open the file to write to
				outfile.open(outfileName, ios::out | ios::trunc);

				jointOutputJson.add_child("frames", framesJson);
				BodyTrackingUtils::seedJointNames(&jointOutputJson);

				std::ostringstream oss;
				boost::property_tree::write_json(oss, jointOutputJson);
				std::regex reg("\\\"([0-9\-]+\\.{0,1}[0-9]*)\\\"");
				std::string result = std::regex_replace(oss.str(), reg, "$1");

				std::ofstream file;
				file.open(outfileName);
				file << result;
				file.close();
			}

			break;
		}
		else if (individualFrameProcessed) {
			// No need to write json when we are only processing a single frame
			// Just break out of loop
			break;
		}

		// Grab next frame
		KinectAzureProcessor::FrameInfo frameInfo = getNextFrame(fileCount, files);

		// Once the next frame is obtained, check if it is the end of the capture
		// The file will be null is the capture has ended
		if (frameInfo.file == NULL) {
			// If the file is null, end processing and continue to the next iteration, where the json joints will be output if applicable
			endProcessing = true;
			continue;
		}

		// Increment the frame's counter
		fileIndexCounter[frameInfo.file->filename] = fileIndexCounter[frameInfo.file->filename]++;

		// Get the timestamp from the frame
		uint64_t timestamp = (uint64_t)frameInfo.timestamp - previousTimestamp;

		if (frame >= discardFrameEnd && frame < orderingFrameEnd) {
			uint64_t timestamp = (uint64_t)frameInfo.timestamp - previousTimestamp;
			//std::cout << std::endl << "Frame" << frame << " Timestamp difference: " << timestamp << std::endl;
			previousTimestamp = frameInfo.timestamp;
			//print_capture_info(frameInfo.file);
			CalibrationInfo ci = { std::string(frameInfo.file->filename), timestamp };
			calibrationInfo.push_back(ci);
		}
		else if (frame == orderingFrameEnd) {
			// Perform all calibrations here
			startFileName = getStartRecording(files, calibrationInfo, fileCount);
			if (startFileName == "") {
				std::cerr << "Error evaluating start frame info." << std::endl;
				return 1;
			}
		}
		else if (frame > orderingFrameEnd) {
			// Group frames together
			uint64_t groupTimestampDiff = frameInfo.timestamp - startGroupTimestamp;
			if (startGroupTimestamp == 0) {
				// Start of processing
				// Continue until we get to the starting file
				if (std::string(frameInfo.file->filename).compare(startFileName) == 0) {
					startGroupTimestamp = frameInfo.timestamp;
				}
			}
			else if (startGroupTimestamp != 0 && groupCount == 0 && groupTimestampDiff <= maxTimestampDiff && std::string(frameInfo.file->filename).compare(previousFileName) == 0) {
				// Case where a capture appears twice in a row (can happen for Body tracking capture running at double fps of other captures)
				startGroupTimestamp = frameInfo.timestamp;
			}
			else if (groupTimestampDiff > maxTimestampDiff) {
				// Process previous group
				// Don't process first group
				if (groupCount != 0) {
					// Only process group if it contains all frames along with body tracking* (only if tracker capture exists)
					if (captureCount >= fileCount && (!trackerCaptureFound || jointsObtained)) {
						if (individualFrameIndex == -1 || groupCount >= individualFrameIndex) {
							missingFrameCount = 0;

							// Decrement frame count for the current frame, this will not be processed and outputted
							fileIndexCounter[frameInfo.file->filename] = fileIndexCounter[frameInfo.file->filename]--;

							// Output combined point cloud
							// If body tracking only is specified, skip this output and only focus on joint tracking
							if (!bodyTrackingOutputOnly) {
								outputPointCloudGroup(groupFrames, groupCount, transformations, joints, captureSpaceBounds, btFileSuffix);
							}

							// Re-increment frame count since it will be accurate for the next group processing
							fileIndexCounter[frameInfo.file->filename] = fileIndexCounter[frameInfo.file->filename]++;

							if (individualFrameIndex > 0) {
								individualFrameProcessed = true;
							}
						}
					}
					else if (captureCount < fileCount) {
						missingFrameCount++;
						std::cerr << "Frames missing for group " << groupCount << ". Skipping." << std::endl;
						if (groupCount == individualFrameIndex) {
							std::cerr << "Frames missing for specified output frame: " << groupCount << ". Next available set of frames will be output." << std::endl;
						}
					}
					else if (trackerCaptureFound && !jointsObtained) {
						std::cerr << "Body tracking missing for group " << groupCount << ". Skipping." << std::endl;
						if (groupCount == individualFrameIndex) {
							std::cerr << "Body tracking missing for specified output frame: " << groupCount << ". Next available set of frames will be output." << std::endl;
						}
					}
				}

				// Print frame info for next group
				groupCount++;
				if (groupCount != previousGroupCount && !individualFrameProcessed) {
					previousGroupCount = groupCount;
					std::cout << std::endl << std::endl << "Processing Group " << groupCount << std::endl;
					print_capture_info(frameInfo.file);
				}

				// Restart group
				jointsObtained = false;
				groupFrames.clear();
				groupFrames.shrink_to_fit();
				startGroupTimestamp = frameInfo.timestamp;
				joints.clear();
				jointFrameModifier = 0.0;
				captureCount = 0;

				// Add current frame to group
				captureCount++;
				if (!skipBtCloudProcessing || !IOUtils::endsWith(frameInfo.file->filename, btFileSuffix)) {
					groupFrames.push_back(generatePointCloud(frameInfo, calibrations));
				}

				// If capture is the desired body tracking file, process joint tracking data
				if (trackerCaptureFound && IOUtils::endsWith(frameInfo.file->filename, btFileSuffix) && !individualFrameProcessed) {
					jointsObtained = BodyTrackingUtils::predictJoints(&framesJson, groupCount + jointFrameModifier, tracker, frameInfo.file->capture, &joints);
					jointFrameModifier += 0.5;
				}
			}
			else {
				print_capture_info(frameInfo.file);
				captureCount++;

				// Add current frame to group
				if (!skipBtCloudProcessing || !IOUtils::endsWith(frameInfo.file->filename, btFileSuffix)) {
					groupFrames.push_back(generatePointCloud(frameInfo, calibrations));
				}

				// If capture is the desired body tracking file, process joint tracking data
				if (trackerCaptureFound && IOUtils::endsWith(frameInfo.file->filename, btFileSuffix) && groupCount != 0) {
					jointsObtained = BodyTrackingUtils::predictJoints(&framesJson, groupCount + jointFrameModifier, tracker, frameInfo.file->capture, &joints);
					jointFrameModifier += 0.5;
				}
			}
		}

		previousFileName = frameInfo.file->filename;
		k4a_capture_release(frameInfo.file->capture);
		frameInfo.file->capture = NULL;

		// Advance the recording with the lowest current timestamp forward.
		k4a_stream_result_t stream_result = k4a_playback_get_next_capture(frameInfo.file->handle, &frameInfo.file->capture);
		if (stream_result == K4A_STREAM_RESULT_FAILED)
		{
			printf("ERROR: Failed to read next capture from file: %s\n", frameInfo.file->filename.c_str());
			result = K4A_RESULT_FAILED;
			break;
		}

		frame++;
	}

	for (size_t i = 0; i < fileCount; i++)
	{
		if (files[i].handle != NULL)
		{
			k4a_playback_close(files[i].handle);
			files[i].handle = NULL;
		}
	}
	free(files);

	return result == K4A_RESULT_SUCCEEDED ? 0 : 1;
}


text:
#include <vector>
#include <string>
#include <boost/thread/tss.hpp>
#include "cot/connection.h"
#include "cot/exception.h"

namespace cot {

boost::thread_specific_ptr<Connection::Resource> Connection::resource;

void Connection::connect(
    const char * host,
    const char * user,
    const char * password,
    const char * name)
{
    if (resource.get() == NULL) {
        resource.reset(new Resource());
    }

    if (!mysql_init(&resource->connect))
        throw Exception(
            std::string("mysql_init(): ") +
            mysql_error(&resource->connect)
        );

    if (!mysql_real_connect(
            &resource->connect,
            host, user, password, name,
            0,
            NULL,
            0))
        throw Exception(
            std::string("mysql_real_connect()") +
            mysql_error(&resource->connect)
        );
}

MYSQL * Connection::connection()
{
    return &resource->connect;
}

} // namespace cot

max_stars_repo_name: stefantsov/cot

max_issues_repo_name: stefantsov/cot

max_forks_repo_name: stefantsov/cot