<commit_before>#include "FltFile.h"
#include "Registry.h"
#include "Record.h"
#include "RecordVisitor.h"
#include "ExternalRecord.h"
#include "flt2osg.h"             // ConvertFromFLT
#include "Input.h"

#include <osg/Node>
#include <osg/NodeVisitor>
#include <osg/Notify>

#include <osgDB/FileUtils>
#include <osgDB/FileNameUtils>

#include <osgSim/GeographicLocation>

#include <string>

using namespace flt;

FltFile::FltFile(
    ColorPool* pColorPool,
    TexturePool* pTexturePool,
    MaterialPool* pMaterialPool,
    LtPtAppearancePool* pLtPtAppearancePool,
    LtPtAnimationPool* pLtPtAnimationPool,
    osgDB::ReaderWriter::Options* options)
{
    _useTextureAlphaForTransparancyBinning = true;
    _doUnitsConversion = true;
    _desiredUnits = ConvertToMeters;

    if (pColorPool)
    {
        // use external color palette, ignore internal
        _useInternalColorPalette = false;
        setColorPool( pColorPool );
    }
    else
    {
        // use internal color palette
        _useInternalColorPalette = true;
        setColorPool( new ColorPool );
    }

    if (pTexturePool)
    {
        // use external texture palette, ignore internal
        _useInternalTexturePalette = false;
        setTexturePool( pTexturePool );
    }
    else
    {
        // use internal texture palette
        _useInternalTexturePalette = true;
        setTexturePool( new TexturePool );
    }

    if (pMaterialPool)
    {
        // use external material palette, ignore internal
        _useInternalMaterialPalette = false;
        setMaterialPool( pMaterialPool );
    }
    else
    {
        // use internal material palette
        _useInternalMaterialPalette = true;
        setMaterialPool( new MaterialPool );
    }

    if (pLtPtAppearancePool && pLtPtAnimationPool) // Can only be non-NULL if parent is 15.8.
    {
        // use external light point appearance and animation palettes, ignore internal
        _useInternalLtPtPalettes = false;
        setLtPtAppearancePool( pLtPtAppearancePool );
        setLtPtAnimationPool( pLtPtAnimationPool );
    }
    else
    {
        // If they aren't both set, then they must both be NULL.
        assert( (pLtPtAppearancePool==NULL) && (pLtPtAppearancePool==NULL) );
        // use internal light point palettes
        _useInternalLtPtPalettes = true;
        setLtPtAppearancePool( new LtPtAppearancePool );
        setLtPtAnimationPool( new LtPtAnimationPool );
    }

    // no support for external light palettes
    setLightPool( new LightPool );

    // instances are always internally defined
    setInstancePool( new InstancePool );

    _options = options;
}

osg::Object* FltFile::readObject(const std::string& fileName)
{
    return readNode(fileName);
}

osg::Node* FltFile::readNode(const std::string& fileName)
{
    _directory = osgDB::getFilePath(fileName);

    if (readModel(fileName))
    {
        // Convert record tree to osg scene graph
        osg::Node* model = convert();

        if (model)
        {
            // Store model origin in returned Node userData.
            osg::ref_ptr<osgSim::GeographicLocation> loc = new osgSim::GeographicLocation;
            double lat, lon;
            getOrigin( lat, lon );
            loc->set( lat, lon );
            model->setUserData( loc.get() );

            osg::notify(osg::INFO)<<"FltFile::readNode("<<fileName<<") lat="<<lat<<" lon="<<lon<<std::endl;

            return model;
        }
    }

    return NULL;
}

osg::Group* FltFile::convert()
{
    ConvertFromFLT visit;
    visit.setUseTextureAlphaForTransparancyBinning(getUseTextureAlphaForTransparancyBinning());
    visit.setDoUnitsConversion(getDoUnitsConversion());
    return visit.convert(getHeaderRecord());
}

// Read flight model (include externals)
bool FltFile::readModel(const std::string& fileName)
{
    if (readFile(fileName))
    {
        readExternals();
        return getHeaderRecord() ? true : false;
    }
    return false;
}

bool FltFile::readFile(const std::string& fileName)
{

    // havn't found file, look in OSGFILEPATH
    std::string foundFileName = osgDB::findDataFile(fileName, _options.get());
    if (foundFileName.empty()) return false;

    FileInput fin;
    if (!fin.open(foundFileName)) return false;

    Record* pRec = fin.readCreateRecord(this);
    if (pRec == NULL)
    {
        osg::notify(osg::WARN) << "File not found " << fileName << std::endl;
        return false;
    }

    _headerRecord = (HeaderRecord*)pRec;
    if (pRec->isPrimaryNode())   // Header
        pRec->readLocalData(fin);// Read rest of file

    fin.close();

    return true;
}

#define REGISTER_FLT 1

// This class was originally scoped within FltFile::readExternals() function.
// Irix 7.3 compilers hork on this.

    class ReadExternal : public RecordVisitor
    {
        public:
            ReadExternal(FltFile* fltFile)
            {
                _pFltFile = fltFile;
                setTraverseMode(RecordVisitor::TRAVERSE_ALL_CHILDREN);
            }

            virtual void apply(ExternalRecord& rec)
            {
                SExternalReference* pSExternal = (SExternalReference*)rec.getData();

                if (pSExternal)
                {
                    FltFile*        pExternalFltFile = NULL;
                    ColorPool*      pColorPool    = NULL;
                    TexturePool*    pTexturePool  = NULL;
                    MaterialPool*   pMaterialPool = NULL;
                    LtPtAppearancePool* pLtPtAppearancePool = NULL;
                    LtPtAnimationPool* pLtPtAnimationPool = NULL;
                    std::string filename( rec.getFilename() );

                    osg::notify(osg::INFO) << "External=" << filename << std::endl;

                    if (rec.getFlightVersion() > 13)
                    {
                        if (!(pSExternal->dwFlags & ExternalRecord::COLOR_PALETTE_OVERRIDE))
                            pColorPool = _pFltFile->getColorPool();

                        if (!(pSExternal->dwFlags & ExternalRecord::TEXTURE_PALETTE_OVERRIDE))
                            pTexturePool = _pFltFile->getTexturePool();

                        if (!(pSExternal->dwFlags & ExternalRecord::MATERIAL_PALETTE_OVERRIDE))
                            pMaterialPool = _pFltFile->getMaterialPool();

                        if (rec.getFlightVersion() >= 1580)
                        {
                            if (!(pSExternal->dwFlags & ExternalRecord::LIGHT_POINT_PALETTE_OVERRIDE))
                            {
                                pLtPtAppearancePool = _pFltFile->getLtPtAppearancePool();
                                pLtPtAnimationPool = _pFltFile->getLtPtAnimationPool();
                            }
                        }
                    }

                #if REGISTER_FLT
                    bool registerFLT = true;
                #else
                    bool registerFLT = false;
                #endif

                    pExternalFltFile = registerFLT ? Registry::instance()->getFltFile(filename) : NULL;
                    if (pExternalFltFile == NULL)
                    {
                        osg::ref_ptr<osgDB::ReaderWriter::Options> options =
                            _pFltFile->getOptions() ? _pFltFile->getOptions() :
                                                      new osgDB::ReaderWriter::Options;

                        //Path for Nested external references
                        osgDB::FilePathList& fpl = options->getDatabasePathList();
                        const std::string& filePath = osgDB::getFilePath(filename);
                        std::string pushAndPopPath;
                        //If absolute path
                        if( (filePath.length()>0 && filePath.find_first_of("/\\")==0) ||
                            (filePath.length()>2 && filePath.substr(1,1)==":" && filePath.find_first_of("/\\")==2) )
                        {
                            pushAndPopPath = filePath;
                        }
                        else
                        {
                            pushAndPopPath = (fpl.empty() | fpl.back().empty() ? "." : fpl.back()) + "/" + filePath;
                        }

                        char optionsString[256];
                        sprintf(optionsString,"FLT_VER %d",rec.getFlightVersion());
                        options->setOptionString(optionsString);

                        //osg::notify(osg::NOTICE)<<"Create local path"<<pushAndPopPath<<std::endl;

                        fpl.push_back(pushAndPopPath);

                        pExternalFltFile = new FltFile( pColorPool, pTexturePool, pMaterialPool,
                            pLtPtAppearancePool, pLtPtAnimationPool, options.get() );

                        if (registerFLT)
                        {
                            Registry::instance()->addFltFile(filename, pExternalFltFile);
                        }

                        pExternalFltFile->readModel(filename);
                    }

                    rec.setExternal(pExternalFltFile);
                }
            }

        public:

            FltFile* _pFltFile;
    };

void FltFile::readExternals()
{
    ReadExternal visitor(this);
    _headerRecord->accept(visitor);
}

int FltFile::getFlightVersion() const
{
    if (_headerRecord.get())
    {
        SHeader* pSHeader = (SHeader*)_headerRecord.get()->getData();
        if (pSHeader)
            return pSHeader->diFormatRevLev;
    }
    return 0;
}

void FltFile::getOrigin( double& latitude, double& longitude ) const
{
    if (_headerRecord.get())
    {
        SHeader* pSHeader = (SHeader*)_headerRecord.get()->getData();
        if (pSHeader)
        {
            latitude = pSHeader->Origin.x();
            longitude = pSHeader->Origin.y();
        }
    }
}

std::string FltFile::getDesiredUnitsString() const
{
    switch (_desiredUnits)
    {
    case ConvertToMeters:
        return "ConvertToMeters";
        break;
    case ConvertToKilometers:
        return "ConvertToKilometers";
        break;
    case ConvertToFeet:
        return "ConvertToFeet";
        break;
    case ConvertToInches:
        return "ConvertToInches";
        break;
    case ConvertToNauticalMiles:
        return "ConvertToNauticalMiles";
        break;
    default:
        return "Invalid";
        break;
    }
}

<commit_msg>From Alberto Farre, "Missed a FilepathList pop_back line from my last submission.<commit_after>#include "FltFile.h"
#include "Registry.h"
#include "Record.h"
#include "RecordVisitor.h"
#include "ExternalRecord.h"
#include "flt2osg.h"             // ConvertFromFLT
#include "Input.h"

#include <osg/Node>
#include <osg/NodeVisitor>
#include <osg/Notify>

#include <osgDB/FileUtils>
#include <osgDB/FileNameUtils>

#include <osgSim/GeographicLocation>

#include <string>

using namespace flt;

FltFile::FltFile(
    ColorPool* pColorPool,
    TexturePool* pTexturePool,
    MaterialPool* pMaterialPool,
    LtPtAppearancePool* pLtPtAppearancePool,
    LtPtAnimationPool* pLtPtAnimationPool,
    osgDB::ReaderWriter::Options* options)
{
    _useTextureAlphaForTransparancyBinning = true;
    _doUnitsConversion = true;
    _desiredUnits = ConvertToMeters;

    if (pColorPool)
    {
        // use external color palette, ignore internal
        _useInternalColorPalette = false;
        setColorPool( pColorPool );
    }
    else
    {
        // use internal color palette
        _useInternalColorPalette = true;
        setColorPool( new ColorPool );
    }

    if (pTexturePool)
    {
        // use external texture palette, ignore internal
        _useInternalTexturePalette = false;
        setTexturePool( pTexturePool );
    }
    else
    {
        // use internal texture palette
        _useInternalTexturePalette = true;
        setTexturePool( new TexturePool );
    }

    if (pMaterialPool)
    {
        // use external material palette, ignore internal
        _useInternalMaterialPalette = false;
        setMaterialPool( pMaterialPool );
    }
    else
    {
        // use internal material palette
        _useInternalMaterialPalette = true;
        setMaterialPool( new MaterialPool );
    }

    if (pLtPtAppearancePool && pLtPtAnimationPool) // Can only be non-NULL if parent is 15.8.
    {
        // use external light point appearance and animation palettes, ignore internal
        _useInternalLtPtPalettes = false;
        setLtPtAppearancePool( pLtPtAppearancePool );
        setLtPtAnimationPool( pLtPtAnimationPool );
    }
    else
    {
        // If they aren't both set, then they must both be NULL.
        assert( (pLtPtAppearancePool==NULL) && (pLtPtAppearancePool==NULL) );
        // use internal light point palettes
        _useInternalLtPtPalettes = true;
        setLtPtAppearancePool( new LtPtAppearancePool );
        setLtPtAnimationPool( new LtPtAnimationPool );
    }

    // no support for external light palettes
    setLightPool( new LightPool );

    // instances are always internally defined
    setInstancePool( new InstancePool );

    _options = options;
}

