text:
//  Copyright (c) 2005-2009 Hartmut Kaiser
//
//  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)

#include <vector>
#include <string>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/string.hpp>

#include <saga/saga/error.hpp>
#include <saga/saga/adaptors/instance_data.hpp>
#include <saga/saga/adaptors/attribute.hpp>
#include <saga/saga/adaptors/packages/cpr_job_cpi_instance_data.hpp>
#include <saga/saga/adaptors/packages/cpr_job_service_cpi_instance_data.hpp>

#include <saga/saga/packages/cpr/version.hpp>
#include <saga/impl/packages/cpr/cpr_job_serialization.hpp>

#include <saga/impl/packages/cpr/cpr_job.hpp>
#include <saga/impl/packages/cpr/cpr_job_service.hpp>
#include <saga/impl/packages/job/job_description.hpp>

///////////////////////////////////////////////////////////////////////////////
// Register the serialization factory type
SAGA_PACKAGE_REGISTER(saga::impl::cpr_job_serialization);

///////////////////////////////////////////////////////////////////////////////
namespace saga { namespace impl
{

    cpr_job_serialization::~cpr_job_serialization()
    {
    }

    void
    serialize_cpr_jd(boost::archive::text_oarchive& oa, saga::cpr::description jd)
    {
        using namespace boost::serialization;

        saga::adaptors::attribute attr(jd);
        std::vector<std::string> keys (attr.list_attributes());
        std::size_t count = keys.size();

        oa << make_nvp("attribute_count", count);
        std::vector<std::string>::iterator end = keys.end();
        for (std::vector<std::string>::iterator it = keys.begin(); it != end;
             ++it)
        {
            bool is_vector_attribute = attr.attribute_is_vector(*it);

            oa << make_nvp("is_vector", is_vector_attribute);
            if (is_vector_attribute) {
            std::vector<std::string> values (attr.get_vector_attribute(*it));

                oa << make_nvp("key", *it);
                oa << make_nvp("values", values);
            }
            else {
            std::string value (attr.get_attribute(*it));

                oa << make_nvp("key", *it);
                oa << make_nvp("value", value);
            }
        }
    }

    saga::cpr::description deserialize_cpr_jd(boost::archive::text_iarchive& ia)
    {
        using namespace boost::serialization;

        saga::cpr::description jd;
        saga::adaptors::attribute attr(jd);
        std::size_t count = 0;

        ia >> make_nvp("attribute_count", count);
        for (std::size_t i = 0; i < count; ++i)
        {
            bool is_vector_attribute = false;
            ia >> make_nvp("is_vector", is_vector_attribute);
            if (is_vector_attribute) {
            std::string key;
            std::vector<std::string> values;

                ia >> make_nvp("key", key);
                ia >> make_nvp("values", values);
                attr.set_vector_attribute(key, values);
            }
            else {
            std::string key, value;

                ia >> make_nvp("key", key);
                ia >> make_nvp("value", value);
                attr.set_attribute(key, value);
            }
        }
        return jd;
    }

    std::string
    cpr_job_serialization::serialize(TR1::shared_ptr<saga::impl::object> obj)
    {
        // get the type of the object (has been serialized already)
        saga::object::type type = obj->get_type();
        if (saga::object::CPRJob     != type  &&
            saga::object::JobService != type  )
        {
            SAGA_THROW_VERBATIM(obj.get(), "job_serialization::serialize: "
                "Unknwon object type", saga::BadParameter);
        }

        SAGA_OSSTREAM strm;
        {
            using namespace boost::serialization;
            using namespace boost::archive;

            // save package version
            text_oarchive oa(strm);
            unsigned int version = SAGA_VERSION_PACKAGE_CPR_FULL;
            oa << make_nvp("version", version);

            if (saga::object::CPRJob == type ) {
            // serialize the cpr_job_cpi_instance data
                typedef saga::adaptors::v1_0::cpr_job_cpi_instance_data
                    instance_data;

            // so the following cast should succeed
            saga::impl::proxy *proxy = dynamic_cast<saga::impl::proxy *>(obj.get());

                if (NULL == proxy)
                {
                    SAGA_THROW_VERBATIM(obj.get(), "cpr_job_serialization::serialize: "
                        "Unknwon object type", saga::BadParameter);
                }

            saga::adaptors::instance_data<instance_data> data(proxy);
            std::string rm (data->rm_.get_url());

                oa << make_nvp("resourcemanager", rm);
                oa << make_nvp("jobid", data->jobid_);

                // serialize the start job description
                serialize_cpr_jd(oa, data->jd_start_);

                // serialize the restart job description
                serialize_cpr_jd(oa, data->jd_restart_);
            }
            else if (saga::object::CPRJobService == type) {
            // serialize the cpr_job_service_cpi_instance_data data
                typedef saga::adaptors::v1_0::cpr_job_service_cpi_instance_data
                    instance_data;

            // so the following cast should succeed
            saga::impl::proxy *proxy = dynamic_cast<saga::impl::proxy *>(obj.get());

                if (NULL == proxy)
                {
                    SAGA_THROW_VERBATIM(obj.get(), "cpr_job_serialization::serialize: "
                        "Unknwon object type", saga::BadParameter);
                }

            saga::adaptors::instance_data<instance_data> data(proxy);
            std::string rm (data->rm_.get_url());

                oa << make_nvp("resourcemanager", rm);
            }
        }
        return SAGA_OSSTREAM_GETSTRING(strm);
    }

    TR1::shared_ptr<saga::impl::object>
    cpr_job_serialization::deserialize(saga::session s, saga::object::type type,
        std::string const& data)
    {
        if (saga::object::CPRJob        != type &&
            saga::object::CPRJobService != type )
        {
            SAGA_THROW_VERBATIM(s, "cpr_job_serialization::deserialize: "
                "Unknwon object type", saga::BadParameter);
        }

        using namespace boost::serialization;
        using namespace boost::archive;

        SAGA_ISSTREAM strm(data);
        text_iarchive ia(strm);

        // save package version
        unsigned int version = 0;
        ia >> make_nvp("version", version);
        if (!SAGA_VERSION_PACKAGE_CPR_ISCOMPATIBLE_EX(version))
        {
            SAGA_THROW_VERBATIM(s, "cpr_job_serialization::deserialize: "
                "Deserialization failed: incompatible version of SAGA cpr_job "
                "package module.", saga::NoSuccess);
        }

        TR1::shared_ptr<saga::impl::object> impl;
        if (saga::object::CPRJob == type) {
        // deserialize the job
        std::string rm;
        std::string jobid;

            ia >> make_nvp("resourcemanager", rm);
            ia >> make_nvp("jobid", jobid);

        saga::cpr::description jd(deserialize_cpr_jd(ia));
        saga::cpr::service js(s, rm);
        saga::cpr::job j (js.get_job(jobid));

            impl = saga::impl::runtime::get_impl_object_sp(j);
        }
        else if (saga::object::CPRJobService == type) {
        // deserialize the service_cpi_instance data
        std::string rm;

            ia >> make_nvp("resourcemanager", rm);
            impl.reset(new saga::impl::cpr_job_service(s, rm));
        }
        return impl;
    }

///////////////////////////////////////////////////////////////////////////////
}}    // namespace saga::impl

max_stars_repo_name: saga-project/saga-cpp

max_issues_repo_name: saga-project/saga-cpp

max_forks_repo_name: saga-project/saga-cpp


text:
//////////////////////////////////////////////////////////////////////////////
//  Boost.Assign v2                                                         //
//                                                                          //
//  Copyright (C) 2003-2004 Thorsten Ottosen                                //
//  Copyright (C) 2011 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ASSIGN_V2_INTERPRETER_DATA_ER_2011_HPP
#define BOOST_ASSIGN_V2_INTERPRETER_DATA_ER_2011_HPP
#include <boost/assign/v2/support/traits/container.hpp>
#include <boost/assign/v2/support/switch.hpp>
#include <boost/assign/v2/support/functor/pair.hpp>
#include <boost/assign/v2/support/functor/value.hpp>
#include <boost/assign/v2/interpreter/fwd.hpp>
#include <boost/assign/v2/option/data.hpp>

namespace boost{
namespace assign{
namespace v2{
//[syntax_interpreter_data
namespace switch_tag{
    struct data_generator{};
}// switch_tag
/*<-*/
#define BOOST_ASSIGN_V2_SWITCH_TAG data_generator
BOOST_ASSIGN_V2_SWITCH_CASE(0, container_aux::is_multi_array, kwd_element_)
BOOST_ASSIGN_V2_SWITCH_CASE(1, container_aux::is_map, kwd_map_)
BOOST_ASSIGN_V2_SWITCH_CASE_DEFAULT(2, kwd_value_)
#undef BOOST_ASSIGN_V2_SWITCH_TAG
/*->*/

namespace interpreter_aux{

    template<
        typename C    // Value- or pointer-container
    >
    struct deduce_data_generator/*<-*/
        : data_generator<
            C,
            typename switch_aux::result<
                switch_tag::data_generator,
                C
            >::type
        >
    {}/*->*/;

}// interpreter_aux
//]
}// v2
}// assign
}// boost

#endif // BOOST_ASSIGN_V2_INTERPRETER_DATA_ER_2011_HPP

ext: hpp


text:
// weak_ptr_alias_test.cpp
//
// Copyright 2007, 2019 Peter Dimov
//
// 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)

#include <boost/core/lightweight_test.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include <memory>
#include <cstddef>

//

class incomplete;

struct X
{
    int v_;

    explicit X( int v ): v_( v )
    {
    }

    ~X()
    {
        v_ = 0;
    }
};

template<class P1, class P2> bool share_ownership( P1 const& p1, P2 const& p2 )
{
    return !p1.owner_before( p2 ) && !p2.owner_before( p1 );
}