osg::Object* FltFile::readObject(const std::string& fileName)
{
    return readNode(fileName);
}

osg::Node* FltFile::readNode(const std::string& fileName)
{
    _directory = osgDB::getFilePath(fileName);

    if (readModel(fileName))
    {
        // Convert record tree to osg scene graph
        osg::Node* model = convert();

        if (model)
        {
            // Store model origin in returned Node userData.
            osg::ref_ptr<osgSim::GeographicLocation> loc = new osgSim::GeographicLocation;
            double lat, lon;
            getOrigin( lat, lon );
            loc->set( lat, lon );
            model->setUserData( loc.get() );

            osg::notify(osg::INFO)<<"FltFile::readNode("<<fileName<<") lat="<<lat<<" lon="<<lon<<std::endl;

            return model;
        }
    }

    return NULL;
}

osg::Group* FltFile::convert()
{
    ConvertFromFLT visit;
    visit.setUseTextureAlphaForTransparancyBinning(getUseTextureAlphaForTransparancyBinning());
    visit.setDoUnitsConversion(getDoUnitsConversion());
    return visit.convert(getHeaderRecord());
}

// Read flight model (include externals)
bool FltFile::readModel(const std::string& fileName)
{
    if (readFile(fileName))
    {
        readExternals();
        return getHeaderRecord() ? true : false;
    }
    return false;
}

bool FltFile::readFile(const std::string& fileName)
{

    // havn't found file, look in OSGFILEPATH
    std::string foundFileName = osgDB::findDataFile(fileName, _options.get());
    if (foundFileName.empty()) return false;

    FileInput fin;
    if (!fin.open(foundFileName)) return false;

    Record* pRec = fin.readCreateRecord(this);
    if (pRec == NULL)
    {
        osg::notify(osg::WARN) << "File not found " << fileName << std::endl;
        return false;
    }

    _headerRecord = (HeaderRecord*)pRec;
    if (pRec->isPrimaryNode())   // Header
        pRec->readLocalData(fin);// Read rest of file

    fin.close();

    return true;
}

#define REGISTER_FLT 1

// This class was originally scoped within FltFile::readExternals() function.
// Irix 7.3 compilers hork on this.

    class ReadExternal : public RecordVisitor
    {
        public:
            ReadExternal(FltFile* fltFile)
            {
                _pFltFile = fltFile;
                setTraverseMode(RecordVisitor::TRAVERSE_ALL_CHILDREN);
            }

            virtual void apply(ExternalRecord& rec)
            {
                SExternalReference* pSExternal = (SExternalReference*)rec.getData();

                if (pSExternal)
                {
                    FltFile*        pExternalFltFile = NULL;
                    ColorPool*      pColorPool    = NULL;
                    TexturePool*    pTexturePool  = NULL;
                    MaterialPool*   pMaterialPool = NULL;
                    LtPtAppearancePool* pLtPtAppearancePool = NULL;
                    LtPtAnimationPool* pLtPtAnimationPool = NULL;
                    std::string filename( rec.getFilename() );

                    osg::notify(osg::INFO) << "External=" << filename << std::endl;

                    if (rec.getFlightVersion() > 13)
                    {
                        if (!(pSExternal->dwFlags & ExternalRecord::COLOR_PALETTE_OVERRIDE))
                            pColorPool = _pFltFile->getColorPool();

                        if (!(pSExternal->dwFlags & ExternalRecord::TEXTURE_PALETTE_OVERRIDE))
                            pTexturePool = _pFltFile->getTexturePool();

                        if (!(pSExternal->dwFlags & ExternalRecord::MATERIAL_PALETTE_OVERRIDE))
                            pMaterialPool = _pFltFile->getMaterialPool();

                        if (rec.getFlightVersion() >= 1580)
                        {
                            if (!(pSExternal->dwFlags & ExternalRecord::LIGHT_POINT_PALETTE_OVERRIDE))
                            {
                                pLtPtAppearancePool = _pFltFile->getLtPtAppearancePool();
                                pLtPtAnimationPool = _pFltFile->getLtPtAnimationPool();
                            }
                        }
                    }

                #if REGISTER_FLT
                    bool registerFLT = true;
                #else
                    bool registerFLT = false;
                #endif

                    pExternalFltFile = registerFLT ? Registry::instance()->getFltFile(filename) : NULL;
                    if (pExternalFltFile == NULL)
                    {
                        osg::ref_ptr<osgDB::ReaderWriter::Options> options =
                            _pFltFile->getOptions() ? _pFltFile->getOptions() :
                                                      new osgDB::ReaderWriter::Options;

                        //Path for Nested external references
                        osgDB::FilePathList& fpl = options->getDatabasePathList();
                        const std::string& filePath = osgDB::getFilePath(filename);
                        std::string pushAndPopPath;
                        //If absolute path
                        if( (filePath.length()>0 && filePath.find_first_of("/\\")==0) ||
                            (filePath.length()>2 && filePath.substr(1,1)==":" && filePath.find_first_of("/\\")==2) )
                        {
                            pushAndPopPath = filePath;
                        }
                        else
                        {
                            pushAndPopPath = (fpl.empty() | fpl.back().empty() ? "." : fpl.back()) + "/" + filePath;
                        }

                        /*char optionsString[256];
                        sprintf(optionsString,"FLT_VER %d",rec.getFlightVersion());
                        options->setOptionString(optionsString);*/

                        //osg::notify(osg::NOTICE)<<"Create local path"<<pushAndPopPath<<std::endl;

                        fpl.push_back(pushAndPopPath);

                        pExternalFltFile = new FltFile( pColorPool, pTexturePool, pMaterialPool,
                            pLtPtAppearancePool, pLtPtAnimationPool, options.get() );

                        if (registerFLT)
                        {
                            Registry::instance()->addFltFile(filename, pExternalFltFile);
                        }

                        pExternalFltFile->readModel(filename);

                        fpl.pop_back();
                    }

                    rec.setExternal(pExternalFltFile);
                }
            }

        public:

            FltFile* _pFltFile;
    };

void FltFile::readExternals()
{
    ReadExternal visitor(this);
    _headerRecord->accept(visitor);
}

int FltFile::getFlightVersion() const
{
    if (_headerRecord.get())
    {
        SHeader* pSHeader = (SHeader*)_headerRecord.get()->getData();
        if (pSHeader)
            return pSHeader->diFormatRevLev;
    }
    return 0;
}

void FltFile::getOrigin( double& latitude, double& longitude ) const
{
    if (_headerRecord.get())
    {
        SHeader* pSHeader = (SHeader*)_headerRecord.get()->getData();
        if (pSHeader)
        {
            latitude = pSHeader->Origin.x();
            longitude = pSHeader->Origin.y();
        }
    }
}

std::string FltFile::getDesiredUnitsString() const
{
    switch (_desiredUnits)
    {
    case ConvertToMeters:
        return "ConvertToMeters";
        break;
    case ConvertToKilometers:
        return "ConvertToKilometers";
        break;
    case ConvertToFeet:
        return "ConvertToFeet";
        break;
    case ConvertToInches:
        return "ConvertToInches";
        break;
    case ConvertToNauticalMiles:
        return "ConvertToNauticalMiles";
        break;
    default:
        return "Invalid";
        break;
    }
}

<|endoftext|>


<commit_before>/*!
  \copyright (c) RDO-Team, 2011
  \file      main.cpp
  \authors     (rdo@rk9.bmstu.ru)
  \authors     (impus@hotbox.ru)
  \date      2.10.2011
  \brief
  \indent    4T
*/

// ---------------------------------------------------------------------------- PCH
// ----------------------------------------------------------------------- INCLUDES
#define BOOST_TEST_MODULE RDOSequencesTest
#include <iostream>
#include <fstream>
#include <list>
#include <boost/test/included/unit_test.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
// ----------------------------------------------------------------------- SYNOPSIS
#include "utils/rdofile.h"
#include "simulator/runtime/rdo_random_distribution.h"
// --------------------------------------------------------------------------------

typedef std::list<double> Container;
typedef const tstring contstr;

const long int g_seed                 = 123456789;                  //!<
contstr        g_fileNormalName       = _T("data_normal.txt");      //!<
contstr        g_fileUniformName      = _T("data_uniform.txt");     //!<
contstr        g_fileExponentialName  = _T("data_exponential.txt"); //!<
contstr        g_fileTriangularName   = _T("data_trinagular.txt");  //!<
const ruint    g_count                = 100000;                     //!<
const double   g_main                 = 10.0;                       //!<
const double   g_var                  = 1.0;                        //!<
const double   g_from                 = 1.0;                        //!<
const double   g_to                   = 7.0;                        //!<
const double   g_top                  = 5.0;                        //!<
const ruint    g_precision            = 20;                         //!<

// --------------------------------------------------------------------------------
// -------Templates
// --------------------------------------------------------------------------------
template <class T, class F, class contstr>
void onGenerateData(F binder, contstr g_fileName)
{
	if (rdo::File::exist(g_fileName.c_str()))
		return;

	T sequence(g_seed);
	Container test;

	for (ruint i = 0; i < g_count; ++i)
	{
		test.push_back(binder.operator()(&sequence));
	}

	std::ofstream stream(g_fileName.c_str());
	stream.precision(g_precision);
	STL_FOR_ALL(test, it)
	{
		stream << *it << std::endl;
	}
}

template <class T, class F, class contstr>
void onCheckData(F binder, contstr g_fileName)
{
	std::ifstream stream(g_fileName.c_str());
	BOOST_CHECK(stream.good());

	Container test;
	T sequence(g_seed);
	for (ruint i = 0; i < g_count; ++i)
	{
		test.push_back(binder.operator()(&sequence));
	}

	stream.precision(g_precision);
	STL_FOR_ALL(test, it)
	{
		BOOST_CHECK(stream.good());
		tstring str;
		stream >> str;

		double val;
		BOOST_CHECK(sscanf_s(str.c_str(), _T("%lf"), &val) == 1);
		BOOST_CHECK(val == *it);
				if (val != *it)
		{
			std::cout.precision(g_precision);
			std::cout << *it << std::endl;
			std::cout << val << std::endl;
		}
	}
}
// --------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(RDOSequencesTest)
/*
// --------------------------------------------------------------------------------
// -------Normal sequence
// --------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RDONormalTestCreate)
{
	onGenerateData<rdoRuntime::RandGeneratorNormal>
		(boost::bind(&rdoRuntime::RandGeneratorNormal::next, _1, g_main, g_var), g_fileNormalName);
}

BOOST_AUTO_TEST_CASE(RDONormalTestCheck)
{
	onCheckData<rdoRuntime::RandGeneratorNormal>
		(boost::bind(&rdoRuntime::RandGeneratorNormal::next, _1, g_main, g_var), g_fileNormalName);
}
// --------------------------------------------------------------------------------

// --------------------------------------------------------------------------------
// -------Uniform sequence
// --------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RDOUniformTestCreate)
{
	onGenerateData<rdoRuntime::RandGeneratorUniform>
		(boost::bind(&rdoRuntime::RandGeneratorUniform::next, _1, g_from, g_to), g_fileUniformName);
}

BOOST_AUTO_TEST_CASE(RDOUniformTestCheck)
{
	onCheckData<rdoRuntime::RandGeneratorUniform>
		(boost::bind(&rdoRuntime::RandGeneratorUniform::next, _1, g_from, g_to), g_fileUniformName);
}
// --------------------------------------------------------------------------------
*/
// --------------------------------------------------------------------------------
// -------Exponential sequence
// --------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RDOExponentialTestCreate)
{
	onGenerateData<rdoRuntime::RandGeneratorExponential>
		(boost::bind(&rdoRuntime::RandGeneratorExponential::next, _1, g_main), g_fileExponentialName);
}

BOOST_AUTO_TEST_CASE(RDOExponentialTestCheck)
{
	onCheckData<rdoRuntime::RandGeneratorExponential>
		(boost::bind(&rdoRuntime::RandGeneratorExponential::next, _1, g_main), g_fileExponentialName);
}
// --------------------------------------------------------------------------------
/*
// --------------------------------------------------------------------------------
// -------Triangular sequence
// --------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RDOTriangularTestCreate)
{
	onGenerateData<rdoRuntime::RandGeneratorTriangular>
		(boost::bind(&rdoRuntime::RandGeneratorTriangular::next, _1, g_from, g_top, g_to), g_fileTriangularName);
}

BOOST_AUTO_TEST_CASE(RDOTriangularTestCheck)
{
	onCheckData<rdoRuntime::RandGeneratorTriangular>
		(boost::bind(&rdoRuntime::RandGeneratorTriangular::next, _1, g_from, g_top, g_to), g_fileTriangularName);
}
// --------------------------------------------------------------------------------
*/
BOOST_AUTO_TEST_SUITE_END()<commit_msg> - доведен до финишного состояния автотест законов на точное соостветствие эталонным данным<commit_after>/*!
  \copyright (c) RDO-Team, 2011
  \file      main.cpp
  \authors     (rdo@rk9.bmstu.ru)
  \authors     (impus@hotbox.ru)
  \date      2.10.2011
  \brief
  \indent    4T
*/

// ---------------------------------------------------------------------------- PCH
// ----------------------------------------------------------------------- INCLUDES
#define BOOST_TEST_MODULE RDOSequencesTest
#include <iostream>
#include <fstream>
#include <list>
#include <boost/test/included/unit_test.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
// ----------------------------------------------------------------------- SYNOPSIS
#include "utils/rdofile.h"
#include "simulator/runtime/rdo_random_distribution.h"
// --------------------------------------------------------------------------------

typedef std::list<double> Container;
typedef const tstring contstr;

const long int g_seed                 = 123456789;                  //!<
contstr        g_fileNormalName       = _T("data_normal.txt");      //!<
contstr        g_fileUniformName      = _T("data_uniform.txt");     //!<
contstr        g_fileExponentialName  = _T("data_exponential.txt"); //!<
contstr        g_fileTriangularName   = _T("data_trinagular.txt");  //!<
const ruint    g_count                = 100000;                     //!<
const double   g_main                 = 10.0;                       //!<
const double   g_var                  = 1.0;                        //!<
const double   g_from                 = 1.0;                        //!<
const double   g_to                   = 7.0;                        //!<
const double   g_top                  = 5.0;                        //!<
const ruint    g_precision            = 20;                         //!<

// --------------------------------------------------------------------------------
// -------Templates
// --------------------------------------------------------------------------------
template <class T, class F, class contstr>
void onGenerateData(F binder, contstr g_fileName)
{
	if (rdo::File::exist(g_fileName.c_str()))
		return;

	T sequence(g_seed);
	Container test;

	for (ruint i = 0; i < g_count; ++i)
	{
		test.push_back(binder.operator()(&sequence));
	}

	std::ofstream stream(g_fileName.c_str());
	stream.precision(g_precision);
	STL_FOR_ALL(test, it)
	{
		stream << *it << std::endl;
	}
}

template <class T, class F, class contstr>
void onCheckData(F binder, contstr g_fileName)
{
	std::ifstream stream(g_fileName.c_str());
	BOOST_CHECK(stream.good());

	Container test;
	T sequence(g_seed);
	for (ruint i = 0; i < g_count; ++i)
	{
		test.push_back(binder.operator()(&sequence));
	}

	stream.precision(g_precision);
	STL_FOR_ALL(test, it)
	{
		BOOST_CHECK(stream.good());
		tstring str;
		stream >> str;

		double val;
		BOOST_CHECK(sscanf_s(str.c_str(), _T("%lf"), &val) == 1);
		BOOST_CHECK(val == *it);
				if (val != *it)
		{
			std::cout.precision(g_precision);
			std::cout << *it << std::endl;
			std::cout << val << std::endl;
		}
	}
}
// --------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(RDOSequencesTest)

// --------------------------------------------------------------------------------
// -------Normal sequence
// --------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RDONormalTestCreate)
{
	onGenerateData<rdoRuntime::RandGeneratorNormal>
		(boost::bind(&rdoRuntime::RandGeneratorNormal::next, _1, g_main, g_var), g_fileNormalName);
}

BOOST_AUTO_TEST_CASE(RDONormalTestCheck)
{
	onCheckData<rdoRuntime::RandGeneratorNormal>
		(boost::bind(&rdoRuntime::RandGeneratorNormal::next, _1, g_main, g_var), g_fileNormalName);
}
// --------------------------------------------------------------------------------

// --------------------------------------------------------------------------------
// -------Uniform sequence
// --------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RDOUniformTestCreate)
{
	onGenerateData<rdoRuntime::RandGeneratorUniform>
		(boost::bind(&rdoRuntime::RandGeneratorUniform::next, _1, g_from, g_to), g_fileUniformName);
}

BOOST_AUTO_TEST_CASE(RDOUniformTestCheck)
{
	onCheckData<rdoRuntime::RandGeneratorUniform>
		(boost::bind(&rdoRuntime::RandGeneratorUniform::next, _1, g_from, g_to), g_fileUniformName);
}
// --------------------------------------------------------------------------------

// --------------------------------------------------------------------------------
// -------Exponential sequence
// --------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RDOExponentialTestCreate)
{
	onGenerateData<rdoRuntime::RandGeneratorExponential>
		(boost::bind(&rdoRuntime::RandGeneratorExponential::next, _1, g_main), g_fileExponentialName);
}

BOOST_AUTO_TEST_CASE(RDOExponentialTestCheck)
{
	onCheckData<rdoRuntime::RandGeneratorExponential>
		(boost::bind(&rdoRuntime::RandGeneratorExponential::next, _1, g_main), g_fileExponentialName);
}
// --------------------------------------------------------------------------------

// --------------------------------------------------------------------------------
// -------Triangular sequence
// --------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RDOTriangularTestCreate)
{
	onGenerateData<rdoRuntime::RandGeneratorTriangular>
		(boost::bind(&rdoRuntime::RandGeneratorTriangular::next, _1, g_from, g_top, g_to), g_fileTriangularName);
}

BOOST_AUTO_TEST_CASE(RDOTriangularTestCheck)
{
	onCheckData<rdoRuntime::RandGeneratorTriangular>
		(boost::bind(&rdoRuntime::RandGeneratorTriangular::next, _1, g_from, g_top, g_to), g_fileTriangularName);
}
// --------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE_END()<|endoftext|>


<commit_before>#include "../../../include/pixkit-image.hpp"

using namespace	cv;
using namespace std;

// SCDBS
bool SCDBS(const cv::Mat &src1b, const cv::Mat &init1b,const bool randGenInit,cv::Mat &dst1b,double *autocoeData,int FilterSize){

	//////////////////////////////////////////////////////////////////////////
	/// exceptions
	if(src1b.type()!=CV_8UC1){
		assert(false);
	}
	if(FilterSize==1){
		assert(false);
	}else if(FilterSize%2==0){
		assert(false);
	}

	//////////////////////////////////////////////////////////////////////////
	/// initialization
	const	int	&m_Height	=	src1b.rows;
	const	int	&m_Width	=	src1b.cols;
	Mat	dst1f;
	dst1f.create(src1b.size(),CV_32FC1);

	//////////////////////////////////////////////////////////////////////////
	/// get autocorrelation.
	int	exFS=FilterSize;
	int	tempFS=FilterSize/2;
	double	**	autocoe		=	new	double	*	[exFS];
	for(int i=0;i<exFS;i++){
		autocoe[i]=&autocoeData[i*exFS];
	}

	//////////////////////////////////////////////////////////////////////////
	/// load original image
	Mat	src1f(src1b.size(),CV_32FC1);
	src1b.convertTo(src1f,CV_32FC1);
	// get initial image
	init1b.convertTo(dst1f,CV_32FC1);

	//////////////////////////////////////////////////////////////////////////
	/// Change grayscale to absorb
	src1f	=	1.-src1f/255.;
	dst1f	=	1.-dst1f/255.;

	/// get error matrix
	Mat	em1f(src1b.size(),CV_32FC1);
	em1f	=	dst1f	-	src1f;

	/// get cross correlation
	double	*	crosscoeData	=	new	double		[m_Height*m_Width];
	double	**	crosscoe		=	new	double	*	[m_Height];
	for(int i=0;i<m_Height;i++){
		crosscoe[i]=&crosscoeData[i*m_Width];
		for(int j=0;j<m_Width;j++){
			crosscoe[i][j]=0.;
			for(int m=i-tempFS;m<=i+tempFS;m++){
				for(int n=j-tempFS;n<=j+tempFS;n++){
					if(m>=0&&m<m_Height&&n>=0&&n<m_Width){
						crosscoe[i][j]+=em1f.ptr<float>(m)[n]*autocoe[tempFS+m-i][tempFS+n-j];
					}
				}
			}
		}
	}

	//////////////////////////////////////////////////////////////////////////
	/// DBS process
	int		BenefitPixelNumber;
	double	dE[10],a0[10],a1[10];
	while(1){
		BenefitPixelNumber=0;
		for(int i=0;i<m_Height;i++){	// entire image
			for(int j=0;j<m_Width;j++){

				//////////////////////////////////////////////////////////////////////////
				// check whether the current position is needed to be processed or not
				if(!randGenInit){
					if(init1b.ptr<uchar>(i)[j]==255){	// inherit
						float	initv	=	1 - (float)init1b.ptr<uchar>(i)[j]/255,	// change to absorb for comparison
								dstv	=	dst1f.ptr<float>(i)[j];
						CV_DbgAssert(initv==dstv);	// these two should be the same
						continue;	// ignore.
					}
				}

				//////////////////////////////////////////////////////////////////////////
				// = = = = = trial part = = = = = //
				// initialize psnr		0: original psnr, 1~8: Swap, 9: Toggel.
				// 8 1 2
				// 7 0 3
				// 6 5 4
				for(int m=0;m<10;m++){
					dE[m]=0.;	// original error =0
					a0[m]=0.;
					a1[m]=0.;
				}
				// change the delta error as per different replacement methods
				for(int mode=1;mode<10;mode++){
					int		m,n;
					if(mode>=1&&mode<=8){
						// set position
						if(mode==1){
							m=1;	n=0;
						}else if(mode==2){
							m=1;	n=1;
						}else if(mode==3){
							m=0;	n=1;
						}else if(mode==4){
							m=-1;	n=1;
						}else if(mode==5){
							m=-1;	n=0;
						}else if(mode==6){
							m=-1;	n=-1;
						}else if(mode==7){
							m=0;	n=-1;
						}else if(mode==8){
							m=1;	n=-1;
						}

						//////////////////////////////////////////////////////////////////////////
						// get dE
						if(i+m>=0&&i+m<m_Height&&j+n>=0&&j+n<m_Width){
							if(!randGenInit){
								if(init1b.ptr<uchar>(i+m)[j+n]==255){	// inherit
									continue;	// ignore
								}
							}
							// get a0 and a1
							if(dst1f.ptr<float>(i)[j]==1){
								a0[mode]=-1;
							}else{
								a0[mode]=1;
							}
							if(dst1f.ptr<float>(i+m)[j+n]==1){
								a1[mode]=-1;
							}else{
								a1[mode]=1;
							}
							// get error
							if(dst1f.ptr<float>(i)[j]!=dst1f.ptr<float>(i+m)[j+n]){
								dE[mode]=(a0[mode]*a0[mode]+a1[mode]*a1[mode])	*autocoe[tempFS][tempFS]	+	2.*a0[mode]	*crosscoe[i][j]		+	2.*a0[mode]*a1[mode]					*autocoe[tempFS+m][tempFS+n]	+	2.*a1[mode]								*crosscoe[i+m][j+n];
							}
						}
					}else if(mode==9){
						if(dst1f.ptr<float>(i)[j]==1){
							a0[mode]=-1;
						}else{
							a0[mode]=1;
						}
						dE[mode]=	autocoe[tempFS][tempFS]	+	2.*a0[mode]*crosscoe[i][j];
					}
				}

				//////////////////////////////////////////////////////////////////////////
				///// get minimum delta error and its position
				int		tempMinNumber	=0;
				double	tempMindE		=dE[0];	// original error =0
				for(int x=1;x<10;x++){
					if(dE[x]<tempMindE){	// get smaller error only
						tempMindE		=dE[x];
						tempMinNumber	=x;
					}
				}

				//////////////////////////////////////////////////////////////////////////
				// check swap position
				int nm,nn;
				if(tempMinNumber==1){
					nm=1;	nn=0;
				}else if(tempMinNumber==2){
					nm=1;	nn=1;
				}else if(tempMinNumber==3){
					nm=0;	nn=1;
				}else if(tempMinNumber==4){
					nm=-1;	nn=1;
				}else if(tempMinNumber==5){
					nm=-1;	nn=0;
				}else if(tempMinNumber==6){
					nm=-1;	nn=-1;
				}else if(tempMinNumber==7){
					nm=0;	nn=-1;
				}else if(tempMinNumber==8){
					nm=1;	nn=-1;
				}

				//////////////////////////////////////////////////////////////////////////
				// = = = = = update part = = = = = //
				if(tempMindE<0.){	// error is reduced
					// update current hft position
					dst1f.ptr<float>(i)[j]	=1.-dst1f.ptr<float>(i)[j];
					if(tempMinNumber>=1&&tempMinNumber<=8){	// swap case
						// get position
						int nm,nn;
						if(tempMinNumber==1){
							nm=1;	nn=0;
						}else if(tempMinNumber==2){
							nm=1;	nn=1;
						}else if(tempMinNumber==3){
							nm=0;	nn=1;
						}else if(tempMinNumber==4){
							nm=-1;	nn=1;
						}else if(tempMinNumber==5){
							nm=-1;	nn=0;
						}else if(tempMinNumber==6){
							nm=-1;	nn=-1;
						}else if(tempMinNumber==7){
							nm=0;	nn=-1;
						}else if(tempMinNumber==8){
							nm=1;	nn=-1;
						}
						// update swapped hft position
						dst1f.ptr<float>(i+nm)[j+nn]	=	1.	-	dst1f.ptr<float>(i+nm)[j+nn];

						// update cross correlation
						for(int m=-tempFS;m<=tempFS;m++){
							for(int n=-tempFS;n<=tempFS;n++){
								if(i+m>=0&&i+m<m_Height&&j+n>=0&&j+n<m_Width){
									crosscoe[i+m][j+n]+=a0[tempMinNumber]*autocoe[tempFS+m][tempFS+n];
								}
								if(i+m+nm>=0&&i+m+nm<m_Height&&j+n+nn>=0&&j+n+nn<m_Width){
									crosscoe[i+m+nm][j+n+nn]+=a1[tempMinNumber]*autocoe[tempFS+m][tempFS+n];
								}
							}
						}
					}else if(tempMinNumber==9){	// toggle case
						// update cross correlation
						for(int m=-tempFS;m<=tempFS;m++){
							for(int n=-tempFS;n<=tempFS;n++){
								if(i+m>=0&&i+m<m_Height&&j+n>=0&&j+n<m_Width){
									crosscoe[i+m][j+n]+=a0[tempMinNumber]*autocoe[tempFS+m][tempFS+n];
								}
							}
						}
					}
					BenefitPixelNumber++;
				} // end of entire image
			}
		}
		if(BenefitPixelNumber==0){
			break;
		}
	}

	//////////////////////////////////////////////////////////////////////////
	/// Change absorb to grayscale
	dst1b.create(src1b.size(),CV_8UC1);
	for(int i=0;i<m_Height;i++){
		for(int j=0;j<m_Width;j++){
			dst1b.ptr<uchar>(i)[j]=(1.-dst1f.ptr<float>(i)[j])*255.;
		}
	}

	//////////////////////////////////////////////////////////////////////////
	/// release space
	delete	[]	crosscoeData;
	delete	[]	crosscoe;
	delete	[]	autocoe;

	return true;
}