int main()
{
    {
        boost::shared_ptr<double> p( new double );

        float m = 0;
        boost::weak_ptr<float> p2( p, &m );

        BOOST_TEST( p2.use_count() == p.use_count() );
        BOOST_TEST( share_ownership( p, p2 ) );
        BOOST_TEST( p2.lock().get() == &m );

        {
            int m2 = 0;
            boost::weak_ptr<int> p3( p2, &m2 );

            BOOST_TEST( p3.use_count() == p2.use_count() );
            BOOST_TEST( share_ownership( p2, p3 ) );
            BOOST_TEST( p3.lock().get() == &m2 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<int const> p3( p, &m2 );

            BOOST_TEST( p3.use_count() == p.use_count() );
            BOOST_TEST( share_ownership( p, p3 ) );
            BOOST_TEST( p3.lock().get() == &m2 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<int volatile> p3( p2, &m2 );

            BOOST_TEST( p3.use_count() == p2.use_count() );
            BOOST_TEST( share_ownership( p2, p3 ) );
            BOOST_TEST( p3.lock().get() == &m2 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<void> p3( p, &m2 );

            BOOST_TEST( p3.use_count() == p.use_count() );
            BOOST_TEST( share_ownership( p, p3 ) );
            BOOST_TEST( p3.lock().get() == &m2 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<void> p3( p2, &m2 );

            BOOST_TEST( p3.use_count() == p2.use_count() );
            BOOST_TEST( share_ownership( p2, p3 ) );
            BOOST_TEST( p3.lock().get() == &m2 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<void const volatile> p3( p, &m2 );

            BOOST_TEST( p3.use_count() == p.use_count() );
            BOOST_TEST( share_ownership( p, p3 ) );
            BOOST_TEST( p3.lock().get() == &m2 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<void const volatile> p3( p2, &m2 );

            BOOST_TEST( p3.use_count() == p2.use_count() );
            BOOST_TEST( share_ownership( p2, p3 ) );
            BOOST_TEST( p3.lock().get() == &m2 );
        }
    }

    {
        boost::shared_ptr<incomplete> p;

        float m = 0;
        boost::weak_ptr<float> p2( p, &m );

        BOOST_TEST( p2.use_count() == p.use_count() );
        BOOST_TEST( share_ownership( p, p2 ) );

        {
            int m2 = 0;
            boost::weak_ptr<int> p3( p2, &m2 );

            BOOST_TEST( p3.use_count() == p2.use_count() );
            BOOST_TEST( share_ownership( p2, p3 ) );
            BOOST_TEST( p3.lock().get() == 0 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<int const> p3( p, &m2 );

            BOOST_TEST( p3.use_count() == p.use_count() );
            BOOST_TEST( share_ownership( p, p3 ) );
            BOOST_TEST( p3.lock().get() == 0 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<int volatile> p3( p2, &m2 );

            BOOST_TEST( p3.use_count() == p2.use_count() );
            BOOST_TEST( share_ownership( p2, p3 ) );
            BOOST_TEST( p3.lock().get() == 0 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<void> p3( p, &m2 );

            BOOST_TEST( p3.use_count() == p.use_count() );
            BOOST_TEST( share_ownership( p, p3 ) );
            BOOST_TEST( p3.lock().get() == 0 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<void> p3( p2, &m2 );

            BOOST_TEST( p3.use_count() == p2.use_count() );
            BOOST_TEST( share_ownership( p2, p3 ) );
            BOOST_TEST( p3.lock().get() == 0 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<void const volatile> p3( p, &m2 );

            BOOST_TEST( p3.use_count() == p.use_count() );
            BOOST_TEST( share_ownership( p, p3 ) );
            BOOST_TEST( p3.lock().get() == 0 );
        }

        {
            int m2 = 0;
            boost::weak_ptr<void const volatile> p3( p2, &m2 );

            BOOST_TEST( p3.use_count() == p2.use_count() );
            BOOST_TEST( share_ownership( p2, p3 ) );
            BOOST_TEST( p3.lock().get() == 0 );
        }
    }

    {
        boost::shared_ptr<X> p( new X( 5 ) );

        boost::weak_ptr<int const> p2( p, &p->v_ );

        BOOST_TEST( p2.use_count() == p.use_count() );
        BOOST_TEST( share_ownership( p, p2 ) );
        BOOST_TEST( p2.lock().get() == &p->v_ );

        p.reset();
        BOOST_TEST( p2.expired() );
    }

    return boost::report_errors();
}

ext: cpp


text: #include <boost/units/base_units/us/fluid_ounce.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include "LoadFile.h"

#include <algorithm>
#include <string>
#include <iostream>

using namespace std;
using namespace fan;

#if ENABLE_ASSIMP
#include <assimp/Importer.hpp>
#include <assimp/scene.h>
#include <assimp/postprocess.h>
#endif  // ENABLE_ASSIMP

#if ENABLE_LIB3DS
#include <lib3ds.h>
#endif  // ENABLE_LIB3DS

#include <boost/shared_ptr.hpp>

#include <fanTriangleMesh.h>
#include <objects/TriangleMeshObject.h>
#include <fanScene.h>

#include <debug/Debug.h>

namespace {

#if ENABLE_ASSIMP
using namespace Assimp;
bool loadPly( char* file,
              fanScene& outScene,
              fanVector3<float>& boundBoxMin,
              fanVector3<float>& boundBoxMax )
{
    Importer importer;
    const aiScene* scene = importer.ReadFile( file, aiProcess_JoinIdenticalVertices | aiProcess_Triangulate );
    if ( !scene ) {
        return false;
    }
    cout << " scene->mNumMeshes " <<  scene->mNumMeshes << endl;
    vector<boost::shared_ptr<fanTriangleMesh> > meshes;
    boost::shared_ptr<fanTriangleMesh> fanMesh;
    bool fixNormals = false;
    for ( size_t i = 0, max = scene->mNumMeshes;
                i < max; ++i ) {
        aiMesh* mesh = scene->mMeshes[i];
        cout << " mesh->mNumVertices " <<  mesh->mNumVertices << endl;
        fanMesh.reset( new fanTriangleMesh() );
        meshes.push_back( fanMesh );
        boost::shared_ptr<fanBufferObject<fanVector3<float> > >
                vertice( new fanBufferObject<fanVector3<float> >(mesh->mNumVertices) );
        boost::shared_ptr<fanBufferObject<fanVector3<float> > >
                normal( new fanBufferObject<fanVector3<float> >(mesh->mNumVertices) );
        fanMesh->mVertices = vertice;
        fanMesh->mNormals = normal;
        if ( mesh->mNormals == NULL ) {
            fixNormals = true;
        }
        for ( size_t j = 0, max = mesh->mNumVertices;
                j < max; ++j ) {
            aiVector3D vector = mesh->mVertices[j];
            vertice->mBuffer[j] = fanVector3<float>(vector.x,
                                                    vector.y,
                                                    vector.z);
            if ( vector.x > boundBoxMax[0] ) {
                boundBoxMax[0] = vector.x;
            }
            if ( vector.x < boundBoxMin[0] ) {
                boundBoxMin[0] = vector.x;
            }
            if ( vector.y > boundBoxMax[1] ) {
                boundBoxMax[1] = vector.y;
            }
            if ( vector.y < boundBoxMin[1] ) {
                boundBoxMin[1] = vector.y;
            }
            if ( vector.z > boundBoxMax[2] ) {
                boundBoxMax[2] = vector.z;
            }
            if ( vector.z < boundBoxMin[2] ) {
                boundBoxMin[2] = vector.z;
            }
            if ( !fixNormals ) {
                vector = mesh->mNormals[j];
                normal->mBuffer[j] = normalize(fanVector3<float>(vector.x,
                                                                 vector.y,
                                                                 vector.z));
            }
        }
        cout << " mesh->mNumFaces " <<  mesh->mNumFaces << endl;
        boost::shared_ptr<fanBufferObject<fanTriangle> >
                face( new fanBufferObject<fanTriangle>(mesh->mNumFaces) );
        fanMesh->mFaces = face;
        for ( size_t j = 0, max = mesh->mNumFaces;
                j < max; ++j ) {
            aiFace aiface = mesh->mFaces[j];
            if ( aiface.mNumIndices != 3 ) {
                continue;
            }
            unsigned int aIndex = aiface.mIndices[0];
            unsigned int bIndex = aiface.mIndices[1];
            unsigned int cIndex = aiface.mIndices[2];
            face->mBuffer[j] = fanTriangle( &vertice->mBuffer[aIndex],
                                            &vertice->mBuffer[bIndex],
                                            &vertice->mBuffer[cIndex],
                                            aIndex, bIndex, cIndex );
        }
    }
    // fix normals
    for ( size_t i = 0, max = fanMesh->mFaces->mSize; i < max; ++i ) {
        fanTriangle& triangle = fanMesh->mFaces->mBuffer[i];
        fanMesh->mNormals->mBuffer[ triangle.pointsIndex[0] ] += triangle.mNormal;
        fanMesh->mNormals->mBuffer[ triangle.pointsIndex[1] ] += triangle.mNormal;
        fanMesh->mNormals->mBuffer[ triangle.pointsIndex[2] ] += triangle.mNormal;
    }
    for ( size_t i = 0, max = fanMesh->mVertices->mSize; i < max; ++i ) {
        fanMesh->mNormals->mBuffer[i] = normalize(fanMesh->mNormals->mBuffer[i]);
    }

    vector<aiNode*> nodes;
    nodes.push_back(scene->mRootNode);
    while ( nodes.size() != 0 ) {
        aiNode* node = nodes[nodes.size()-1];
        nodes.pop_back();
        if ( node == NULL ) {
            continue;
        }
        cout << " node->mNumChildren " << node->mNumChildren << endl;
        for ( size_t i = 0; i < node->mNumChildren; ++i ) {
            nodes.push_back( node->mChildren[i] );
        }
        aiMatrix4x4 pos = node->mTransformation;
        boost::shared_ptr<TriangleMeshObject>
            object( new TriangleMeshObject(
                        fanMatrix< float, 4, 4>{pos.a1,pos.a2,pos.a3,pos.a4,
                                                pos.b1,pos.b2,pos.b3,pos.b4,
                                                pos.c1,pos.c2,pos.c3,pos.c4,
                                                pos.d1,pos.d2,pos.d3,pos.d4, } ) );
        cout << " node->mNumMeshes " << node->mNumMeshes << endl;
        cout << " objectToWorld " << object->mObjectToWorld << endl;
        for ( size_t i = 0; i < node->mNumMeshes; ++i ) {
            object->mMeshes.push_back( meshes[i] );
        }
        outScene.mTriangleMeshObjects.push_back( object );
    }

    return true;
}
#endif  // ENABLE_ASSIMP

#if ENABLE_LIB3DS
bool load3ds( char* file,
              fanScene& outScene,
              fanVector3<float>& boundBoxMin,
              fanVector3<float>& boundBoxMax )
{
    bool loadResult = false;
    Lib3dsFile* _3dsFile = NULL;
    do {
        _3dsFile = lib3ds_file_open( file );
        if ( !_3dsFile ) {
            cerr << "load fail " << file << std::endl;
            break;
        }
        vector<boost::shared_ptr<fanTriangleMesh> > meshes;
        boost::shared_ptr<fanTriangleMesh> fanMesh;
        cout << "nmeshes " << _3dsFile->nmeshes << std::endl;
        for ( int i = 0, max = _3dsFile->nmeshes;
                    i < max; ++i ) {
            Lib3dsMesh* mesh = _3dsFile->meshes[i];
            cout << "nvertices " << mesh->nvertices << std::endl;

            fanMesh.reset( new fanTriangleMesh() );
            meshes.push_back( fanMesh );

            fanMesh->mVertices.reset(
                    new fanBufferObject<fanVector3<float> >( mesh->nvertices ) );
            fanMesh->mFaces.reset(
                    new fanBufferObject<fanTriangle>( mesh->nfaces ) );
            fanMesh->mNormals.reset(
                    new fanBufferObject<fanVector3<float> >( mesh->nvertices ) );

            for ( int j = 0, max = mesh->nvertices;
                    j < max; ++j ) {
                (fanMesh->mVertices)->mBuffer[j] =
                    fanVector3<float>( mesh->vertices[j][0],
                                       mesh->vertices[j][1],
                                       mesh->vertices[j][2] );
                if ( mesh->vertices[j][0] > boundBoxMax[0] ) {
                    boundBoxMax[0] = mesh->vertices[j][0];
                }
                if ( mesh->vertices[j][0] < boundBoxMin[0] ) {
                    boundBoxMin[0] = mesh->vertices[j][0];
                }
                if ( mesh->vertices[j][1] > boundBoxMax[1] ) {
                    boundBoxMax[1] = mesh->vertices[j][1];
                }
                if ( mesh->vertices[j][1] < boundBoxMin[1] ) {
                    boundBoxMin[1] = mesh->vertices[j][1];
                }
                if ( mesh->vertices[j][2] > boundBoxMax[2] ) {
                    boundBoxMax[2] = mesh->vertices[j][2];
                }
                if ( mesh->vertices[j][2] < boundBoxMin[2] ) {
                    boundBoxMin[2] = mesh->vertices[j][2];
                }

            }
            float (*vertices_normals)[3] = (float(*)[3])malloc(sizeof(float) * 9 * mesh->nfaces);
            float (*face_normals)[3] = (float(*)[3])malloc(sizeof(float) * 3 * mesh->nfaces);
            lib3ds_mesh_calculate_vertex_normals( mesh, vertices_normals );
            lib3ds_mesh_calculate_face_normals( mesh, face_normals );
            cout << "nfaces " << mesh->nfaces << std::endl;
            for ( int j = 0, max = mesh->nfaces;
                    j < max; ++j ) {
                Lib3dsFace face = mesh->faces[j];
                (fanMesh->mFaces)->mBuffer[j] =
                    fanTriangle( &(fanMesh->mVertices->mBuffer[face.index[0]]),
                                 &(fanMesh->mVertices->mBuffer[face.index[1]]),
                                 &(fanMesh->mVertices->mBuffer[face.index[2]]),
                                 face.index[0], face.index[1], face.index[2],
                                 fanVector3<float>(),
                                 fanVector3<float>( face_normals[j][0],
                                                    face_normals[j][1],
                                                    face_normals[j][2] ) );
                fanMesh->mNormals->mBuffer[ face.index[0] ] =
                    fanVector3<float>( vertices_normals[3*j][0],
                                       vertices_normals[3*j][1],
                                       vertices_normals[3*j][2] );
                fanMesh->mNormals->mBuffer[ face.index[1] ] =
                    fanVector3<float>( vertices_normals[3*j+1][0],
                                       vertices_normals[3*j+1][1],
                                       vertices_normals[3*j+1][2] );
                fanMesh->mNormals->mBuffer[ face.index[2] ] =
                    fanVector3<float>( vertices_normals[3*j+2][0],
                                       vertices_normals[3*j+2][1],
                                       vertices_normals[3*j+2][2] );
            }
            free( vertices_normals );
            free( face_normals );
        }

        vector<Lib3dsNode*> nodes;
        nodes.push_back(_3dsFile->nodes);
        while ( nodes.size() != 0 ) {
            Lib3dsNode* node = nodes[nodes.size()-1];
            nodes.pop_back();

            if ( node == NULL ) {
                continue;
            }

            nodes.push_back( node->childs );
            nodes.push_back( node->next );

            if ( node->type == LIB3DS_NODE_MESH_INSTANCE ) {
            /*
            Lib3dsMeshInstanceNode* meshNode = (Lib3dsMeshInstanceNode*) node;
            cerr << node->name << " pos " <<
                meshNode->pos[0] << "," << meshNode->pos[1] << "," << meshNode->pos[2] << std::endl;
            cerr << "    rot " << meshNode->rot[0] << "," << meshNode->rot[1] << "," << meshNode->rot[2] << "," << meshNode->rot[3] << std::endl;
            */
                int meshIndex = lib3ds_file_mesh_by_name( _3dsFile, node->name );
                if ( meshIndex != -1 ) {
                    boost::shared_ptr<TriangleMeshObject>
                        object( new TriangleMeshObject(
                                    fanMatrix< float, 4, 4>{ 1,0,0,0,
                                                             0,1,0,0,
                                                             0,0,1,0,
                                                             0,0,0,1, } ) );
                    object->mMeshes.push_back( meshes[meshIndex] );
                    outScene.mTriangleMeshObjects.push_back( object );
                }
            }
        }
        cout << "triangleMeshObjects.size() " << outScene.mTriangleMeshObjects.size() << std::endl;
        loadResult = true;
    } while ( false );

    if ( _3dsFile ) {
        lib3ds_file_free( _3dsFile );
    }
    return loadResult;
}
#endif  // ENABLE_LIB3DS

}

bool loadFile( char* file,
               fan::fanScene& outScene,
               fan::fanVector3<float>& boundBoxMin,
               fan::fanVector3<float>& boundBoxMax )
{
    (void) outScene; (void) boundBoxMax; (void) boundBoxMin;
    string filename( file );

    size_t extensionSeperatorPos = filename.rfind( '.' );
    if ( extensionSeperatorPos == string::npos ) {
        cerr << "Could not find file extension." << std::endl;
        return false;
    }
    string filenameExtension = filename.substr( extensionSeperatorPos + 1,
                                                filename.length() );
    std::transform( filenameExtension.begin(), filenameExtension.end(),
                    filenameExtension.begin(), ::tolower );

#if ENABLE_ASSIMP
    if ( filenameExtension == "ply" ) {
        return loadPly( file, outScene, boundBoxMin, boundBoxMax );
    } else
#endif  // ENABLE_ASSIMP
#if ENABLE_LIB3DS
    if ( filenameExtension == "3ds" ) {
        return load3ds( file, outScene, boundBoxMin, boundBoxMax );
    } else
#endif  // ENABLE_LIB3DS
    {
        cerr << "file type " << filenameExtension << " not supported." << std::endl;
    }
    return false;
}

max_stars_repo_name: aurthconan/fanLens

max_issues_repo_name: aurthconan/fanLens

max_forks_repo_name: aurthconan/fanLens


text: #include <boost/units/physical_dimensions/pressure.hpp>


text:
// External headers.
#ifndef INCLUDED_BOOST_PYTHON
#include <boost/python.hpp>
#define INCLUDED_BOOST_PYTHON
#endif

// Project headers.
#ifndef INCLUDED_CALC_SPATIAL
#include "calc_spatial.h"
#define INCLUDED_CALC_SPATIAL
#endif

// Module headers.
#ifndef INCLUDED_MOC
#include "Moc.h"
#define INCLUDED_MOC
#endif

BOOST_PYTHON_MODULE(_pcraster_moc) {
  namespace bp = boost::python;
  namespace mp = moc::python;

  bp::class_<mp::Moc, boost::noncopyable>(
         "initialise",
         bp::init<geo::RasterSpace const&, double, UINT4, calc::Field const*,
              calc::Field const*, calc::Field const*>())
    .def("transport", &mp::Moc::transport)
    .def("adjust", &mp::Moc::adjust)
    ;
}

ext: cc

max_stars_repo_name: quanpands/wflow

max_issues_repo_name: quanpands/wflow

max_forks_repo_name: quanpands/wflow


text:
//
// connection.hpp
// ~~~~~~~~~~~~~~
//
// Copyright (c) 2003-2008 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// 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)
//

#ifndef HTTP_CONNECTION_HPP
#define HTTP_CONNECTION_HPP

#include "stdafx.h"

#include <queue>

#include <boost/asio.hpp>
#include <boost/asio/steady_timer.hpp>
#include <boost/array.hpp>
#include <boost/noncopyable.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>

#include "reply.hpp"
#include "request.hpp"
#include "request_handler.hpp"
#include "request_parser.hpp"
#include "AssetServer.h"
#include "Settings.h"

namespace http {
namespace server {

class connection_manager;

/// Represents a single connection from a client.
class connection
  : public boost::enable_shared_from_this<connection>,
    private boost::noncopyable
{
public:
  /// Construct a connection with the given io_service.
  explicit connection(boost::asio::io_service& io_service,
      connection_manager& manager, request_handler& handler);

  /// Get the socket associated with the connection.
  boost::asio::ip::tcp::socket& socket();

  /// Start the first asynchronous operation for the connection.
  void start();

  /// Stop all asynchronous operations associated with the connection.
  void stop();

private:
  /// Maximum time for this connection to stay open for requests in seconds
  static const int MAXIMUM_LIVE_TIME = 30;

  /// Handle completion of a read operation.
  void handle_read(const boost::system::error_code& e,
      std::size_t bytes_transferred);

  /// Handle completion of a write operation.
  void handle_write(const boost::system::error_code& e);

  /// Handle completion of a header write operation.
  void handle_tb_header_write(const boost::system::error_code& e);

  /// Used with token bucket throttling. Checks the token bucket for tokens
  /// and tries to send the next piece of the response
  void try_send_next_chunk();

   /// Handle completion of a token bucket based write operation.
  void handle_tb_write(const boost::system::error_code& e,
	  size_t bytes_transferred);

  void reply_ready();

  void init_new_request();

  void process_next_request();

  void on_timeout(const boost::system::error_code& e);

  void on_tb_wait_timeout(const boost::system::error_code& e);

  void reset_timeout_timer();

  /// Socket for the connection.
  boost::asio::ip::tcp::socket socket_;

  /// The manager for this connection.
  connection_manager& connection_manager_;

  /// The handler used to process the incoming request.
  request_handler& request_handler_;

  /// Buffer for incoming data.
  boost::array<char, 8192> buffer_;

  /// A list of waiting requests
  std::queue<request::ptr> requests_;

  /// The parser for the incoming request.
  request_parser request_parser_;

  /// The reply to be sent back to the client.
  reply reply_;

  /// Asset server we're working with
  whip::AssetServer::ptr _assetServer;

  /// Time this connection was last active
  boost::posix_time::ptime _lastActiveTime;

  /// Whether or not to close the connection after the response is written
  bool _closeAfterResponseWritten;

  /// If debugging is enabled
  bool _debug;

  /// Closes the connection after MAXIMUM_LIVE_TIME
  boost::asio::deadline_timer _connKillTimer;

  /// Whether or not a request is being processed
  bool _requestInProgress;

  boost::asio::steady_timer _tokenBucketTimer;
};

typedef boost::shared_ptr<connection> connection_ptr;

} // namespace server
} // namespace http

#endif // HTTP_CONNECTION_HPP

max_stars_repo_name: llaxton/M-O-S-E-S-aperture

max_issues_repo_name: llaxton/M-O-S-E-S-aperture

max_forks_repo_name: llaxton/M-O-S-E-S-aperture


text:
////////////////////////////////////////////////////////////////////////////
//	Module 		: editor_environment_thunderbolts_gradient.hpp
//	Created 	: 04.01.2008
//  Modified 	: 10.01.2008
//	Author		: Dmitriy Iassenev
//	Description : editor environment thunderbolts gradient class
////////////////////////////////////////////////////////////////////////////

#ifndef EDITOR_WEATHER_THUNDERBOLTS_GRADIENT_HPP_INCLUDED
#define EDITOR_WEATHER_THUNDERBOLTS_GRADIENT_HPP_INCLUDED

#ifdef INGAME_EDITOR

#include <boost/noncopyable.hpp>
#include "../include/editor/property_holder.hpp"
#include "thunderbolt.h"

namespace editor {

class property_holder;

namespace environment {

class manager;

namespace thunderbolts {

class gradient :
	public SThunderboltDesc::SFlare,
	private boost::noncopyable
{
public:
						gradient		();
						~gradient		();
			void		load			(CInifile& config, shared_str const& section_id, LPCSTR prefix);
			void		save			(CInifile& config, shared_str const& section_id, LPCSTR prefix);
			void		fill			(
							::editor::environment::manager& environment,
							LPCSTR name,
							LPCSTR description,
							editor::property_holder& holder
						);

private:
	LPCSTR	xr_stdcall	shader_getter	() const;
	void	xr_stdcall	shader_setter	(LPCSTR	value);

	LPCSTR	xr_stdcall	texture_getter	() const;
	void	xr_stdcall	texture_setter	(LPCSTR	value);

private:
	property_holder*	m_property_holder;
}; // class gradient

} // namespace thunderbolts
} // namespace environment
} // namespace editor

#endif // #ifdef INGAME_EDITOR

#endif // ifndef EDITOR_WEATHER_THUNDERBOLTS_GRADIENT_HPP_INCLUDED


text:
#include <database/MySQLGunzDB.h>

#include <cstddef>
#include <cassert>

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

using namespace boost;
using namespace mysqlpp;

static void make_account_info_query(Query& q, const char* username, const char* password)
{
	q << "SELECT aid, ugradeid, pgradeid FROM account"
	     " WHERE username=" << mysqlpp::quote << username <<
	     " AND password=md5(" << mysqlpp::quote << password << ")"
	     " LIMIT 1";
}

static AccountInfo handle_account_info_query(const StoreQueryResult& result, const char* username)
{
	size_t rowCount = result.num_rows();

	assert(rowCount < 2);

	if(rowCount == 0)
		throw InvalidAccountInfo();

	const Row& row = result[0];

	AccountInfo ret(row["aid"], username, row["ugradeid"], row["pgradeid"]);

	if ((ret.access == 253) || (ret.access == 105))
		throw BannedUser(username);

	return ret;
}

AccountInfo MySQLGunzDB::GetAccountInfo(const std::string& user, const std::string& password)
{
	return run_query<AccountInfo>(
		boost::bind(make_account_info_query, _1, user.c_str(), password.c_str()),
		boost::bind(handle_account_info_query, _1, user.c_str())
	);
}

max_stars_repo_name: gogo-dev/GoGo

max_issues_repo_name: gogo-dev/GoGo

max_forks_repo_name: gogo-dev/GoGo


text:
/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "apps-precomp.h"  // Precompiled headers
//
#include <mrpt/apps/MonteCarloLocalization_App.h>
#include <mrpt/bayes/CParticleFilter.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimpleMap.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/math/distributions.h>
#include <mrpt/math/ops_vectors.h>	// << for vector<>
#include <mrpt/math/utils.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/CDisk.h>
#include <mrpt/opengl/CEllipsoid2D.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose2DInterpolator.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/slam/CMonteCarloLocalization2D.h>
#include <mrpt/slam/CMonteCarloLocalization3D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <Eigen/Dense>

using namespace mrpt::apps;

// ---------------------------------------
//   MonteCarloLocalization_Base
// ---------------------------------------
MonteCarloLocalization_Base::MonteCarloLocalization_Base()
{
	// Set logger display name:
	this->setLoggerName("MonteCarloLocalization_Base");
}

void MonteCarloLocalization_Base::initialize(int argc, const char** argv)
{
	MRPT_START

	MRPT_LOG_INFO_FMT(
		" pf-localization - Part of the MRPT\n"
		" MRPT C++ Library: %s - Sources timestamp: %s\n\n",
		mrpt::system::MRPT_getVersion().c_str(),
		mrpt::system::MRPT_getCompilationDate().c_str());

	// Process arguments:
	if (argc < 2)
	{ THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str()); }

	// Config file:
	const std::string configFile = std::string(argv[1]);

	ASSERT_FILE_EXISTS_(configFile);
	params.setContent(mrpt::io::file_get_contents(configFile));

	impl_initialize(argc, argv);

	MRPT_END
}

// All these "using"s, to easily reuse old code from the pf-localization app:
using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::opengl;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace mrpt::bayes;
using namespace mrpt::obs;
using namespace mrpt::io;
using namespace mrpt::img;
using namespace mrpt::config;
using namespace mrpt::serialization;
using namespace std;

// Auxiliary trait classes for template implementations below:
template <class PDF>
struct pf2gauss_t
{
};

template <>
struct pf2gauss_t<mrpt::slam::CMonteCarloLocalization2D>
{
	using type = mrpt::poses::CPosePDFGaussian;
	static constexpr bool PF_IS_3D = false;

	static void resetOnFreeSpace(
		CMonteCarloLocalization2D& pdf, mrpt::maps::CMultiMetricMap& metricMap,
		size_t PARTICLE_COUNT, const mrpt::math::TPose3D& init_min,
		const mrpt::math::TPose3D& init_max)
	{
		pdf.resetUniformFreeSpace(
			metricMap.mapByClass<COccupancyGridMap2D>().get(), 0.7f,
			PARTICLE_COUNT, init_min.x, init_max.x, init_min.y, init_max.y,
			init_min.yaw, init_max.yaw);
	}

	static void resetUniform(
		CMonteCarloLocalization2D& pdf, size_t PARTICLE_COUNT,
		const mrpt::math::TPose3D& init_min,
		const mrpt::math::TPose3D& init_max)
	{
		pdf.resetUniform(
			init_min.x, init_max.x, init_min.y, init_max.y, init_min.yaw,
			init_max.yaw, PARTICLE_COUNT);
	}
};
template <>
struct pf2gauss_t<CMonteCarloLocalization3D>
{
	using type = CPose3DPDFGaussian;
	static constexpr bool PF_IS_3D = true;

	static void resetOnFreeSpace(
		[[maybe_unused]] CMonteCarloLocalization3D& pdf,
		[[maybe_unused]] mrpt::maps::CMultiMetricMap& metricMap,
		[[maybe_unused]] size_t PARTICLE_COUNT,
		[[maybe_unused]] const mrpt::math::TPose3D& init_min,
		[[maybe_unused]] const mrpt::math::TPose3D& init_max)
	{
		THROW_EXCEPTION("init_PDF_mode=0 not supported for 3D particles");
	}

	static void resetUniform(
		CMonteCarloLocalization3D& pdf, size_t PARTICLE_COUNT,
		const mrpt::math::TPose3D& init_min,
		const mrpt::math::TPose3D& init_max)
	{
		pdf.resetUniform(init_min, init_max, PARTICLE_COUNT);
	}
};

template <class MONTECARLO_TYPE>
void MonteCarloLocalization_Base::do_pf_localization()
{
	auto& cfg = this->params;  // rename for reusing old code

	// Number of initial particles (if size>1, run the experiments N times)
	std::vector<int> particles_count;

	// Load parameters:

	// Mandatory entries:
	cfg.read_vector(
		sect, "particles_count", std::vector<int>(1, 0), particles_count,
		/*Fail if not found*/ true);
	string OUT_DIR_PREFIX =
		cfg.read_string(sect, "logOutput_dir", "", /*Fail if not found*/ true);

	// Non-mandatory entries:
	string MAP_FILE = cfg.read_string(sect, "map_file", "");
	size_t rawlog_offset = cfg.read_int(sect, "rawlog_offset", 0);
	string GT_FILE = cfg.read_string(sect, "ground_truth_path_file", "");
	const auto NUM_REPS = cfg.read_uint64_t(sect, "experimentRepetitions", 1);
	int SCENE3D_FREQ = cfg.read_int(sect, "3DSceneFrequency", 10);
	bool SCENE3D_FOLLOW = cfg.read_bool(sect, "3DSceneFollowRobot", true);
	unsigned int testConvergenceAt =
		cfg.read_int(sect, "experimentTestConvergenceAtStep", -1);

	bool SAVE_STATS_ONLY = cfg.read_bool(sect, "SAVE_STATS_ONLY", false);
	bool DO_RELIABILITY_ESTIMATE = false;
	bool DO_SCAN_LIKELIHOOD_DEBUG = false;
	MRPT_LOAD_CONFIG_VAR(DO_RELIABILITY_ESTIMATE, bool, cfg, sect);
	MRPT_LOAD_CONFIG_VAR(DO_SCAN_LIKELIHOOD_DEBUG, bool, cfg, sect);

	bool SHOW_PROGRESS_3D_REAL_TIME =
		cfg.read_bool(sect, "SHOW_PROGRESS_3D_REAL_TIME", false);
	int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS =
		cfg.read_int(sect, "SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS", 1);
	double STATS_CONF_INTERVAL =
		cfg.read_double(sect, "STATS_CONF_INTERVAL", 0.2);

	CPose2D initial_odo;
	initial_odo.x(cfg.read_double(sect, "initial_odo_x", 0));
	initial_odo.y(cfg.read_double(sect, "initial_odo_y", 0));
	initial_odo.phi(cfg.read_double(sect, "initial_odo_phi", 0));

#if !MRPT_HAS_WXWIDGETS
	SHOW_PROGRESS_3D_REAL_TIME = false;
#endif

	// Default odometry uncertainty parameters in "actOdom2D_params" depending
	// on how fast the robot moves, etc...
	//  Only used for observations-only rawlogs:
	CActionRobotMovement2D::TMotionModelOptions actOdom2D_params;
	actOdom2D_params.modelSelection = CActionRobotMovement2D::mmGaussian;
	actOdom2D_params.gaussianModel.minStdXY =
		cfg.read_double("DummyOdometryParams", "minStdXY", 0.04);
	actOdom2D_params.gaussianModel.minStdPHI =
		DEG2RAD(cfg.read_double("DummyOdometryParams", "minStdPHI", 2.0));

	CActionRobotMovement3D::TMotionModelOptions actOdom3D_params;
	actOdom3D_params.mm6DOFModel.additional_std_XYZ =
		cfg.read_double("DummyOdometryParams", "additional_std_XYZ", 0.01);
	actOdom3D_params.mm6DOFModel.additional_std_angle = DEG2RAD(
		cfg.read_double("DummyOdometryParams", "additional_std_angle", 0.1));

	// PF-algorithm Options:
	// ---------------------------
	CParticleFilter::TParticleFilterOptions pfOptions;
	pfOptions.loadFromConfigFile(cfg, "PF_options");

	// PDF Options:
	// ------------------
	TMonteCarloLocalizationParams pdfPredictionOptions;
	pdfPredictionOptions.KLD_params.loadFromConfigFile(cfg, "KLD_options");

	// Metric map options:
	// -----------------------------
	TSetOfMetricMapInitializers mapList;
	mapList.loadFromConfigFile(cfg, "MetricMap");

	MRPT_LOG_INFO_STREAM(
		"" << endl
		   << "MAP_FILE       : " << MAP_FILE << endl
		   << "GT_FILE        : " << GT_FILE << endl
		   << "OUT_DIR_PREFIX : " << OUT_DIR_PREFIX << endl
		   << "particles_count :" << particles_count << endl);

	// --------------------------------------------------------------------
	//						EXPERIMENT PREPARATION
	// --------------------------------------------------------------------
	CTicTac tictac;
	CSimpleMap simpleMap;
	CParticleFilter::TParticleFilterStats PF_stats;

	// Load the set of metric maps to consider in the experiments:
	auto metricMap = CMultiMetricMap::Create();
	metricMap->setListOfMaps(mapList);

	{
		std::stringstream ss;
		pfOptions.dumpToTextStream(ss);
		mapList.dumpToTextStream(ss);

		MRPT_LOG_INFO_STREAM(ss.str());
	}

	getRandomGenerator().randomize();

	// Load the map (if any):
	// -------------------------
	if (MAP_FILE.size())
	{
		ASSERT_(fileExists(MAP_FILE));

		// Detect file extension:
		// -----------------------------
		string mapExt = lowerCase(extractFileExtension(
			MAP_FILE, true));  // Ignore possible .gz extensions

		if (!mapExt.compare("simplemap"))
		{
			// It's a ".simplemap":
			// -------------------------
			MRPT_LOG_INFO_STREAM("Loading '.simplemap' file: " << MAP_FILE);
			{
				CFileGZInputStream f(MAP_FILE);
				archiveFrom(f) >> simpleMap;
			}
			MRPT_LOG_INFO_STREAM(
				"Map loaded ok, with " << simpleMap.size() << " frames.");

			ASSERT_(simpleMap.size() > 0);

			// Build metric map:
			// ------------------------------
			MRPT_LOG_INFO("Building metric map(s) from '.simplemap'...");
			metricMap->loadFromProbabilisticPosesAndObservations(simpleMap);
			MRPT_LOG_INFO("Done.");
		}
		else if (!mapExt.compare("gridmap"))
		{
			// It's a ".gridmap":
			// -------------------------
			MRPT_LOG_INFO("Loading gridmap from '.gridmap'...");
			auto grid = metricMap->mapByClass<COccupancyGridMap2D>();
			ASSERT_(grid);
			{
				CFileGZInputStream f(MAP_FILE);
				archiveFrom(f) >> (*grid);
			}
			MRPT_LOG_INFO("Done.");
		}
		else
		{
			THROW_EXCEPTION_FMT(
				"Map file has unknown extension: '%s'", mapExt.c_str());
		}
	}

	// Load the Ground Truth:
	CMatrixDouble GT(0, 0);
	if (fileExists(GT_FILE))
	{
		MRPT_LOG_INFO("Loading ground truth file...");
		GT.loadFromTextFile(GT_FILE);
		MRPT_LOG_INFO("Done.");

		prepareGT(GT);
	}
	else
		MRPT_LOG_INFO("Ground truth file: not available.");

	// PDF initialization uniform distribution limits:
	const auto init_min = mrpt::math::TPose3D(
		cfg.read_double(sect, "init_PDF_min_x", 0),
		cfg.read_double(sect, "init_PDF_min_y", 0),
		cfg.read_double(sect, "init_PDF_min_z", 0),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_yaw_deg", -180.0)),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_pitch_deg", 0)),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_roll_deg", 0)));

	const auto init_max = mrpt::math::TPose3D(
		cfg.read_double(sect, "init_PDF_max_x", 0),
		cfg.read_double(sect, "init_PDF_max_y", 0),
		cfg.read_double(sect, "init_PDF_max_z", 0),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_yaw_deg", +180.0)),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_pitch_deg", 0)),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_roll_deg", 0)));

	MRPT_LOG_INFO_STREAM(
		"Initial PDF limits:\n Min=" << init_min.asString()
									 << "\n Max=" << init_max.asString());

	// Gridmap / area of initial uncertainty:
	COccupancyGridMap2D::TEntropyInfo gridInfo;
	if (auto grid = metricMap->mapByClass<COccupancyGridMap2D>(); grid)
	{
		grid->computeEntropy(gridInfo);
		MRPT_LOG_INFO_FMT(
			"The gridmap has %.04fm2 observed area, %u observed cells.",
			gridInfo.effectiveMappedArea,
			(unsigned)gridInfo.effectiveMappedCells);
	}
	else
	{
		gridInfo.effectiveMappedArea =
			(init_max.x - init_min.x) * (init_max.y - init_min.y);
	}

	for (int PARTICLE_COUNT : particles_count)
	{
		MRPT_LOG_INFO_FMT(
			"Initial PDF: %f particles/m2",
			PARTICLE_COUNT / gridInfo.effectiveMappedArea);

		// Global stats for all the experiment loops:
		int nConvergenceTests = 0, nConvergenceOK = 0;
		CVectorDouble convergenceErrors;
		std::mutex convergenceErrors_mtx;

		// --------------------------------------------------------------------
		//					EXPERIMENT REPETITIONS LOOP
		// --------------------------------------------------------------------

		// Generate list of repetition indices:
		std::vector<unsigned int> rep_indices(NUM_REPS);
		std::iota(rep_indices.begin(), rep_indices.end(), 0);

		// Run each repetition in parallel:
		// GCC 7 still has not implemented C++17 for_each(std::execution::par
		// So: home-made parallel loops:

		// std::for_each(std::execution::par, rep_indices.begin(),
		// rep_indices.end(),
		auto run_localization_code = [&](const size_t repetition) {
			CVectorDouble indivConvergenceErrors, executionTimes, odoError;
			MRPT_LOG_INFO_STREAM(
				"====== RUNNING FOR " << PARTICLE_COUNT
									  << " INITIAL PARTICLES  - Repetition "
									  << 1 + repetition << " / " << NUM_REPS);

			// Create 3D window if requested:
			CDisplayWindow3D::Ptr win3D;
			if (SHOW_PROGRESS_3D_REAL_TIME)
			{
				win3D = std::make_shared<CDisplayWindow3D>(
					"pf-localization - The MRPT project", 1000, 600);
				win3D->setCameraAzimuthDeg(-45);
			}

			// Create the 3D scene and get the map only once, later we'll modify
			// only the particles, etc..
			COpenGLScene scene;
			if (SCENE3D_FREQ > 0 || SHOW_PROGRESS_3D_REAL_TIME)
			{
				mrpt::math::TBoundingBoxf bbox({-50, -50, 0}, {50, 50, 0});
				if (auto pts = metricMap->getAsSimplePointsMap(); pts)
					bbox = pts->boundingBox();

				scene.insert(mrpt::opengl::CGridPlaneXY::Create(
					bbox.min.x, bbox.max.x, bbox.min.y, bbox.max.y, 0, 5));

				if (win3D)
					win3D->setCameraZoom(
						2 *
						std::max(
							bbox.max.x - bbox.min.x, bbox.max.y - bbox.min.y));

				scene.insert(metricMap->getVisualization());
			}

			// The experiment directory is:
			string sOUT_DIR_PARTS, sOUT_DIR_3D;
			const auto sOUT_DIR = format(
				"%s_%03u_%07i", OUT_DIR_PREFIX.c_str(),
				static_cast<unsigned int>(repetition), PARTICLE_COUNT);
			MRPT_LOG_INFO_FMT("Creating directory: %s", sOUT_DIR.c_str());
			deleteFilesInDirectory(sOUT_DIR);
			createDirectory(sOUT_DIR);
			ASSERT_DIRECTORY_EXISTS_(sOUT_DIR);

			if (!SAVE_STATS_ONLY)
			{
				sOUT_DIR_PARTS = format("%s/particles", sOUT_DIR.c_str());
				sOUT_DIR_3D = format("%s/3D", sOUT_DIR.c_str());

				MRPT_LOG_INFO_FMT(
					"Creating directory: %s", sOUT_DIR_PARTS.c_str());
				deleteFilesInDirectory(sOUT_DIR_PARTS);
				createDirectory(sOUT_DIR_PARTS);
				ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_PARTS);

				MRPT_LOG_INFO_FMT(
					"Creating directory: %s", sOUT_DIR_3D.c_str());
				deleteFilesInDirectory(sOUT_DIR_3D);
				createDirectory(sOUT_DIR_3D);
				ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_3D);

				using namespace std::string_literals;
				metricMap->saveMetricMapRepresentationToFile(
					sOUT_DIR + "/map"s);
			}

			MONTECARLO_TYPE pdf;

			// PDF Options:
			pdf.options = pdfPredictionOptions;

			pdf.options.metricMap = metricMap;

			// Create the PF object:
			CParticleFilter PF;
			PF.m_options = pfOptions;

			size_t step = 0;
			size_t rawlogEntry = 0;

			// Initialize the PDF:
			// -----------------------------
			tictac.Tic();
			if (!cfg.read_bool(
					sect, "init_PDF_mode", false,
					/*Fail if not found*/ true))
			{
				// Reset uniform on free space:
				pf2gauss_t<MONTECARLO_TYPE>::resetOnFreeSpace(
					pdf, *metricMap, PARTICLE_COUNT, init_min, init_max);
			}
			else
			{
				// Reset uniform:
				pf2gauss_t<MONTECARLO_TYPE>::resetUniform(
					pdf, PARTICLE_COUNT, init_min, init_max);
			}

			MRPT_LOG_INFO_FMT(
				"PDF of %u particles initialized in %.03fms", PARTICLE_COUNT,
				1000 * tictac.Tac());

			pdf.saveToTextFile(
				format("%s/particles_0_initial.txt", sOUT_DIR_PARTS.c_str()));

			// -----------------------------
			//		Particle filter
			// -----------------------------
			CPose2D odometryEstimation = initial_odo;

			using PDF_MEAN_TYPE = typename MONTECARLO_TYPE::type_value;

			PDF_MEAN_TYPE pdfEstimation;

			bool end = false;

			CFileOutputStream f_cov_est, f_pf_stats, f_odo_est;

			if (!SAVE_STATS_ONLY)
			{
				f_cov_est.open(sOUT_DIR.c_str() + string("/cov_est.txt"));
				f_pf_stats.open(sOUT_DIR.c_str() + string("/PF_stats.txt"));
				f_odo_est.open(sOUT_DIR.c_str() + string("/odo_est.txt"));
			}

			Clock::time_point cur_obs_timestamp;
			CPose2D last_used_abs_odo(0, 0, 0),
				pending_most_recent_odo(0, 0, 0);

			for (; !end; step++)
			{
				// Finish if ESC is pushed:
				if (allow_quit_on_esc_key && os::kbhit())
					if (os::getch() == 27)
					{
						end = true;
						break;
					}

				// Load pose change from the rawlog:
				// ----------------------------------------
				CActionCollection::Ptr action;
				CSensoryFrame::Ptr observations;
				CObservation::Ptr obs;

				if (!impl_get_next_observations(action, observations, obs))
				{
					// EOF
					end = true;
					break;
				}

				// Determine if we are reading a Act-SF or an Obs-only
				// rawlog:
				if (obs)
				{
					// It's an observation-only rawlog: build an auxiliary
					// pair of action-SF, since
					//  montecarlo-localization only accepts those pairs as
					//  input:

					// If it's an odometry reading, don't feed it to the PF.
					// Instead,
					// store its value for use as an "action" together with
					// the next actual observation:
					if (IS_CLASS(*obs, CObservationOdometry))
					{
						auto obs_odo =
							std::dynamic_pointer_cast<CObservationOdometry>(
								obs);
						pending_most_recent_odo = obs_odo->odometry;
						static bool is_1st_odo = true;
						if (is_1st_odo)
						{
							is_1st_odo = false;
							last_used_abs_odo = pending_most_recent_odo;
						}
						continue;
					}
					else
					{
						// SF: Just one observation:
						// ------------------------------------------------------
						observations = std::make_shared<CSensoryFrame>();
						observations->insert(obs);

						// ActionCollection: Just one action with a dummy
						// odometry
						// ------------------------------------------------------
						action = std::make_shared<CActionCollection>();

						if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
						{
							CActionRobotMovement3D actOdom3D;

							const CPose3D odo_incr = CPose3D(
								pending_most_recent_odo - last_used_abs_odo);
							last_used_abs_odo = pending_most_recent_odo;

							actOdom3D.computeFromOdometry(
								odo_incr, actOdom3D_params);
							action->insert(actOdom3D);
						}
						else
						{
							CActionRobotMovement2D actOdom2D;

							const CPose2D odo_incr =
								pending_most_recent_odo - last_used_abs_odo;
							last_used_abs_odo = pending_most_recent_odo;

							actOdom2D.computeFromOdometry(
								odo_incr, actOdom2D_params);
							action->insert(actOdom2D);
						}
					}
				}
				else
				{
					// Already in Act-SF format, nothing else to do!
				}

				CPose2D expectedPose;  // Ground truth

				if (observations->size() > 0)
					cur_obs_timestamp =
						observations->getObservationByIndex(0)->timestamp;
				else if (obs)
					cur_obs_timestamp = obs->timestamp;

				// Test for end condition if we are testing convergence:
				if (step == testConvergenceAt)
				{
					nConvergenceTests++;

					// Convergence??
					if (sqrt(pdf.getCovariance().det()) < 2)
					{
						if (pdfEstimation.distanceTo(expectedPose) < 2)
							nConvergenceOK++;
					}
					end = true;
				}

				if (step < rawlog_offset) continue;

				// Do not execute the PF at "step=0", to let the initial
				// PDF to be reflected in the logs.
				if (step > rawlog_offset)
				{
					// Show 3D?
					if (SHOW_PROGRESS_3D_REAL_TIME)
					{
						const auto [cov, meanPose] = pdf.getCovarianceAndMean();

						if (rawlogEntry >= 2)
							getGroundTruth(
								expectedPose, rawlogEntry - 2, GT,
								cur_obs_timestamp);

						// The particles' cov:
						{
							CRenderizable::Ptr ellip =
								scene.getByName("parts_cov");
							if (!ellip)
							{
								if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
								{
									auto el = CEllipsoid3D::Create();
									ellip =
										mrpt::ptr_cast<CRenderizable>::from(el);
									el->setLineWidth(2);
									el->setQuantiles(3);
									el->enableDrawSolid3D(false);
								}
								else
								{
									auto el = CEllipsoid2D::Create();
									ellip =
										mrpt::ptr_cast<CRenderizable>::from(el);
									el->setLineWidth(2);
									el->setQuantiles(3);
									el->enableDrawSolid3D(false);
								}
								ellip->setName("parts_cov");
								ellip->setColor(1, 0, 0, 0.6);
								scene.insert(ellip);
							}
							else
							{
								if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
								{
									mrpt::ptr_cast<CEllipsoid3D>::from(ellip)
										->setCovMatrix(
											cov.template blockCopy<3, 3>());
								}
								else
								{
									mrpt::ptr_cast<CEllipsoid2D>::from(ellip)
										->setCovMatrix(
											cov.template blockCopy<2, 2>());
								}
							}
							double ellipse_z =
								mrpt::poses::CPose3D(meanPose).z() + 0.01;

							ellip->setLocation(
								meanPose.x(), meanPose.y(), ellipse_z);
						}

						COpenGLScene::Ptr ptrSceneWin =
							win3D->get3DSceneAndLock();

						win3D->setCameraPointingToPoint(
							meanPose.x(), meanPose.y(), 0);

						win3D->addTextMessage(
							10, 10,
							mrpt::format(
								"timestamp: %s",
								mrpt::system::dateTimeLocalToString(
									cur_obs_timestamp)
									.c_str()),
							6001);

						win3D->addTextMessage(
							10, 33,
							mrpt::format(
								"#particles= %7u",
								static_cast<unsigned int>(pdf.size())),
							6002);

						win3D->addTextMessage(
							10, 55,
							mrpt::format(
								"mean pose (x y phi_deg)= %s",
								meanPose.asString().c_str()),
							6003);

						*ptrSceneWin = scene;
						win3D->unlockAccess3DScene();

						// Update:
						win3D->forceRepaint();

						std::this_thread::sleep_for(std::chrono::milliseconds(
							SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
					}  // end show 3D real-time

					// ----------------------------------------
					// RUN ONE STEP OF THE PARTICLE FILTER:
					// ----------------------------------------
					tictac.Tic();
					if (!SAVE_STATS_ONLY)
					{
						MRPT_LOG_INFO_STREAM(
							"Step " << step << ". Executing ParticleFilter on "
									<< pdf.particlesCount() << " particles...");
					}

					PF.executeOn(
						pdf,
						action.get(),  // Action
						observations.get(),	 // Obs.
						&PF_stats  // Output statistics
					);

					double run_time = tictac.Tac();
					executionTimes.push_back(run_time);
					if (!SAVE_STATS_ONLY)
					{
						MRPT_LOG_INFO_FMT(
							"Done in %.03fms, ESS=%f", 1e3 * run_time,
							pdf.ESS());
					}
				}

				// Avrg. error:
				// ----------------------------------------
				CActionRobotMovement2D::Ptr best_mov_estim =
					action->getBestMovementEstimation();
				if (best_mov_estim)
				{
					odometryEstimation = odometryEstimation +
						best_mov_estim->poseChange->getMeanVal();
				}

				pdf.getMean(pdfEstimation);

				// save estimated mean in history:
				if (cur_obs_timestamp != INVALID_TIMESTAMP &&
					fill_out_estimated_path)
				{
					out_estimated_path.insert(
						cur_obs_timestamp,
						mrpt::math::TPose3D(pdfEstimation.asTPose()));
				}

				getGroundTruth(
					expectedPose, rawlogEntry, GT, cur_obs_timestamp);

				if (expectedPose.x() != 0 || expectedPose.y() != 0 ||
					expectedPose.phi() != 0)
				{  // Averaged error to GT
					double sumW = 0;
					double locErr = 0;
					for (size_t k = 0; k < pdf.size(); k++)
						sumW += exp(pdf.getW(k));
					for (size_t k = 0; k < pdf.size(); k++)
					{
						const auto pk = pdf.getParticlePose(k);
						locErr += mrpt::hypot_fast(
									  expectedPose.x() - pk.x,
									  expectedPose.y() - pk.y) *
							exp(pdf.getW(k)) / sumW;
					}
					convergenceErrors_mtx.lock();
					convergenceErrors.push_back(locErr);
					convergenceErrors_mtx.unlock();

					indivConvergenceErrors.push_back(locErr);
					odoError.push_back(
						expectedPose.distanceTo(odometryEstimation));
				}

				const auto [cov, meanPose] = pdf.getCovarianceAndMean();
				const auto current_pdf_gaussian =
					typename pf2gauss_t<MONTECARLO_TYPE>::type(meanPose, cov);

				// Text output:
				// ----------------------------------------
				if (!SAVE_STATS_ONLY)
				{
					MRPT_LOG_INFO_STREAM(
						"Odometry estimation: " << odometryEstimation);
					MRPT_LOG_INFO_STREAM(
						"PDF estimation: "
						<< pdfEstimation << ", ESS (B.R.)= "
						<< PF_stats.ESS_beforeResample << " tr(cov): "
						<< std::sqrt(current_pdf_gaussian.cov.trace()));

					if (GT.rows() > 0)
						MRPT_LOG_INFO_STREAM("Ground truth: " << expectedPose);
				}

				// Evaluate the "reliability" of the pose estimation
				// (for now, only for 2D laser scans + grid maps)
				double obs_reliability_estim = .0;
				if (DO_RELIABILITY_ESTIMATE)
				{
					// We need: a gridmap & a 2D LIDAR:
					CObservation2DRangeScan::Ptr obs_scan;
					if (observations)
						obs_scan = observations->getObservationByClass<
							CObservation2DRangeScan>(0);  // Get the 0'th
					// scan, if
					// several are
					// present.
					COccupancyGridMap2D::Ptr gridmap =
						metricMap->mapByClass<COccupancyGridMap2D>();
					if (obs_scan && gridmap)  // We have both, go on:
					{
						// Simulate scan + uncertainty:
						COccupancyGridMap2D::TLaserSimulUncertaintyParams
							ssu_params;
						COccupancyGridMap2D::TLaserSimulUncertaintyResult
							ssu_out;
						ssu_params.method = COccupancyGridMap2D::sumUnscented;
						// ssu_params.UT_alpha = 0.99;
						// obs_scan->stdError = 0.07;
						// obs_scan->maxRange = 10.0;

						ssu_params.robotPose =
							CPosePDFGaussian(current_pdf_gaussian);
						ssu_params.aperture = obs_scan->aperture;
						ssu_params.rangeNoiseStd = obs_scan->stdError;
						ssu_params.nRays = obs_scan->getScanSize();
						ssu_params.rightToLeft = obs_scan->rightToLeft;
						ssu_params.sensorPose = obs_scan->sensorPose;
						ssu_params.maxRange = obs_scan->maxRange;

						gridmap->laserScanSimulatorWithUncertainty(
							ssu_params, ssu_out);

						// Evaluate reliability:
						CObservation2DRangeScanWithUncertainty::TEvalParams
							evalParams;
						// evalParams.prob_outliers = 0.40;
						// evalParams.max_prediction_std_dev = 1.0;
						obs_reliability_estim =
							ssu_out.scanWithUncert.evaluateScanLikelihood(
								*obs_scan, evalParams);

						if (DO_SCAN_LIKELIHOOD_DEBUG)
						{
							static mrpt::gui::CDisplayWindowPlots win;

							std::vector<float> ranges_mean, ranges_obs;
							for (size_t i = 0; i < obs_scan->getScanSize(); i++)
							{
								ranges_mean.push_back(
									ssu_out.scanWithUncert.rangeScan
										.getScanRange(i));
								ranges_obs.push_back(obs_scan->getScanRange(i));
							}

							win.plot(ranges_mean, "3k-", "mean");
							win.plot(ranges_obs, "r-", "obs");

							Eigen::VectorXd ci1 =
								ssu_out.scanWithUncert.rangesMean.asEigen() +
								3 *
									ssu_out.scanWithUncert.rangesCovar.asEigen()
										.diagonal()
										.array()
										.sqrt()
										.matrix();
							Eigen::VectorXd ci2 =
								ssu_out.scanWithUncert.rangesMean.asEigen() -
								3 *
									ssu_out.scanWithUncert.rangesCovar.asEigen()
										.diagonal()
										.array()
										.sqrt()
										.matrix();
							win.plot(ci1, "k-", "CI+");
							win.plot(ci2, "k-", "CI-");

							win.setWindowTitle(mrpt::format(
								"obs_reliability_estim: %f",
								obs_reliability_estim));
							win.axis_fit();
						}
					}
					MRPT_LOG_INFO_STREAM(
						"Reliability measure [0-1]: " << obs_reliability_estim);
				}

				if (!SAVE_STATS_ONLY)
				{
					f_cov_est.printf("%e\n", sqrt(cov.det()));
					f_pf_stats.printf(
						"%u %e %e %f %f\n", (unsigned int)pdf.size(),
						PF_stats.ESS_beforeResample,
						PF_stats.weightsVariance_beforeResample,
						obs_reliability_estim,
						sqrt(current_pdf_gaussian.cov.det()));
					f_odo_est.printf(
						"%f %f %f\n", odometryEstimation.x(),
						odometryEstimation.y(), odometryEstimation.phi());
				}

				if ((!SAVE_STATS_ONLY && SCENE3D_FREQ > 0) ||
					SHOW_PROGRESS_3D_REAL_TIME)
				{
					// Generate 3D scene:
					// ------------------------------

					// The Ground Truth (GT):
					if (GT.rows() > 0)
					{
						CRenderizable::Ptr GTpt = scene.getByName("GT");
						if (!GTpt)
						{
							GTpt = std::make_shared<CDisk>();
							GTpt = std::make_shared<CDisk>();
							GTpt->setName("GT");
							GTpt->setColor(0, 0, 0, 0.9);

							mrpt::ptr_cast<CDisk>::from(GTpt)->setDiskRadius(
								0.04f);
							scene.insert(GTpt);
						}

						GTpt->setPose(expectedPose);
					}

					// The particles:
					{
						CRenderizable::Ptr parts = scene.getByName("particles");
						if (parts) scene.removeObject(parts);

						auto p =
							pdf.template getAs3DObject<CSetOfObjects::Ptr>();
						p->setName("particles");
						scene.insert(p);
					}

					// The laser scan:
					{
						CRenderizable::Ptr scanPts = scene.getByName("scan");
						if (!scanPts)
						{
							scanPts = std::make_shared<CPointCloud>();
							scanPts->setName("scan");
							scanPts->setColor(1, 0, 0, 0.9);
							mrpt::ptr_cast<CPointCloud>::from(scanPts)
								->enableColorFromZ(false);
							mrpt::ptr_cast<CPointCloud>::from(scanPts)
								->setPointSize(4);
							scene.insert(scanPts);
						}

						CSimplePointsMap map;

						CPose3D robotPose3D(meanPose);

						map.clear();
						observations->insertObservationsInto(map);

						mrpt::ptr_cast<CPointCloud>::from(scanPts)
							->loadFromPointsMap(&map);
						mrpt::ptr_cast<CPointCloud>::from(scanPts)->setPose(
							robotPose3D);
					}

					// The camera:
					scene.enableFollowCamera(SCENE3D_FOLLOW);

					// Views:
					COpenGLViewport::Ptr view1 = scene.getViewport("main");
					{
						CCamera& cam = view1->getCamera();
						cam.setAzimuthDegrees(-90);
						cam.setElevationDegrees(90);
						cam.setPointingAt(meanPose);
						cam.setZoomDistance(5);
						cam.setOrthogonal();
					}
				}

				if (!SAVE_STATS_ONLY && SCENE3D_FREQ != -1 &&
					((step + 1) % SCENE3D_FREQ) == 0)
				{
					// Save 3D scene:
					CFileGZOutputStream f(format(
						"%s/progress_%05u.3Dscene", sOUT_DIR_3D.c_str(),
						(unsigned)step));
					archiveFrom(f) << scene;

					// Generate text files for matlab:
					// ------------------------------------
					pdf.saveToTextFile(format(
						"%s/particles_%05u.txt", sOUT_DIR_PARTS.c_str(),
						(unsigned)step));
				}

			};	// while rawlogEntries

			indivConvergenceErrors.saveToTextFile(sOUT_DIR + "/GT_error.txt");
			odoError.saveToTextFile(sOUT_DIR + "/ODO_error.txt");
			executionTimes.saveToTextFile(sOUT_DIR + "/exec_times.txt");

			if (win3D && NUM_REPS == 1) mrpt::system::pause();
		};	// for repetitions

		CTicTac tictacGlobal;
		tictacGlobal.Tic();

		const auto max_num_threads = std::thread::hardware_concurrency();
		size_t runs_per_thread = NUM_REPS;
		if (max_num_threads > 1)
			runs_per_thread = static_cast<size_t>(
				std::ceil((NUM_REPS) / static_cast<double>(max_num_threads)));

		MRPT_LOG_INFO_STREAM(
			"Running " << NUM_REPS << " repetitions, on max " << max_num_threads
					   << " parallel threads: " << runs_per_thread
					   << " runs/thread.");

		std::vector<std::thread> running_tasks;
		for (size_t r = 0; r < NUM_REPS; r += runs_per_thread)
		{
			auto runner = [&](size_t i_start, size_t i_end) {
				if (i_end > NUM_REPS) i_end = NUM_REPS;	 // sanity check
				for (size_t i = i_start; i < i_end; i++)
					run_localization_code(i);
			};

			running_tasks.emplace_back(runner, r, r + runs_per_thread);
			std::this_thread::sleep_for(std::chrono::milliseconds(100));
		}

		// Wait for all threads to end:
		for (auto& t : running_tasks)
			if (t.joinable()) t.join();

		double repetitionTime = tictacGlobal.Tac();

		// Avr. error:
		double covergenceErrorMean = 0, convergenceErrorsMin = 0,
			   convergenceErrorsMax = 0;
		if (!convergenceErrors.empty())
			math::confidenceIntervals(
				convergenceErrors, covergenceErrorMean, convergenceErrorsMin,
				convergenceErrorsMax, STATS_CONF_INTERVAL);

		// Save overall results:
		{
			CFileOutputStream f(
				format("%s_SUMMARY.txt", OUT_DIR_PREFIX.c_str()),
				OpenMode::APPEND);

			f.printf(
				"%% Ratio_covergence_success  #particles  "
				"average_time_per_execution  convergence_mean_error "
				"convergence_error_conf_int_inf "
				"convergence_error_conf_int_sup "
				"\n");
			if (!nConvergenceTests) nConvergenceTests = 1;
			f.printf(
				"%f %u %f %f %f %f\n",
				((double)nConvergenceOK) / nConvergenceTests, PARTICLE_COUNT,
				repetitionTime / NUM_REPS, covergenceErrorMean,
				convergenceErrorsMin, convergenceErrorsMax);
		}

		MRPT_LOG_INFO_FMT("Total execution time: %.06f sec", repetitionTime);

	}  // end of loop for different # of particles
}

void MonteCarloLocalization_Base::getGroundTruth(
	mrpt::poses::CPose2D& expectedPose, size_t rawlogEntry,
	const mrpt::math::CMatrixDouble& GT, const Clock::time_point& cur_time)
{
	// Either:
	// - time x y phi
	// or
	// - time x y z yaw pitch roll

	if (GT.cols() == 4 || GT.cols() == 7)
	{
		bool GT_index_is_time;

		// First column can be: timestamps, or rawlogentries:
		//  Auto-figure it out:
		if (GT.rows() > 2)
		{
			GT_index_is_time =
				floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
		}
		else
		{
			GT_index_is_time = false;
		}

		if (GT_index_is_time)
		{
			// Look for the timestamp:
			using namespace std::chrono_literals;

			bool interp_ok = false;
			GT_path.interpolate(cur_time, expectedPose, interp_ok);
			if (!interp_ok)
			{
				/*
				cerr << format(
					"GT time not found: %f\n",
					mrpt::system::timestampTotime_t(cur_time));
					*/
			}
		}
		else
		{
			// Look for the rawlogEntry:
			size_t k, N = GT.rows();
			for (k = 0; k < N; k++)
			{
				if (GT(k, 0) == rawlogEntry) break;
			}

			if (k < N)
			{
				expectedPose.x(GT(k, 1));
				expectedPose.y(GT(k, 2));
				expectedPose.phi(GT(k, 3));
			}
		}
	}
	else if (GT.cols() == 3)
	{
		if ((int)rawlogEntry < GT.rows())
		{
			expectedPose.x(GT(rawlogEntry, 0));
			expectedPose.y(GT(rawlogEntry, 1));
			expectedPose.phi(GT(rawlogEntry, 2));
		}
	}
	else if (GT.cols() > 0)
		THROW_EXCEPTION("Unexpected number of columns in ground truth file");
}

void MonteCarloLocalization_Base::prepareGT(const mrpt::math::CMatrixDouble& GT)
{
	// Either:
	// - time x y phi
	// or
	// - time x y z yaw pitch roll
	if (GT.cols() == 4 || GT.cols() == 7)
	{
		const bool GT_is_3D = (GT.cols() == 7);
		bool GT_index_is_time = false;

		// First column can be: timestamps, or rawlogentries:
		//  Auto-figure it out:
		if (GT.rows() > 2)
		{
			GT_index_is_time =
				floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
		}

		if (GT_index_is_time)
		{
			// Look for the timestamp:
			using namespace std::chrono_literals;
			GT_path.setMaxTimeInterpolation(200ms);

			for (int i = 0; i < GT.rows(); i++)
			{
				GT_path.insert(
					mrpt::Clock::fromDouble(GT(i, 0)),
					TPose2D(GT(i, 1), GT(i, 2), GT(i, GT_is_3D ? 4 : 3)));
			}
		}
	}
}

void MonteCarloLocalization_Base::run()
{
	MRPT_START

	// Detect 2D vs 3D particle filter?
	const bool is_3D = params.read_bool(sect, "use_3D_poses", false);

	if (is_3D)
	{
		MRPT_LOG_INFO("Running for: CMonteCarloLocalization3D");
		do_pf_localization<CMonteCarloLocalization3D>();
	}
	else
	{
		MRPT_LOG_INFO("Running for: CMonteCarloLocalization2D");
		do_pf_localization<CMonteCarloLocalization2D>();
	}

	MRPT_END
}

// ---------------------------------------
//   MonteCarloLocalization_Rawlog
// ---------------------------------------
MonteCarloLocalization_Rawlog::MonteCarloLocalization_Rawlog()
{
	setLoggerName("MonteCarloLocalization_Rawlog");
}

void MonteCarloLocalization_Rawlog::impl_initialize(int argc, const char** argv)
{
	MRPT_START
	// Rawlog file: from args. line or from config file:
	if (argc == 3) m_rawlogFileName = std::string(argv[2]);
	else
		m_rawlogFileName = params.read_string(
			sect, "rawlog_file", std::string("log.rawlog"), true);

	m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0);

	ASSERT_FILE_EXISTS_(m_rawlogFileName);

	MRPT_END
}

max_stars_repo_name: wstnturner/mrpt

max_issues_repo_name: wstnturner/mrpt

max_forks_repo_name: wstnturner/mrpt


text:
//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "PacketVersionResolver.hpp"

#include <boost/core/ignore_unused.hpp>

namespace armnn
{

namespace profiling
{

bool PacketKey::operator<(const PacketKey& rhs) const
{
    bool result = true;
    if (m_FamilyId == rhs.m_FamilyId)
    {
            result = m_PacketId < rhs.m_PacketId;
    }
    else if (m_FamilyId > rhs.m_FamilyId)
    {
        result = false;
    }
    return result;
}

bool PacketKey::operator>(const PacketKey& rhs) const
{
    return rhs < *this;
}

bool PacketKey::operator<=(const PacketKey& rhs) const
{
    return !(*this > rhs);
}

bool PacketKey::operator>=(const PacketKey& rhs) const
{
    return !(*this < rhs);
}

bool PacketKey::operator==(const PacketKey& rhs) const
{
    return m_FamilyId == rhs.m_FamilyId && m_PacketId == rhs.m_PacketId;
}

bool PacketKey::operator!=(const PacketKey& rhs) const
{
    return !(*this == rhs);
}

Version PacketVersionResolver::ResolvePacketVersion(uint32_t familyId, uint32_t packetId) const
{
    boost::ignore_unused(familyId, packetId);
    // NOTE: For now every packet specification is at version 1.0.0
    return Version(1, 0, 0);
}

} // namespace profiling

} // namespace armnn

max_stars_repo_name: korabelnikov/armnn

max_issues_repo_name: korabelnikov/armnn

max_forks_repo_name: korabelnikov/armnn


text:
//
// local-stream.cpp
//
//  Created by Peter Gusev on 18 April 2016.
//  Copyright 2013-2016 Regents of the University of California
//

#include <boost/asio.hpp>

#include "local-stream.hpp"
#include "audio-stream-impl.hpp"
#include "video-stream-impl.hpp"
#include "audio-capturer.hpp"

using namespace ndnrtc;
using namespace std;
using namespace ndn;

//******************************************************************************
std::vector<std::pair<std::string, std::string>>
LocalAudioStream::getRecordingDevices()
{
	return AudioCapturer::getRecordingDevices();
}

std::vector<std::pair<std::string, std::string>>
LocalAudioStream::getPlayoutDevices()
{
	return AudioCapturer::getPlayoutDevices();
}

//******************************************************************************
LocalAudioStream::LocalAudioStream(const std::string& basePrefix,
			const MediaStreamSettings& settings):
pimpl_(std::make_shared<AudioStreamImpl>(basePrefix, settings))
{
	pimpl_->setupInvocation(MediaStreamBase::MetaCheckIntervalMs,
		std::bind(&MediaStreamBase::periodicInvocation, pimpl_));
	pimpl_->publishMeta();
}

LocalAudioStream::~LocalAudioStream()
{
	pimpl_->cancelInvocation();
	pimpl_->stop();
}

void LocalAudioStream::start()
{
	pimpl_->start();
}

void LocalAudioStream::stop()
{
	pimpl_->stop();
}

void
LocalAudioStream::addThread(const AudioThreadParams params)
{
	pimpl_->addThread(&params);
}

void LocalAudioStream::removeThread(const string& threadName)
{
	pimpl_->removeThread(threadName);
}

bool LocalAudioStream::isRunning() const
{
	return pimpl_->isRunning();
}

string
LocalAudioStream::getPrefix() const
{
	return pimpl_->getPrefix();
}

std::string
LocalAudioStream::getBasePrefix() const
{
	return pimpl_->getBasePrefix();
}

std::string
LocalAudioStream::getStreamName() const
{
	return pimpl_->getStreamName();
}

vector<string>
LocalAudioStream::getThreads() const
{
	return pimpl_->getThreads();
}

void
LocalAudioStream::setLogger(std::shared_ptr<ndnlog::new_api::Logger> logger)
{
	pimpl_->setLogger(logger);
}

std::shared_ptr<ndnlog::new_api::Logger>
LocalAudioStream::getLogger() const
{
    return pimpl_->getLogger();
}

statistics::StatisticsStorage
LocalAudioStream::getStatistics() const
{
	return pimpl_->getStatistics();
}

std::shared_ptr<StorageEngine>
LocalAudioStream::getStorage() const
{
    return pimpl_->getStorage();
}

//******************************************************************************
LocalVideoStream::LocalVideoStream(const std::string& streamPrefix,
	const MediaStreamSettings& settings, bool useFec):
pimpl_(std::make_shared<VideoStreamImpl>(streamPrefix, settings, useFec))
{
	pimpl_->setupInvocation(MediaStreamBase::MetaCheckIntervalMs,
		std::bind(&MediaStreamBase::periodicInvocation, pimpl_));
	pimpl_->publishMeta();
}

LocalVideoStream::~LocalVideoStream()
{
}

void
LocalVideoStream::addThread(const VideoThreadParams params)
{
	pimpl_->addThread(&params);
}

void LocalVideoStream::removeThread(const string& threadName)
{
	pimpl_->removeThread(threadName);
}

int LocalVideoStream::incomingArgbFrame(const unsigned int width,
	const unsigned int height,
	unsigned char* argbFrameData,
	unsigned int frameSize)
{
	return pimpl_->incomingFrame(ArgbRawFrameWrapper({width, height, argbFrameData, frameSize}));
}

int LocalVideoStream::incomingI420Frame(const unsigned int width,
	const unsigned int height,
	const unsigned int strideY,
	const unsigned int strideU,
	const unsigned int strideV,
	const unsigned char* yBuffer,
	const unsigned char* uBuffer,
	const unsigned char* vBuffer)
{
	return pimpl_->incomingFrame(I420RawFrameWrapper({width, height, strideY, strideU,
		strideV, yBuffer, uBuffer, vBuffer}));
}

int LocalVideoStream::incomingNV21Frame(const unsigned int width,
			const unsigned int height,
			const unsigned int strideY,
			const unsigned int strideUV,
			const unsigned char* yBuffer,
			const unsigned char* uvBuffer)
{
	return pimpl_->incomingFrame(YUV_NV21FrameWrapper({width, height, strideY,
		strideUV, yBuffer, uvBuffer}));
}

const std::map<std::string, FrameInfo>&
LocalVideoStream::getLastPublishedInfo() const
{
    return pimpl_->getLastPublished();
}

string
LocalVideoStream::getPrefix() const
{
	return pimpl_->getPrefix();
}

std::string
LocalVideoStream::getBasePrefix() const
{
	return pimpl_->getBasePrefix();
}

std::string
LocalVideoStream::getStreamName() const
{
	return pimpl_->getStreamName();
}

vector<string>
LocalVideoStream::getThreads() const
{
	return pimpl_->getThreads();
}

void
LocalVideoStream::setLogger(std::shared_ptr<ndnlog::new_api::Logger> logger)
{
	pimpl_->setLogger(logger);
}

std::shared_ptr<ndnlog::new_api::Logger>
LocalVideoStream::getLogger() const
{
    return pimpl_->getLogger();
}

statistics::StatisticsStorage
LocalVideoStream::getStatistics() const
{
	return pimpl_->getStatistics();
}

std::shared_ptr<StorageEngine>
LocalVideoStream::getStorage() const
{
    return pimpl_->getStorage();
}

max_stars_repo_name: peurpdapeurp/ndnrtc

max_issues_repo_name: peurpdapeurp/ndnrtc

max_forks_repo_name: peurpdapeurp/ndnrtc


text:
//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk_voronoi_simplex_solver_policy.h>
#include <OpenTissue/collision/gjk/gjk_compute_closest_points.h>
#include <OpenTissue/collision/gjk/gjk_support_functors.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk_compute_closest_points);

BOOST_AUTO_TEST_CASE(case_by_case_testing)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;

  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;
  typedef math_types::coordsys_type                        transformation_type;
  typedef math_types::value_traits                         value_traits;

  OpenTissue::gjk::VoronoiSimplexSolverPolicy const simplex_solver_policy = OpenTissue::gjk::VoronoiSimplexSolverPolicy();

  OpenTissue::gjk::Sphere<math_types> const supportA;
  OpenTissue::gjk::Sphere<math_types> const supportB;

  size_t    const max_iterations       = 100u;
  real_type const absolute_tolerance   = boost::numeric_cast<real_type>(10e-6);
  real_type const relative_tolerance   = boost::numeric_cast<real_type>(10e-6);
  real_type const stagnation_tolerance = boost::numeric_cast<real_type>(10e-15);

  // Two unit-spheres placed ontop of each other
  {
    transformation_type transformA;
    transformation_type transformB;

    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    transformA.T().clear();
    transformA.Q().identity();
    transformB.T().clear();
    transformB.Q().identity();

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    std::cout << "\tstatus = " << OpenTissue::gjk::get_status_message(status) << std::endl;
    std::cout << "\tdistance = " << distance << std::endl;
    std::cout << "\titerations = " << iterations << std::endl;
    std::cout << "\ttrue distance = " << true_distance << std::endl;

  }
  // Two unit-spheres overlapping but both placed on the x-axis
  {
    transformation_type transformA;
    transformation_type transformB;

    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    transformA.T().clear();
    transformA.Q().identity();
    transformA.T()(0) = -1.5;
    transformB.T().clear();
    transformB.Q().identity();

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    std::cout << "\tstatus = " << OpenTissue::gjk::get_status_message(status) << std::endl;
    std::cout << "\tdistance = " << distance << std::endl;
    std::cout << "\titerations = " << iterations << std::endl;
    std::cout << "\ttrue distance = " << true_distance << std::endl;
  }
  // Two unit-spheres exactly touching in one point (= one intersection point) and but both placed on the x-axis
  {
    transformation_type transformA;
    transformation_type transformB;

    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    transformA.T().clear();
    transformA.Q().identity();
    transformA.T()(0) = -2.0;
    transformB.T().clear();
    transformB.Q().identity();

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    std::cout << "\tstatus = " << OpenTissue::gjk::get_status_message(status) << std::endl;
    std::cout << "\tdistance = " << distance << std::endl;
    std::cout << "\titerations = " << iterations << std::endl;
    std::cout << "\ttrue distance = " << true_distance << std::endl;

  }
  // Two unit-spheres non-overlapping but both placed on the x-axis
  {
    transformation_type transformA;
    transformation_type transformB;

    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    transformA.T().clear();
    transformA.Q().identity();
    transformA.T()(0) = -2.5;
    transformB.T().clear();
    transformB.Q().identity();

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    std::cout << "\tstatus = " << OpenTissue::gjk::get_status_message(status) << std::endl;
    std::cout << "\tdistance = " << distance << std::endl;
    std::cout << "\titerations = " << iterations << std::endl;
    std::cout << "\ttrue distance = " << true_distance << std::endl;
  }

}

BOOST_AUTO_TEST_CASE(random_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;

  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;
  typedef math_types::coordsys_type                        transformation_type;
  typedef math_types::value_traits                         value_traits;

  typedef OpenTissue::gjk::Simplex<vector3_type>           simplex_type;

  OpenTissue::gjk::VoronoiSimplexSolverPolicy const simplex_solver_policy = OpenTissue::gjk::VoronoiSimplexSolverPolicy();

  OpenTissue::gjk::Sphere<math_types> const supportA;
  OpenTissue::gjk::Sphere<math_types> const supportB;

  size_t    const max_iterations       = 100u;
  real_type const absolute_tolerance   = boost::numeric_cast<real_type>(10e-6);
  real_type const relative_tolerance   = boost::numeric_cast<real_type>(10e-10);
  real_type const stagnation_tolerance = boost::numeric_cast<real_type>(0.0);

  for(size_t i=0;i<100u;++i)
  {
    transformation_type transformA;
    transformation_type transformB;

    quaternion_type Q;
    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    OpenTissue::math::random( transformA.T(), -2.0, 2.0 );
    OpenTissue::math::random( transformB.T(), -2.0, 2.0 );
    Q.random();
    transformA.Q() = OpenTissue::math::unit ( Q );
    Q.random();
    transformB.Q() = OpenTissue::math::unit ( Q );

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    real_type true_distance = length( transformA.T() - transformB.T() ) - value_traits::two();

    if(  true_distance > absolute_tolerance )
    {
      BOOST_CHECK_CLOSE( true_distance, distance, 0.05 );

      BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );            // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                    // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                       // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );   // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );           // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );          // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );      // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                    // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE ); // Would indicate convergence to positive distance
    }
    else
    {
      BOOST_CHECK( 0.0 <= distance );
      BOOST_CHECK( distance < absolute_tolerance );

      BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                     // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE );  // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );           // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );       // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                      // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );  // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );          // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );         // Indicates penetration
      //BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                 // Indicates penetration
    }

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , status
      );

    if(  true_distance > absolute_tolerance )
    {
      BOOST_CHECK_CLOSE( true_distance, distance, 0.05 );

      BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );            // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                    // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                       // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );   // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );           // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );          // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );      // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                    // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE ); // Would indicate convergence to positive distance
    }
    else
    {
      BOOST_CHECK( 0.0 <= distance );
      BOOST_CHECK( distance < absolute_tolerance );

      BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                     // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE );  // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );           // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );       // Can only occur in case of positive distance
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                      // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );  // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );          // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );         // Indicates penetration
      //BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                 // Indicates penetration
    }
  }
}

BOOST_AUTO_TEST_CASE(cylinder_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;

  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;
  typedef math_types::coordsys_type                        transformation_type;
  typedef math_types::value_traits                         value_traits;

  typedef OpenTissue::gjk::Simplex<vector3_type>           simplex_type;

  OpenTissue::gjk::VoronoiSimplexSolverPolicy const simplex_solver_policy = OpenTissue::gjk::VoronoiSimplexSolverPolicy();

  // We let the cylinder move around the sphere in a fixed distance. Thus
  // we always know the closest point by construction.
  OpenTissue::gjk::Sphere<math_types> const supportA;
  OpenTissue::gjk::Cylinder<math_types> const supportB;

  size_t    const max_iterations       = 1000u;
  real_type const absolute_tolerance   = boost::numeric_cast<real_type>(0.0);
  real_type const relative_tolerance   = boost::numeric_cast<real_type>(10e-10); // Don't be to over agressive!
  real_type const stagnation_tolerance = boost::numeric_cast<real_type>(0.0);

  real_type rad_x = value_traits::zero();
  real_type rad_z = value_traits::zero();

  for(size_t i=0;i<100u;++i)
  {
    transformation_type transformA;
    transformation_type transformB;

    quaternion_type Qx,Qz;
    vector3_type a;
    vector3_type b;
    size_t iterations     = 0u;
    size_t status         = 0u;
    real_type distance    = value_traits::infinity();

    // Place sphere at origin
    transformA.T().clear();
    transformA.Q().identity();

    // Place cylinder a fixed distance away from sphere
    transformB.T().clear();
    transformB.Q().identity();

    // Minimum distance between sphere and cylinder is fixed by design

    // The unrotated cloest points
    vector3_type tst_a = vector3_type(1.0, 0.0, 0.0);
    vector3_type tst_b = vector3_type(1.1, 0.0, 0.0);

    // Apply some rotation to the cylinder, which does not alter the closest points
    Qx.Rx( rad_x );
    Qz.Rz( rad_z );
    rad_x += value_traits::pi()/100.0;
    rad_z += value_traits::pi()/100.0;

    transformB.Q() = prod( Qz, prod( Qx , transformB.Q() ));
    transformB.T() = Qz.rotate( vector3_type( 2.1, 0.0, 0.0 ) );

    // Update closest points with rotation
    tst_a = Qz.rotate(tst_a);
    tst_b = Qz.rotate(tst_b);

    OpenTissue::gjk::compute_closest_points(
      transformA
      , supportA
      , transformB
      , supportB
      , a
      , b
      , distance
      , iterations
      , status
      , absolute_tolerance
      , relative_tolerance
      , stagnation_tolerance
      , max_iterations
      , simplex_solver_policy
      );

    // Test if closest distance was correct
    BOOST_CHECK_CLOSE( 0.1, distance, 0.01 );

    // Test if status code was as expected
      BOOST_CHECK( status != OpenTissue::gjk::ABSOLUTE_CONVERGENCE );            // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::INTERSECTION );                    // Would indicate penetration
      BOOST_CHECK( status != OpenTissue::gjk::ITERATING );                       // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::EXCEEDED_MAX_ITERATIONS_LIMIT );   // Would indicate internal error in GJK
      BOOST_CHECK( status != OpenTissue::gjk::NON_DESCEND_DIRECTION );           // Would indicate internal error in GJK
      //BOOST_CHECK( status != OpenTissue::gjk::RELATIVE_CONVERGENCE );          // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::SIMPLEX_EXPANSION_FAILED );      // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::STAGNATION );                    // Would indicate convergence to positive distance
      //BOOST_CHECK( status != OpenTissue::gjk::LOWER_ERROR_BOUND_CONVERGENCE ); // Would indicate convergence to positive distance

    // Test if closest points make sense
    BOOST_CHECK( fabs(a(0) - tst_a(0)) < 10e-6 );
    BOOST_CHECK( fabs(a(1) - tst_a(1)) < 10e-6 );
    BOOST_CHECK( fabs(a(2) - tst_a(2)) < 10e-6 );

    BOOST_CHECK( fabs(b(0) - tst_b(0)) < 10e-6 );
    BOOST_CHECK( fabs(b(1) - tst_b(1)) < 10e-6 );
    BOOST_CHECK( fabs(b(2) - tst_b(2)) < 10e-6 );
  }

}

BOOST_AUTO_TEST_SUITE_END();

max_stars_repo_name: ricortiz/OpenTissue

max_issues_repo_name: ricortiz/OpenTissue

max_forks_repo_name: ricortiz/OpenTissue