//////////////////////////////////////////////////////////////////////////
bool pixkit::halftoning::ordereddithering::DMS_2Level2012_genDitherArray(cv::Mat &DA, int daSize){

	//////////////////////////////////////////////////////////////////////////
	///// init
	const	uchar	MAX_GRAYSCALE	=	255;
	const	uchar	MIN_GRAYSCALE	=	0;

	//////////////////////////////////////////////////////////////////////////
	///// initialization
	cv::Mat compressedTable1b, src1b, dst1b;
	compressedTable1b.create(Size(daSize, daSize), CV_8UC1);
	src1b.create(Size(daSize, daSize), CV_8UC1);
	dst1b.create(Size(daSize, daSize), CV_8UC1);
	src1b.setTo(MIN_GRAYSCALE);
	dst1b.setTo(MIN_GRAYSCALE);
	compressedTable1b.setTo(MIN_GRAYSCALE);
	// get coe
	Mat	hvs_model_cpp;
	pixkit::halftoning::ungrouped::generateTwoComponentGaussianModel(hvs_model_cpp,43.2,38.7,0.02,0.06);

	//////////////////////////////////////////////////////////////////////////
	///// process for masks of 0 to 255
	Mat	pre_dst1b;
	Mat	init1b(Size(daSize, daSize), CV_8UC1);
	init1b.setTo(MIN_GRAYSCALE);
	for (int grayscale = MIN_GRAYSCALE; grayscale <= MAX_GRAYSCALE; grayscale++){
		cout << "grayscale = " << (int)grayscale << endl;

		//////////////////////////////////////////////////////////////////////////
		///// init
		src1b.setTo(grayscale);
		pre_dst1b = dst1b.clone(); // recode the result of dst(g-1)

		//////////////////////////////////////////////////////////////////////////
		///// rand generates the initial mask
		bool	randGenInit	=false;
		if(cv::sum(init1b)[0]==0){
			randGenInit	=	true;
			srand(0);
			for(int i=0;i<init1b.rows;i++){
				for(int j=0;j<init1b.cols;j++){
					double	temp=((double)rand())/32767.;
					init1b.ptr<uchar>(i)[j]=temp<0.5?MIN_GRAYSCALE:MAX_GRAYSCALE;
				}
			}
		}

		//////////////////////////////////////////////////////////////////////////
		///// process
		SCDBS(src1b,init1b,randGenInit,dst1b,&(double&)hvs_model_cpp.data[0],hvs_model_cpp.rows);

		//////////////////////////////////////////////////////////////////////////
		///// get init
		init1b	=	dst1b.clone();

		//////////////////////////////////////////////////////////////////////////
		///// record position that the white point is firstly toggled
		for (int i = 0; i < dst1b.rows; i++){
			for (int j = 0; j < dst1b.cols; j++){
				if (dst1b.ptr<uchar>(i)[j] == MAX_GRAYSCALE &&  pre_dst1b.ptr<uchar>(i)[j] == MIN_GRAYSCALE){
					compressedTable1b.ptr<uchar>(i)[j]	=	grayscale;
				}
			}
		}
	}

	DA	=	compressedTable1b.clone();

	return true;
}

bool pixkit::halftoning::ordereddithering::DMS_2Level2012(const cv::Mat &src1b, const cv::Mat &ditherarray1b,cv::Mat &dst1b){
	dst1b.create(src1b.size(),src1b.type());
	for(int i=0;i<src1b.rows;i++){
		for(int j=0;j<src1b.cols;j++){
			if(src1b.ptr<uchar>(i)[j]>=ditherarray1b.ptr<uchar>(i%ditherarray1b.rows)[j%ditherarray1b.cols]){
				dst1b.ptr<uchar>(i)[j]	=	255;
			}else{
				dst1b.ptr<uchar>(i)[j]	=	0;
			}
		}
	}
	return true;
}<commit_msg>add warp-around property<commit_after>#include "../../../include/pixkit-image.hpp"

using namespace	cv;
using namespace std;

// coordination correction
inline int	cirCC(int cor,int limitation){
	int	tempv	=	cor%limitation;
	if(tempv>=0){
		return	tempv;
	}else{	// if(tempv<0)
		return	limitation+tempv;
	}
}

// SCDBS
bool SCDBS(const cv::Mat &src1b, const cv::Mat &init1b,const bool first_grayscale,cv::Mat &dst1b,double *c_ppData,int FilterSize){

	//////////////////////////////////////////////////////////////////////////
	/// exceptions
	if(src1b.type()!=CV_8UC1){
		assert(false);
	}
	if(FilterSize==1){
		assert(false);
	}else if(FilterSize%2==0){
		assert(false);
	}

	//////////////////////////////////////////////////////////////////////////
	/// initialization
	const	int	&height	=	src1b.rows;
	const	int	&width	=	src1b.cols;
	Mat	dst1f;
	dst1f.create(src1b.size(),CV_32FC1);

	//////////////////////////////////////////////////////////////////////////
	/// get autocorrelation.
	int	exFS=FilterSize;
	int	tempFS=FilterSize/2;
	double	**	c_pp		=	new	double	*	[exFS];
	for(int i=0;i<exFS;i++){
		c_pp[i]=&c_ppData[i*exFS];
	}

	//////////////////////////////////////////////////////////////////////////
	/// load original image
	Mat	src1f(src1b.size(),CV_32FC1);
	src1b.convertTo(src1f,CV_32FC1);
	// get initial image
	init1b.convertTo(dst1f,CV_32FC1);

	//////////////////////////////////////////////////////////////////////////
	/// Change grayscale to absorb
	src1f	=	1.-src1f/255.;
	dst1f	=	1.-dst1f/255.;

	/// get error matrix
	Mat	em1f(src1b.size(),CV_32FC1);
	em1f	=	dst1f	-	src1f;

	/// get cross correlation
	Mat	crosscoe1d(Size(width,height),CV_64FC1);
	crosscoe1d.setTo(0);
	for(int i=0;i<crosscoe1d.rows;i++){
		for(int j=0;j<crosscoe1d.cols;j++){
			for(int m=i-tempFS;m<=i+tempFS;m++){
				for(int n=j-tempFS;n<=j+tempFS;n++){
					crosscoe1d.ptr<double>(i)[j]+=em1f.ptr<float>(cirCC(m,height))[cirCC(n,width)]*c_pp[tempFS+m-i][tempFS+n-j];
				}
			}
		}
	}

	//////////////////////////////////////////////////////////////////////////
	///// DBS process
	int		BenefitPixelNumber;
	double	dE[10],a0[10],a1[10];
	while(1){
		BenefitPixelNumber=0;
		for(int i=0;i<height;i++){	// entire image
			for(int j=0;j<width;j++){

				//////////////////////////////////////////////////////////////////////////
				// check whether the current position is needed to be processed or not
				if(!first_grayscale){
					if(init1b.ptr<uchar>(i)[j]==255){	// inherit
						float	initv	=	1 - (float)init1b.ptr<uchar>(i)[j]/255,	// change to absorb for comparison
								dstv	=	dst1f.ptr<float>(i)[j];
						CV_DbgAssert(initv==dstv);	// these two should be the same
						continue;	// ignore.
					}
				}

				//////////////////////////////////////////////////////////////////////////
				// = = = = = trial part = = = = = //
				// initialize psnr		0: original psnr, 1~8: Swap, 9: Toggel.
				// 8 1 2
				// 7 0 3
				// 6 5 4
				for(int m=0;m<10;m++){
					dE[m]=0.;	// original error =0
					a0[m]=0.;
					a1[m]=0.;
				}
				// change the delta error as per different replacement methods
				for(int mode=1;mode<10;mode++){
					int		m,n;
					if(mode>=1&&mode<=8){
						// set position
						if(mode==1){
							m=1;	n=0;
						}else if(mode==2){
							m=1;	n=1;
						}else if(mode==3){
							m=0;	n=1;
						}else if(mode==4){
							m=-1;	n=1;
						}else if(mode==5){
							m=-1;	n=0;
						}else if(mode==6){
							m=-1;	n=-1;
						}else if(mode==7){
							m=0;	n=-1;
						}else if(mode==8){
							m=1;	n=-1;
						}

						//////////////////////////////////////////////////////////////////////////
						// get dE
						if(!first_grayscale){
							if(init1b.ptr<uchar>(cirCC(i+m,height))[cirCC(j+n,width)]==255){	// inherit
								continue;	// ignore
							}
						}
						// get a0 and a1
						if(dst1f.ptr<float>(i)[j]==1){
							a0[mode]=-1;
						}else{
							a0[mode]=1;
						}
						if(dst1f.ptr<float>(cirCC(i+m,height))[cirCC(j+n,width)]==1){
							a1[mode]=-1;
						}else{
							a1[mode]=1;
						}
						// get error
						if(dst1f.ptr<float>(i)[j]!=dst1f.ptr<float>(cirCC(i+m,height))[cirCC(j+n,width)]){
							dE[mode]=(a0[mode]*a0[mode]+a1[mode]*a1[mode])	*c_pp[tempFS][tempFS]	+	2.*a0[mode]	*crosscoe1d.ptr<double>(i)[j]
							+2.*a0[mode]*a1[mode]	*	c_pp[tempFS+m][tempFS+n]
							+2.*a1[mode]			*	crosscoe1d.ptr<double>(cirCC(i+m,height))[cirCC(j+n,width)];
						}
					}else if(mode==9){
						if(dst1f.ptr<float>(i)[j]==1){
							a0[mode]=-1;
						}else{
							a0[mode]=1;
						}
						dE[mode]=	c_pp[tempFS][tempFS]	+	2.*a0[mode]*crosscoe1d.ptr<double>(i)[j];
					}
				}

				//////////////////////////////////////////////////////////////////////////
				///// get minimum delta error and its position
				int		tempMinNumber	=0;
				double	tempMindE		=dE[0];	// original error =0
				for(int x=1;x<10;x++){
					if(dE[x]<tempMindE){	// get smaller error only
						tempMindE		=dE[x];
						tempMinNumber	=x;
					}
				}

				//////////////////////////////////////////////////////////////////////////
				// = = = = = update part = = = = = //
				if(tempMindE<0.){	// error is reduced
					// update current hft position
					dst1f.ptr<float>(i)[j]	=	saturate_cast<float>(1.-dst1f.ptr<float>(i)[j]);
					if(tempMinNumber>=1&&tempMinNumber<=8){	// swap case
						// get position, and check swap position
						int nm,nn;
						if(tempMinNumber==1){
							nm=1;	nn=0;
						}else if(tempMinNumber==2){
							nm=1;	nn=1;
						}else if(tempMinNumber==3){
							nm=0;	nn=1;
						}else if(tempMinNumber==4){
							nm=-1;	nn=1;
						}else if(tempMinNumber==5){
							nm=-1;	nn=0;
						}else if(tempMinNumber==6){
							nm=-1;	nn=-1;
						}else if(tempMinNumber==7){
							nm=0;	nn=-1;
						}else if(tempMinNumber==8){
							nm=1;	nn=-1;
						}
						// update swapped hft position
						dst1f.ptr<float>(cirCC(i+nm,height))[cirCC(j+nn,width)]	=	saturate_cast<float>(1.	-	dst1f.ptr<float>(cirCC(i+nm,height))[cirCC(j+nn,width)]);

						// update cross correlation
						for(int m=-tempFS;m<=tempFS;m++){
							for(int n=-tempFS;n<=tempFS;n++){
								crosscoe1d.ptr<double>(cirCC(i+m,height))[cirCC(j+n,width)]+=a0[tempMinNumber]*c_pp[tempFS+m][tempFS+n];
								crosscoe1d.ptr<double>(cirCC(i+m+nm,height))[cirCC(j+n+nn,width)]+=a1[tempMinNumber]*c_pp[tempFS+m][tempFS+n];
							}
						}
					}else if(tempMinNumber==9){	// toggle case
						// update cross correlation
						for(int m=-tempFS;m<=tempFS;m++){
							for(int n=-tempFS;n<=tempFS;n++){
								crosscoe1d.ptr<double>(cirCC(i+m,height))[cirCC(j+n,width)]+=a0[tempMinNumber]*c_pp[tempFS+m][tempFS+n];
							}
						}
					}
					BenefitPixelNumber++;
				} // end of entire image
			}
		}
		if(BenefitPixelNumber==0){
			break;
		}
	}

	//////////////////////////////////////////////////////////////////////////
	/// Change absorb to grayscale
	dst1b.create(src1b.size(),CV_8UC1);
	for(int i=0;i<height;i++){
		for(int j=0;j<width;j++){
			dst1b.ptr<uchar>(i)[j]=cvRound((1.-dst1f.ptr<float>(i)[j])*255.);
		}
	}

	//////////////////////////////////////////////////////////////////////////
	/// release space
	delete	[]	c_pp;

	return true;
}

//////////////////////////////////////////////////////////////////////////
bool pixkit::halftoning::ordereddithering::DMS_2Level2012_genDitherArray(cv::Mat &DA, int daSize){
// the N defined in paper is supposed as daSize in this program.

	//////////////////////////////////////////////////////////////////////////
	///// init
	const	uchar	MAX_GRAYSCALE	=	255;	// defined R in paper
	const	uchar	MIN_GRAYSCALE	=	0;
	const	uchar	afa_1			=	0;		// parameter defined in paper

	//////////////////////////////////////////////////////////////////////////
	///// initialization
	cv::Mat compressedTable1b, src1b, dst1b;
	compressedTable1b.create(Size(daSize, daSize), CV_8UC1);
	src1b.create(Size(daSize, daSize), CV_8UC1);
	dst1b.create(Size(daSize, daSize), CV_8UC1);
	src1b.setTo(MIN_GRAYSCALE);
	dst1b.setTo(MIN_GRAYSCALE);
	compressedTable1b.setTo(MIN_GRAYSCALE);
	// get coe
	Mat	hvs_model_cpp;
	pixkit::halftoning::ungrouped::generateTwoComponentGaussianModel(hvs_model_cpp,43.2,38.7,0.02,0.06);	// checked

	//////////////////////////////////////////////////////////////////////////
	///// process for masks of 0 to 255
	Mat	pre_dst1b;
	Mat	init1b(Size(daSize, daSize), CV_8UC1);
	init1b.setTo(afa_1);	// as described in paper, this value should be zero.
	for (int eta = MIN_GRAYSCALE; eta <= MAX_GRAYSCALE; eta++){		// eta is defined as a grayscale in paper
		cout << "grayscale = " << (int)eta << endl;

		//////////////////////////////////////////////////////////////////////////
		///// init
		src1b.setTo(eta);
		pre_dst1b = dst1b.clone(); // recode the result of dst(g-1)

		//////////////////////////////////////////////////////////////////////////
		///// process
		// check whether it's the first grayscale
		bool	first_grayscale	=false;
		if(cv::sum(init1b)[0]==0){
			first_grayscale	=	true;
		}
		// process
		SCDBS(src1b,init1b,first_grayscale,dst1b,&(double&)hvs_model_cpp.data[0],hvs_model_cpp.rows);

		//////////////////////////////////////////////////////////////////////////
		///// get init
		init1b	=	dst1b.clone();

		//////////////////////////////////////////////////////////////////////////
		///// record position that the white point is firstly toggled
		for (int i = 0; i < dst1b.rows; i++){
			for (int j = 0; j < dst1b.cols; j++){
				if (dst1b.ptr<uchar>(i)[j] == MAX_GRAYSCALE &&  pre_dst1b.ptr<uchar>(i)[j] == MIN_GRAYSCALE){
					compressedTable1b.ptr<uchar>(i)[j]	=	eta;
				}
			}
		}
	}

	DA	=	compressedTable1b.clone();

	return true;
}

bool pixkit::halftoning::ordereddithering::DMS_2Level2012(const cv::Mat &src1b, const cv::Mat &ditherarray1b,cv::Mat &dst1b){
	dst1b.create(src1b.size(),src1b.type());
	for(int i=0;i<src1b.rows;i++){
		for(int j=0;j<src1b.cols;j++){
			if(src1b.ptr<uchar>(i)[j]>=ditherarray1b.ptr<uchar>(i%ditherarray1b.rows)[j%ditherarray1b.cols]){
				dst1b.ptr<uchar>(i)[j]	=	255;
			}else{
				dst1b.ptr<uchar>(i)[j]	=	0;
			}
		}
	}
	return true;
}<|endoftext|>


<commit_before>/*
    This file is part of Magnum.

    Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016
              Vladimír Vondruš <mosra@centrum.cz>

    Permission is hereby granted, free of charge, to any person obtaining a
    copy of this software and associated documentation files (the "Software"),
    to deal in the Software without restriction, including without limitation
    the rights to use, copy, modify, merge, publish, distribute, sublicense,
    and/or sell copies of the Software, and to permit persons to whom the
    Software is furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included
    in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
    THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
    DEALINGS IN THE SOFTWARE.
*/

#include <Corrade/Utility/Resource.h>
#include <Magnum/AbstractShaderProgram.h>
#include <Magnum/Buffer.h>
#include <Magnum/Context.h>
#include <Magnum/DefaultFramebuffer.h>
#include <Magnum/Framebuffer.h>
#include <Magnum/Image.h>
#include <Magnum/PixelFormat.h>
#include <Magnum/Renderbuffer.h>
#include <Magnum/RenderbufferFormat.h>
#include <Magnum/Renderer.h>
#include <Magnum/Shader.h>
#include <Magnum/Texture.h>
#include <Magnum/TextureFormat.h>
#include <Magnum/Version.h>
#include <Magnum/Math/Color.h>
#include <Magnum/MeshTools/CompressIndices.h>
#include <Magnum/MeshTools/Interleave.h>
#include <Magnum/Platform/Sdl2Application.h>
#include <Magnum/Primitives/Cube.h>
#include <Magnum/Primitives/Plane.h>
#include <Magnum/Primitives/UVSphere.h>
#include <Magnum/Trade/MeshData3D.h>
#include <Magnum/SceneGraph/Scene.h>
#include <Magnum/SceneGraph/MatrixTransformation3D.h>
#include <Magnum/SceneGraph/Camera.h>
#include <Magnum/SceneGraph/Drawable.h>

using namespace Magnum;

typedef SceneGraph::Object<SceneGraph::MatrixTransformation3D> Object3D;
typedef SceneGraph::Scene<SceneGraph::MatrixTransformation3D> Scene3D;

class PhongIdShader: public AbstractShaderProgram {
    public:
        typedef Attribute<0, Vector3> Position;
        typedef Attribute<1, Vector3> Normal;

        enum: UnsignedInt {
            ColorOutput = 0,
            ObjectIdOutput = 1
        };

        explicit PhongIdShader();

        PhongIdShader& setObjectId(Int id) {
            setUniform(_objectIdUniform, id);
            return *this;
        }

        PhongIdShader& setLightPosition(const Vector3& position) {
            setUniform(_lightPositionUniform, position);
            return *this;
        }

        PhongIdShader& setAmbientColor(const Color3& color) {
            setUniform(_ambientColorUniform, color);
            return *this;
        }

        PhongIdShader& setColor(const Color3& color) {
            setUniform(_colorUniform, color);
            return *this;
        }

        PhongIdShader& setTransformationMatrix(const Matrix4& matrix) {
            setUniform(_transformationMatrixUniform, matrix);
            return *this;
        }

        PhongIdShader& setNormalMatrix(const Matrix3x3& matrix) {
            setUniform(_normalMatrixUniform, matrix);
            return *this;
        }

        PhongIdShader& setProjectionMatrix(const Matrix4& matrix) {
            setUniform(_projectionMatrixUniform, matrix);
            return *this;
        }

    private:
        Int _objectIdUniform,
            _lightPositionUniform,
            _ambientColorUniform,
            _colorUniform,
            _transformationMatrixUniform,
            _normalMatrixUniform,
            _projectionMatrixUniform;
};

PhongIdShader::PhongIdShader() {
    Utility::Resource rs("picking-data");

    Shader vert{Version::GL330, Shader::Type::Vertex},
        frag{Version::GL330, Shader::Type::Fragment};
    vert.addSource(rs.get("PhongId.vert"));
    frag.addSource(rs.get("PhongId.frag"));
    CORRADE_INTERNAL_ASSERT(Shader::compile({vert, frag}));
    attachShaders({vert, frag});
    CORRADE_INTERNAL_ASSERT(link());

    _objectIdUniform = uniformLocation("objectId");
    _lightPositionUniform = uniformLocation("light");
    _ambientColorUniform = uniformLocation("ambientColor");
    _colorUniform = uniformLocation("color");
    _transformationMatrixUniform = uniformLocation("transformationMatrix");
    _projectionMatrixUniform = uniformLocation("projectionMatrix");
    _normalMatrixUniform = uniformLocation("normalMatrix");
}

class PickableObject: public Object3D, SceneGraph::Drawable3D {
    public:
        explicit PickableObject(UnsignedByte id, PhongIdShader& shader, const Color3& color, Mesh& mesh, Object3D& parent, SceneGraph::DrawableGroup3D& drawables): Object3D{&parent}, SceneGraph::Drawable3D{*this, &drawables}, _id{id}, _selected{false}, _shader(shader), _color{color}, _mesh(mesh) {}

        void setSelected(bool selected) { _selected = selected; }

    private:
        virtual void draw(const Matrix4& transformationMatrix, SceneGraph::Camera3D& camera) {
            _shader.setTransformationMatrix(transformationMatrix)
                .setNormalMatrix(transformationMatrix.rotationScaling())
                .setProjectionMatrix(camera.projectionMatrix())
                .setAmbientColor(_selected ? _color*0.3f : Color3{})
                .setColor(_color*(_selected ? 2.0f : 1.0f))
                /* relative to the camera */
                .setLightPosition({13.0f, 2.0f, 5.0f})
                .setObjectId(_id);
            _mesh.draw(_shader);
        }

        UnsignedByte _id;
        bool _selected;
        PhongIdShader& _shader;
        Color3 _color;
        Mesh& _mesh;
};

class PickingExample: public Platform::Application {
    public:
        explicit PickingExample(const Arguments& arguments);

    private:
        void drawEvent() override;
        void mousePressEvent(MouseEvent& event) override;
        void mouseMoveEvent(MouseMoveEvent& event) override;
        void mouseReleaseEvent(MouseEvent& event) override;

        Scene3D _scene;
        Object3D* _cameraObject;
        SceneGraph::Camera3D* _camera;
        SceneGraph::DrawableGroup3D _drawables;

        PhongIdShader _shader;
        Buffer _cubeVertices, _cubeIndices,
            _sphereVertices, _sphereIndices,
            _planeVertices;
        Mesh _cube, _plane, _sphere;

        enum { ObjectCount = 6 };
        PickableObject* _objects[ObjectCount];

        Framebuffer _framebuffer;
        Renderbuffer _color, _objectId, _depth;

        Vector2i _previousMousePosition, _mousePressPosition;
};

PickingExample::PickingExample(const Arguments& arguments): Platform::Application{arguments, Configuration{}.setTitle("Magnum object picking example")}, _framebuffer{defaultFramebuffer.viewport()} {
    MAGNUM_ASSERT_VERSION_SUPPORTED(Version::GL330);

    /* Global renderer configuration */
    Renderer::enable(Renderer::Feature::DepthTest);

    /* Configure framebuffer (using R8UI for object ID which means 255 objects max) */
    _color.setStorage(RenderbufferFormat::RGBA8, defaultFramebuffer.viewport().size());
    _objectId.setStorage(RenderbufferFormat::R8UI, defaultFramebuffer.viewport().size());
    _depth.setStorage(RenderbufferFormat::DepthComponent24, defaultFramebuffer.viewport().size());
    _framebuffer.attachRenderbuffer(Framebuffer::ColorAttachment{0}, _color)
               .attachRenderbuffer(Framebuffer::ColorAttachment{1}, _objectId)
               .attachRenderbuffer(Framebuffer::BufferAttachment::Depth, _depth)
               .mapForDraw({{PhongIdShader::ColorOutput, Framebuffer::ColorAttachment{0}},
                            {PhongIdShader::ObjectIdOutput, Framebuffer::ColorAttachment{1}}});
    CORRADE_INTERNAL_ASSERT(_framebuffer.checkStatus(FramebufferTarget::Draw) == Framebuffer::Status::Complete);

    /* Set up meshes */
    {
        Trade::MeshData3D data = Primitives::Cube::solid();
        _cubeVertices.setData(MeshTools::interleave(data.positions(0), data.normals(0)), BufferUsage::StaticDraw);
        _cubeIndices.setData(MeshTools::compressIndicesAs<UnsignedShort>(data.indices()), BufferUsage::StaticDraw);
        _cube.setCount(data.indices().size())
            .setPrimitive(data.primitive())
            .addVertexBuffer(_cubeVertices, 0, PhongIdShader::Position{}, PhongIdShader::Normal{})
            .setIndexBuffer(_cubeIndices, 0, Mesh::IndexType::UnsignedShort);
    } {
        Trade::MeshData3D data = Primitives::UVSphere::solid(16, 32);
        _sphereVertices.setData(MeshTools::interleave(data.positions(0), data.normals(0)), BufferUsage::StaticDraw);
        _sphereIndices.setData(MeshTools::compressIndicesAs<UnsignedShort>(data.indices()), BufferUsage::StaticDraw);
        _sphere.setCount(data.indices().size())
            .setPrimitive(data.primitive())
            .addVertexBuffer(_sphereVertices, 0, PhongIdShader::Position{}, PhongIdShader::Normal{})
            .setIndexBuffer(_sphereIndices, 0, Mesh::IndexType::UnsignedShort);
    } {
        Trade::MeshData3D data = Primitives::Plane::solid();
        _planeVertices.setData(MeshTools::interleave(data.positions(0), data.normals(0)), BufferUsage::StaticDraw);
        _plane.setCount(data.positions(0).size())
            .setPrimitive(data.primitive())
            .addVertexBuffer(_planeVertices, 0, PhongIdShader::Position{}, PhongIdShader::Normal{});
    }

    /* Set up objects */
    (*(_objects[0] = new PickableObject{1, _shader, Color3::fromHSV(25.0_degf, 0.9f, 0.9f), _cube, _scene, _drawables}))
        .rotate(34.0_degf, Vector3(1.0f).normalized())
        .translate({1.0f, 0.3f, -1.2f});
    (*(_objects[1] = new PickableObject{2, _shader, Color3::fromHSV(54.0_degf, 0.9f, 0.9f), _sphere, _scene, _drawables}))
        .translate({-1.2f, -0.3f, -0.2f});
    (*(_objects[2] = new PickableObject{3, _shader, Color3::fromHSV(105.0_degf, 0.9f, 0.9f), _plane, _scene, _drawables}))
        .rotate(254.0_degf, Vector3(1.0f).normalized())
        .scale(Vector3(0.45f))
        .translate({0.5f, 1.3f, 1.5f});
    (*(_objects[3] = new PickableObject{4, _shader, Color3::fromHSV(162.0_degf, 0.9f, 0.9f), _sphere, _scene, _drawables}))
        .translate({-0.2f, -1.7f, -2.7f});
    (*(_objects[4] = new PickableObject{5, _shader, Color3::fromHSV(210.0_degf, 0.9f, 0.9f), _sphere, _scene, _drawables}))
        .translate({0.7f, 0.6f, 2.2f})
        .scale(Vector3(0.75f));
    (*(_objects[5] = new PickableObject{6, _shader, Color3::fromHSV(280.0_degf, 0.9f, 0.9f), _cube, _scene, _drawables}))
        .rotate(-92.0_degf, Vector3(1.0f).normalized())
        .scale(Vector3(0.25f))
        .translate({-0.5f, -0.3f, 1.8f});

    /* Configure camera */
    _cameraObject = new Object3D{&_scene};
    _cameraObject->translate(Vector3::zAxis(8.0f));
    _camera = new SceneGraph::Camera3D{*_cameraObject};
    _camera->setAspectRatioPolicy(SceneGraph::AspectRatioPolicy::Extend)
        .setProjectionMatrix(Matrix4::perspectiveProjection(35.0_degf, 4.0f/3.0f, 0.001f, 100.0f))
        .setViewport(defaultFramebuffer.viewport().size());
}

void PickingExample::drawEvent() {
    /* Draw to custom framebuffer */
    _framebuffer.clear(FramebufferClear::Color|FramebufferClear::Depth)
        .bind();
    _camera->draw(_drawables);

    /* Blit color to window framebuffer */
    _framebuffer.mapForRead(Framebuffer::ColorAttachment{0});
    AbstractFramebuffer::blit(_framebuffer, defaultFramebuffer,
        {{}, _framebuffer.viewport().size()}, FramebufferBlit::Color);

    swapBuffers();
}

void PickingExample::mousePressEvent(MouseEvent& event) {
    if(event.button() != MouseEvent::Button::Left) return;

    _previousMousePosition = _mousePressPosition = event.position();
    event.setAccepted();
}

void PickingExample::mouseMoveEvent(MouseMoveEvent& event) {
    if(!(event.buttons() & MouseMoveEvent::Button::Left)) return;

    const Vector2 delta = 3.0f*
        Vector2{event.position() - _previousMousePosition}/
        Vector2{defaultFramebuffer.viewport().size()};

    (*_cameraObject)
        .rotate(Rad{-delta.y()}, _cameraObject->transformation().right().normalized())
        .rotateY(Rad{-delta.x()});

    _previousMousePosition = event.position();
    event.setAccepted();
    redraw();
}

void PickingExample::mouseReleaseEvent(MouseEvent& event) {
    if(event.button() != MouseEvent::Button::Left || _mousePressPosition != event.position()) return;

    /* Read object ID at given click position (framebuffer has Y up while windowing system Y down) */
    _framebuffer.mapForRead(Framebuffer::ColorAttachment{1});
    Image2D data = _framebuffer.read(
        Range2Di::fromSize({event.position().x(), _framebuffer.viewport().sizeY() - event.position().y() - 1}, {1, 1}),
        {PixelFormat::RedInteger, PixelType::UnsignedByte});

    /* Highlight object under mouse and deselect all other */
    for(auto* o: _objects) o->setSelected(false);
    UnsignedByte id = data.data<UnsignedByte>()[0];
    if(id > 0 && id < ObjectCount + 1)
        _objects[id - 1]->setSelected(true);

    event.setAccepted();
    redraw();
}

MAGNUM_APPLICATION_MAIN(PickingExample)
<commit_msg>picking: make the example working on Mesa.<commit_after>/*
    This file is part of Magnum.

    Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016
              Vladimír Vondruš <mosra@centrum.cz>

    Permission is hereby granted, free of charge, to any person obtaining a
    copy of this software and associated documentation files (the "Software"),
    to deal in the Software without restriction, including without limitation
    the rights to use, copy, modify, merge, publish, distribute, sublicense,
    and/or sell copies of the Software, and to permit persons to whom the
    Software is furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included
    in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
    THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
    DEALINGS IN THE SOFTWARE.
*/

#include <Corrade/Utility/Resource.h>
#include <Magnum/AbstractShaderProgram.h>
#include <Magnum/Buffer.h>
#include <Magnum/Context.h>
#include <Magnum/DefaultFramebuffer.h>
#include <Magnum/Framebuffer.h>
#include <Magnum/Image.h>
#include <Magnum/PixelFormat.h>
#include <Magnum/Renderbuffer.h>
#include <Magnum/RenderbufferFormat.h>
#include <Magnum/Renderer.h>
#include <Magnum/Shader.h>
#include <Magnum/Texture.h>
#include <Magnum/TextureFormat.h>
#include <Magnum/Version.h>
#include <Magnum/Math/Color.h>
#include <Magnum/MeshTools/CompressIndices.h>
#include <Magnum/MeshTools/Interleave.h>
#include <Magnum/Platform/Sdl2Application.h>
#include <Magnum/Primitives/Cube.h>
#include <Magnum/Primitives/Plane.h>
#include <Magnum/Primitives/UVSphere.h>
#include <Magnum/Trade/MeshData3D.h>
#include <Magnum/SceneGraph/Scene.h>
#include <Magnum/SceneGraph/MatrixTransformation3D.h>
#include <Magnum/SceneGraph/Camera.h>
#include <Magnum/SceneGraph/Drawable.h>

using namespace Magnum;

typedef SceneGraph::Object<SceneGraph::MatrixTransformation3D> Object3D;
typedef SceneGraph::Scene<SceneGraph::MatrixTransformation3D> Scene3D;

class PhongIdShader: public AbstractShaderProgram {
    public:
        typedef Attribute<0, Vector3> Position;
        typedef Attribute<1, Vector3> Normal;

        enum: UnsignedInt {
            ColorOutput = 0,
            ObjectIdOutput = 1
        };

        explicit PhongIdShader();

        PhongIdShader& setObjectId(Int id) {
            setUniform(_objectIdUniform, id);
            return *this;
        }

        PhongIdShader& setLightPosition(const Vector3& position) {
            setUniform(_lightPositionUniform, position);
            return *this;
        }

        PhongIdShader& setAmbientColor(const Color3& color) {
            setUniform(_ambientColorUniform, color);
            return *this;
        }

        PhongIdShader& setColor(const Color3& color) {
            setUniform(_colorUniform, color);
            return *this;
        }

        PhongIdShader& setTransformationMatrix(const Matrix4& matrix) {
            setUniform(_transformationMatrixUniform, matrix);
            return *this;
        }

        PhongIdShader& setNormalMatrix(const Matrix3x3& matrix) {
            setUniform(_normalMatrixUniform, matrix);
            return *this;
        }

        PhongIdShader& setProjectionMatrix(const Matrix4& matrix) {
            setUniform(_projectionMatrixUniform, matrix);
            return *this;
        }

    private:
        Int _objectIdUniform,
            _lightPositionUniform,
            _ambientColorUniform,
            _colorUniform,
            _transformationMatrixUniform,
            _normalMatrixUniform,
            _projectionMatrixUniform;
};

PhongIdShader::PhongIdShader() {
    Utility::Resource rs("picking-data");

    Shader vert{Version::GL330, Shader::Type::Vertex},
        frag{Version::GL330, Shader::Type::Fragment};
    vert.addSource(rs.get("PhongId.vert"));
    frag.addSource(rs.get("PhongId.frag"));
    CORRADE_INTERNAL_ASSERT(Shader::compile({vert, frag}));
    attachShaders({vert, frag});
    CORRADE_INTERNAL_ASSERT(link());

    _objectIdUniform = uniformLocation("objectId");
    _lightPositionUniform = uniformLocation("light");
    _ambientColorUniform = uniformLocation("ambientColor");
    _colorUniform = uniformLocation("color");
    _transformationMatrixUniform = uniformLocation("transformationMatrix");
    _projectionMatrixUniform = uniformLocation("projectionMatrix");
    _normalMatrixUniform = uniformLocation("normalMatrix");
}

class PickableObject: public Object3D, SceneGraph::Drawable3D {
    public:
        explicit PickableObject(UnsignedByte id, PhongIdShader& shader, const Color3& color, Mesh& mesh, Object3D& parent, SceneGraph::DrawableGroup3D& drawables): Object3D{&parent}, SceneGraph::Drawable3D{*this, &drawables}, _id{id}, _selected{false}, _shader(shader), _color{color}, _mesh(mesh) {}

        void setSelected(bool selected) { _selected = selected; }

    private:
        virtual void draw(const Matrix4& transformationMatrix, SceneGraph::Camera3D& camera) {
            _shader.setTransformationMatrix(transformationMatrix)
                .setNormalMatrix(transformationMatrix.rotationScaling())
                .setProjectionMatrix(camera.projectionMatrix())
                .setAmbientColor(_selected ? _color*0.3f : Color3{})
                .setColor(_color*(_selected ? 2.0f : 1.0f))
                /* relative to the camera */
                .setLightPosition({13.0f, 2.0f, 5.0f})
                .setObjectId(_id);
            _mesh.draw(_shader);
        }

        UnsignedByte _id;
        bool _selected;
        PhongIdShader& _shader;
        Color3 _color;
        Mesh& _mesh;
};

class PickingExample: public Platform::Application {
    public:
        explicit PickingExample(const Arguments& arguments);

    private:
        void drawEvent() override;
        void mousePressEvent(MouseEvent& event) override;
        void mouseMoveEvent(MouseMoveEvent& event) override;
        void mouseReleaseEvent(MouseEvent& event) override;

        Scene3D _scene;
        Object3D* _cameraObject;
        SceneGraph::Camera3D* _camera;
        SceneGraph::DrawableGroup3D _drawables;

        PhongIdShader _shader;
        Buffer _cubeVertices, _cubeIndices,
            _sphereVertices, _sphereIndices,
            _planeVertices;
        Mesh _cube, _plane, _sphere;

        enum { ObjectCount = 6 };
        PickableObject* _objects[ObjectCount];

        Framebuffer _framebuffer;
        Renderbuffer _color, _objectId, _depth;

        Vector2i _previousMousePosition, _mousePressPosition;
};

PickingExample::PickingExample(const Arguments& arguments): Platform::Application{arguments, Configuration{}.setTitle("Magnum object picking example")}, _framebuffer{defaultFramebuffer.viewport()} {
    MAGNUM_ASSERT_VERSION_SUPPORTED(Version::GL330);

    /* Global renderer configuration */
    Renderer::enable(Renderer::Feature::DepthTest);

    /* Configure framebuffer (using R8UI for object ID which means 255 objects max) */
    _color.setStorage(RenderbufferFormat::RGBA8, defaultFramebuffer.viewport().size());
    _objectId.setStorage(RenderbufferFormat::R8UI, defaultFramebuffer.viewport().size());
    _depth.setStorage(RenderbufferFormat::DepthComponent24, defaultFramebuffer.viewport().size());
    _framebuffer.attachRenderbuffer(Framebuffer::ColorAttachment{0}, _color)
               .attachRenderbuffer(Framebuffer::ColorAttachment{1}, _objectId)
               .attachRenderbuffer(Framebuffer::BufferAttachment::Depth, _depth)
               .mapForDraw({{PhongIdShader::ColorOutput, Framebuffer::ColorAttachment{0}},
                            {PhongIdShader::ObjectIdOutput, Framebuffer::ColorAttachment{1}}});
    CORRADE_INTERNAL_ASSERT(_framebuffer.checkStatus(FramebufferTarget::Draw) == Framebuffer::Status::Complete);

    /* Set up meshes */
    {
        Trade::MeshData3D data = Primitives::Cube::solid();
        _cubeVertices.setData(MeshTools::interleave(data.positions(0), data.normals(0)), BufferUsage::StaticDraw);
        _cubeIndices.setData(MeshTools::compressIndicesAs<UnsignedShort>(data.indices()), BufferUsage::StaticDraw);
        _cube.setCount(data.indices().size())
            .setPrimitive(data.primitive())
            .addVertexBuffer(_cubeVertices, 0, PhongIdShader::Position{}, PhongIdShader::Normal{})
            .setIndexBuffer(_cubeIndices, 0, Mesh::IndexType::UnsignedShort);
    } {
        Trade::MeshData3D data = Primitives::UVSphere::solid(16, 32);
        _sphereVertices.setData(MeshTools::interleave(data.positions(0), data.normals(0)), BufferUsage::StaticDraw);
        _sphereIndices.setData(MeshTools::compressIndicesAs<UnsignedShort>(data.indices()), BufferUsage::StaticDraw);
        _sphere.setCount(data.indices().size())
            .setPrimitive(data.primitive())
            .addVertexBuffer(_sphereVertices, 0, PhongIdShader::Position{}, PhongIdShader::Normal{})
            .setIndexBuffer(_sphereIndices, 0, Mesh::IndexType::UnsignedShort);
    } {
        Trade::MeshData3D data = Primitives::Plane::solid();
        _planeVertices.setData(MeshTools::interleave(data.positions(0), data.normals(0)), BufferUsage::StaticDraw);
        _plane.setCount(data.positions(0).size())
            .setPrimitive(data.primitive())
            .addVertexBuffer(_planeVertices, 0, PhongIdShader::Position{}, PhongIdShader::Normal{});
    }

    /* Set up objects */
    (*(_objects[0] = new PickableObject{1, _shader, Color3::fromHSV(25.0_degf, 0.9f, 0.9f), _cube, _scene, _drawables}))
        .rotate(34.0_degf, Vector3(1.0f).normalized())
        .translate({1.0f, 0.3f, -1.2f});
    (*(_objects[1] = new PickableObject{2, _shader, Color3::fromHSV(54.0_degf, 0.9f, 0.9f), _sphere, _scene, _drawables}))
        .translate({-1.2f, -0.3f, -0.2f});
    (*(_objects[2] = new PickableObject{3, _shader, Color3::fromHSV(105.0_degf, 0.9f, 0.9f), _plane, _scene, _drawables}))
        .rotate(254.0_degf, Vector3(1.0f).normalized())
        .scale(Vector3(0.45f))
        .translate({0.5f, 1.3f, 1.5f});
    (*(_objects[3] = new PickableObject{4, _shader, Color3::fromHSV(162.0_degf, 0.9f, 0.9f), _sphere, _scene, _drawables}))
        .translate({-0.2f, -1.7f, -2.7f});
    (*(_objects[4] = new PickableObject{5, _shader, Color3::fromHSV(210.0_degf, 0.9f, 0.9f), _sphere, _scene, _drawables}))
        .translate({0.7f, 0.6f, 2.2f})
        .scale(Vector3(0.75f));
    (*(_objects[5] = new PickableObject{6, _shader, Color3::fromHSV(280.0_degf, 0.9f, 0.9f), _cube, _scene, _drawables}))
        .rotate(-92.0_degf, Vector3(1.0f).normalized())
        .scale(Vector3(0.25f))
        .translate({-0.5f, -0.3f, 1.8f});

    /* Configure camera */
    _cameraObject = new Object3D{&_scene};
    _cameraObject->translate(Vector3::zAxis(8.0f));
    _camera = new SceneGraph::Camera3D{*_cameraObject};
    _camera->setAspectRatioPolicy(SceneGraph::AspectRatioPolicy::Extend)
        .setProjectionMatrix(Matrix4::perspectiveProjection(35.0_degf, 4.0f/3.0f, 0.001f, 100.0f))
        .setViewport(defaultFramebuffer.viewport().size());
}

void PickingExample::drawEvent() {
    /* Draw to custom framebuffer */
    _framebuffer.clear(FramebufferClear::Color|FramebufferClear::Depth)
        .bind();
    _camera->draw(_drawables);

    /* Bind the main buffer back */
    defaultFramebuffer.bind();

    /* Blit color to window framebuffer */
    _framebuffer.mapForRead(Framebuffer::ColorAttachment{0});
    AbstractFramebuffer::blit(_framebuffer, defaultFramebuffer,
        {{}, _framebuffer.viewport().size()}, FramebufferBlit::Color);

    swapBuffers();
}

void PickingExample::mousePressEvent(MouseEvent& event) {
    if(event.button() != MouseEvent::Button::Left) return;

    _previousMousePosition = _mousePressPosition = event.position();
    event.setAccepted();
}

void PickingExample::mouseMoveEvent(MouseMoveEvent& event) {
    if(!(event.buttons() & MouseMoveEvent::Button::Left)) return;

    const Vector2 delta = 3.0f*
        Vector2{event.position() - _previousMousePosition}/
        Vector2{defaultFramebuffer.viewport().size()};

    (*_cameraObject)
        .rotate(Rad{-delta.y()}, _cameraObject->transformation().right().normalized())
        .rotateY(Rad{-delta.x()});

    _previousMousePosition = event.position();
    event.setAccepted();
    redraw();
}

void PickingExample::mouseReleaseEvent(MouseEvent& event) {
    if(event.button() != MouseEvent::Button::Left || _mousePressPosition != event.position()) return;

    /* Read object ID at given click position (framebuffer has Y up while windowing system Y down) */
    _framebuffer.mapForRead(Framebuffer::ColorAttachment{1});
    Image2D data = _framebuffer.read(
        Range2Di::fromSize({event.position().x(), _framebuffer.viewport().sizeY() - event.position().y() - 1}, {1, 1}),
        {PixelFormat::RedInteger, PixelType::UnsignedByte});

    /* Highlight object under mouse and deselect all other */
    for(auto* o: _objects) o->setSelected(false);
    UnsignedByte id = data.data<UnsignedByte>()[0];
    if(id > 0 && id < ObjectCount + 1)
        _objects[id - 1]->setSelected(true);

    event.setAccepted();
    redraw();
}

MAGNUM_APPLICATION_MAIN(PickingExample)
<|endoftext|>


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

#include <planning_request_adapter/planning_request_adapter.h>
#include <planning_models/conversions.h>
#include <boost/bind.hpp>

namespace planning_request_adapter
{
static bool callPlannerInterfaceSolve(const planning_interface::Planner *planner,
                                      const planning_scene::PlanningSceneConstPtr& planning_scene,
                                      const moveit_msgs::GetMotionPlan::Request &req,
                                      moveit_msgs::GetMotionPlan::Response &res)
{
  return planner->solve(planning_scene, req, res);
}
}

bool planning_request_adapter::PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerPtr &planner,
                                                                    const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                    const moveit_msgs::GetMotionPlan::Request &req,
                                                                    moveit_msgs::GetMotionPlan::Response &res,
                                                                    std::vector<std::size_t> &added_path_index) const
{
  return adaptAndPlan(boost::bind(&callPlannerInterfaceSolve, planner.get(), _1, _2, _3), planning_scene, req, res, added_path_index);
}

bool planning_request_adapter::PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerPtr &planner,
                                                                    const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                    const moveit_msgs::GetMotionPlan::Request &req,
                                                                    moveit_msgs::GetMotionPlan::Response &res) const
{
  std::vector<std::size_t> dummy;
  return adaptAndPlan(planner, planning_scene, req, res, dummy);
}

namespace planning_request_adapter
{

// boost bind is not happy with overloading, so we add intermediate function objects

static bool callAdapter1(const PlanningRequestAdapter *adapter,
                         const planning_interface::PlannerPtr &planner,
                         const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::GetMotionPlan::Request &req,
                         moveit_msgs::GetMotionPlan::Response &res,
                         std::vector<std::size_t> &added_path_index)
{
  return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
}

static bool callAdapter2(const PlanningRequestAdapter *adapter,
                         const PlannerFn &planner,
                         const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::GetMotionPlan::Request &req,
                         moveit_msgs::GetMotionPlan::Response &res,
                         std::vector<std::size_t> &added_path_index)
{
  return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
}

}

bool planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerPtr &planner,
                                                                         const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                         const moveit_msgs::GetMotionPlan::Request &req,
                                                                         moveit_msgs::GetMotionPlan::Response &res) const
{
  std::vector<std::size_t> dummy;
  return adaptAndPlan(planner, planning_scene, req, res, dummy);
}

bool planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerPtr &planner,
                                                                         const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                         const moveit_msgs::GetMotionPlan::Request &req,
                                                                         moveit_msgs::GetMotionPlan::Response &res,
                                                                         std::vector<std::size_t> &added_path_index) const
{
  // if there are no adapters, run the planner directly
  if (adapters_.empty())
  {
    added_path_index.clear();
    return planner->solve(planning_scene, req, res);
  }
  else
  {
    // the index values added by each adapter
    std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());

    // if there are adapters, construct a function pointer for each, in order,
    // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order.
    PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3, boost::ref(added_path_index_each.back()));
    for (int i = adapters_.size() - 2 ; i >= 0 ; --i)
      fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i]));
    bool result = fn(planning_scene, req, res);
    added_path_index.clear();

    // merge the index values from each adapter
    for (std::size_t i = 0 ; i < added_path_index_each.size() ; ++i)
      for (std::size_t j = 0 ; j < added_path_index_each[i].size() ; ++j)
      {
        for (std::size_t k = 0 ; k < added_path_index.size() ; ++k)
          if (added_path_index_each[i][j] <= added_path_index[k])
            added_path_index[k]++;
        added_path_index.push_back(added_path_index_each[i][j]);
      }

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

#include <planning_request_adapter/planning_request_adapter.h>
#include <planning_models/conversions.h>
#include <boost/bind.hpp>
#include <algorithm>

namespace planning_request_adapter
{
static bool callPlannerInterfaceSolve(const planning_interface::Planner *planner,
                                      const planning_scene::PlanningSceneConstPtr& planning_scene,
                                      const moveit_msgs::GetMotionPlan::Request &req,
                                      moveit_msgs::GetMotionPlan::Response &res)
{
  return planner->solve(planning_scene, req, res);
}
}

bool planning_request_adapter::PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerPtr &planner,
                                                                    const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                    const moveit_msgs::GetMotionPlan::Request &req,
                                                                    moveit_msgs::GetMotionPlan::Response &res,
                                                                    std::vector<std::size_t> &added_path_index) const
{
  return adaptAndPlan(boost::bind(&callPlannerInterfaceSolve, planner.get(), _1, _2, _3), planning_scene, req, res, added_path_index);
}

bool planning_request_adapter::PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerPtr &planner,
                                                                    const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                    const moveit_msgs::GetMotionPlan::Request &req,
                                                                    moveit_msgs::GetMotionPlan::Response &res) const
{
  std::vector<std::size_t> dummy;
  return adaptAndPlan(planner, planning_scene, req, res, dummy);
}

namespace planning_request_adapter
{

// boost bind is not happy with overloading, so we add intermediate function objects

static bool callAdapter1(const PlanningRequestAdapter *adapter,
                         const planning_interface::PlannerPtr &planner,
                         const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::GetMotionPlan::Request &req,
                         moveit_msgs::GetMotionPlan::Response &res,
                         std::vector<std::size_t> &added_path_index)
{
  return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
}

static bool callAdapter2(const PlanningRequestAdapter *adapter,
                         const PlannerFn &planner,
                         const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::GetMotionPlan::Request &req,
                         moveit_msgs::GetMotionPlan::Response &res,
                         std::vector<std::size_t> &added_path_index)
{
  return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
}

}

bool planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerPtr &planner,
                                                                         const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                         const moveit_msgs::GetMotionPlan::Request &req,
                                                                         moveit_msgs::GetMotionPlan::Response &res) const
{
  std::vector<std::size_t> dummy;
  return adaptAndPlan(planner, planning_scene, req, res, dummy);
}

bool planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerPtr &planner,
                                                                         const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                         const moveit_msgs::GetMotionPlan::Request &req,
                                                                         moveit_msgs::GetMotionPlan::Response &res,
                                                                         std::vector<std::size_t> &added_path_index) const
{
  // if there are no adapters, run the planner directly
  if (adapters_.empty())
  {
    added_path_index.clear();
    return planner->solve(planning_scene, req, res);
  }
  else
  {
    // the index values added by each adapter
    std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());

    // if there are adapters, construct a function pointer for each, in order,
    // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order.
    PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3, boost::ref(added_path_index_each.back()));
    for (int i = adapters_.size() - 2 ; i >= 0 ; --i)
      fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i]));
    bool result = fn(planning_scene, req, res);
    added_path_index.clear();

    // merge the index values from each adapter
    for (std::size_t i = 0 ; i < added_path_index_each.size() ; ++i)
      for (std::size_t j = 0 ; j < added_path_index_each[i].size() ; ++j)
      {
        for (std::size_t k = 0 ; k < added_path_index.size() ; ++k)
          if (added_path_index_each[i][j] <= added_path_index[k])
            added_path_index[k]++;
        added_path_index.push_back(added_path_index_each[i][j]);
      }
    std::sort(added_path_index.begin(), added_path_index.end());
    return result;
  }
}
<|endoftext|>


<commit_before>/*
 * =====================================================================================
 *
 *       Filename:  1.cpp
 *
 *    Description:  Solution to the first problem of the `all' section.
 *
 *        Version:  1.0
 *        Created:  27.01.2015 19,21,01
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Radoslav Georgiev (sid), rgeorgiev583@gmail.com
 *        Company:  Faculty of Mathematics and Informatics at Sofia University
 *
 * =====================================================================================
 */

#include <iostream>
using namespace std;

int fact(int n)
{
    int P = 1;

    for (int i = 2; i <= n; i++)
        P *= i;

    return P;
}

bool areEqual(int n, int m)
{
    double S = 0;

    for (int i = 1; i <= n; i++)
    {
        int P = 1;

        for (int j = i; j <= i + m; j++)
            P *= j;

        S += 1 / (double) P;
    }

    int P = 1;

    for (int i = n + 1; i <= n + m; i++)
        P *= i;

    return S == 1 / (double) m * (1 / (double) fact(m) - 1 / (double) P);
}

bool areEqualForAll(int a, int b)
{
    for (int i = 1; i <= a; i++)
        for (int j = 1; j <= b; j++)
            if (!areEqual(i, j))
                return false;

    return true;
}

int main()
{
    int n, m;

    cin >> n >> m;
    cout << (areEqualForAll(n, m) ? "Yes" : "No");

    return 0;
}

<commit_msg>changed doubles comparison method to is-absolute-difference-less-than-epsilon<commit_after>/*
 * =====================================================================================
 *
 *       Filename:  1.cpp
 *
 *    Description:  Solution to the first problem of the `all' section.
 *
 *        Version:  1.0
 *        Created:  27.01.2015 19,21,01
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Radoslav Georgiev (sid), rgeorgiev583@gmail.com
 *        Company:  Faculty of Mathematics and Informatics at Sofia University
 *
 * =====================================================================================
 */

#include <iostream>
using namespace std;

const double EPS = 0.001;

int fact(int n)
{
    int P = 1;

    for (int i = 2; i <= n; i++)
        P *= i;

    return P;
}

bool eq(double a, double b)
{
    return a - b > -EPS && a - b < EPS;
}

bool areEqual(int n, int m)
{
    double S = 0;

    for (int i = 1; i <= n; i++)
    {
        int P = 1;

        for (int j = i; j <= i + m; j++)
            P *= j;

        S += 1 / (double) P;
    }

    int P = 1;

    for (int i = n + 1; i <= n + m; i++)
        P *= i;

    return eq(S, 1 / (double) m * (1 / (double) fact(m) - 1 / (double) P));
}

bool areEqualForAll(int a, int b)
{
    for (int i = 1; i <= a; i++)
        for (int j = 1; j <= b; j++)
            if (!areEqual(i, j))
                return false;

    return true;
}

int main()
{
    int n, m;

    cin >> n >> m;
    cout << (areEqualForAll(n, m) ? "Yes" : "No");

    return 0;
}

<|endoftext|>