<commit_before>/*
 *  Copyright (C) 2010 - 2015 Leonid Kostrykin
 *
 *  Chair of Medical Engineering (mediTEC)
 *  RWTH Aachen University
 *  Pauwelsstr. 20
 *  52074 Aachen
 *  Germany
 *
 */

#include <Carna/qt/Display.h>
#include <Carna/base/FrameRenderer.h>
#include <Carna/base/SpatialMovement.h>
#include <Carna/base/GLContext.h>
#include <Carna/base/Camera.h>
#include <Carna/base/Log.h>
#include <Carna/base/ProjectionControl.h>
#include <Carna/base/CameraControl.h>
#include <Carna/presets/MeshColorCodingStage.h>
#include <QGLContext>
#include <QMouseEvent>
#include <QWheelEvent>
#include <set>

namespace Carna
{

namespace qt
{

// ----------------------------------------------------------------------------------
// Display :: Details
// ----------------------------------------------------------------------------------

struct Display::Details
{
    Details( const std::function< void( base::FrameRenderer& ) >& );
    const std::function< void( base::FrameRenderer& ) > setupRenderer;

    static std::set< const Display* > sharingDisplays;
    static const Display* pickSharingDisplay();

    typedef base::QGLContextAdapter< QGLContext > GLContext;

    std::unique_ptr< GLContext > glc;
    std::unique_ptr< base::FrameRenderer > renderer;
    bool glInitializationFinished;

    ViewportMode vpMode;

    base::Camera* cam;
    base::Node* root;
    std::unique_ptr< base::Aggregation< base::CameraControl > > camControl;
    std::unique_ptr< base::Aggregation< base::ProjectionControl > > projControl;

    bool mouseInteraction;
    QPoint mousepos;
    float radiansPerPixel;
    float axialMovementSpeed;

    presets::MeshColorCodingStage* mccs;
    std::unique_ptr< base::SpatialMovement > spatialMovement;

    void updateProjection( Display& );
    bool fitSquare() const;
};

std::set< const Display* > Display::Details::sharingDisplays = std::set< const Display* >();

Display::Details::Details( const std::function< void( base::FrameRenderer& ) >& setupRenderer )
    : setupRenderer( setupRenderer )
    , glInitializationFinished( false )
    , vpMode( fitAuto )
    , cam( nullptr )
    , root( nullptr )
    , radiansPerPixel( DEFAULT_ROTATION_SPEED )
    , axialMovementSpeed( DEFAULT_AXIAL_MOVEMENT_SPEED )
    , mccs( nullptr )
{
}

const Display* Display::Details::pickSharingDisplay()
{
    if( sharingDisplays.empty() )
    {
        return nullptr;
    }
    else
    {
        return *sharingDisplays.begin();
    }
}

void Display::Details::updateProjection( Display& display )
{
    if( display.hasCamera() && display.hasProjectionControl() && display.width() > 0 && display.height() > 0 )
    {
        const unsigned int width  = static_cast< unsigned int >( display. width() );
        const unsigned int height = static_cast< unsigned int >( display.height() );

        /* Update projection parameters.
         */
        if( fitSquare() )
        {
            const unsigned int vpSize = std::min( width, height );
            display.projectionControl().setViewportWidth ( vpSize );
            display.projectionControl().setViewportHeight( vpSize );
        }
        else
        {
            display.projectionControl().setViewportWidth (  width );
            display.projectionControl().setViewportHeight( height );
        }

        /* Fetch new projection matrix.
         */
        base::math::Matrix4f projection;
        display.projectionControl().updateProjection( projection );
        display.camera().setProjection( projection );

        /* Redraw if required.
         */
        if( glInitializationFinished )
        {
            display.updateGL();
        }
    }
}

bool Display::Details::fitSquare() const
{
    switch( vpMode )
    {

    case Display::fitSquare:
        return true;

    case Display::fitFrame:
        return false;

    case Display::fitAuto:
        return projControl.get() != nullptr && projControl->get() != nullptr;

    default:
        CARNA_FAIL( "Unknown ViewportMode value." );

    }
}

// ----------------------------------------------------------------------------------
// Display
// ----------------------------------------------------------------------------------

const float Display::DEFAULT_ROTATION_SPEED       = 1e-2f;
const float Display::DEFAULT_AXIAL_MOVEMENT_SPEED = 1e+0f;

Display::Display( const std::function< void( base::FrameRenderer& ) >& setupRenderer, QWidget* parent )
    : QGLWidget( parent, Details::pickSharingDisplay() )
    , pimpl( new Details( setupRenderer ) )
{
    Details::sharingDisplays.insert( this );
}

Display::~Display()
{
    Details::sharingDisplays.erase( this );
    if( pimpl->glc.get() != nullptr )
    {
        pimpl->glc->makeActive();
    }
}

void Display::setViewportMode( ViewportMode vpMode )
{
    if( vpMode != pimpl->vpMode )
    {
        pimpl->vpMode = vpMode;
        if( pimpl->renderer.get() != nullptr )
        {
            resizeGL( width(), height() );
        }
    }
}

void Display::setCamera( base::Camera& cam )
{
    pimpl->cam = &cam;
    pimpl->updateProjection( *this );
    if( hasCameraControl() )
    {
        cameraControl().setCamera( cam );
    }

    /* We will search the root node the first time we render the frame.
     */
    pimpl->root = nullptr;
}

bool Display::hasCamera() const
{
    return pimpl->cam != nullptr;
}

base::Camera& Display::camera()
{
    return *pimpl->cam;
}

const base::Camera& Display::camera() const
{
    return *pimpl->cam;
}

void Display::initializeGL()
{
    CARNA_ASSERT( pimpl->glc.get() == nullptr );
    pimpl->glc.reset( new Details::GLContext() );
}

void Display::resizeGL( int w, int h )
{
    CARNA_ASSERT( pimpl->glc.get() != nullptr );
    const unsigned int width  = static_cast< unsigned int >( w );
    const unsigned int height = static_cast< unsigned int >( h );
    if( pimpl->renderer == nullptr )
    {
        pimpl->renderer.reset( new base::FrameRenderer( *pimpl->glc, width, height, pimpl->fitSquare() ) );
        pimpl->setupRenderer( *pimpl->renderer );
        pimpl->mccs = pimpl->renderer->findStage< presets::MeshColorCodingStage >().get();
        pimpl->updateProjection( *this );
        pimpl->glInitializationFinished = true;
    }
    else
    {
        pimpl->renderer->reshape( width, height, pimpl->fitSquare() );
        pimpl->updateProjection( *this );
    }
}

void Display::paintGL()
{
    CARNA_ASSERT( pimpl->renderer.get() != nullptr );

    if( pimpl->cam == nullptr )
    {
        base::Log::instance().record( base::Log::debug, "Display has no camera but should render." );
    }
    else
    {
        if( pimpl->root == nullptr )
        {
            pimpl->root = &pimpl->cam->findRoot();
        }
        pimpl->renderer->render( *pimpl->cam, *pimpl->root );
    }
}

void Display::setRotationSpeed( float radiansPerPixel )
{
    pimpl->radiansPerPixel = radiansPerPixel;
}

void Display::setAxialMovementSpeed( float axialMovementSpeed )
{
    pimpl->axialMovementSpeed = axialMovementSpeed;
}

void Display::mousePressEvent( QMouseEvent* ev )
{
    /* First, try to pick object at clicked location.
     */
    const base::Geometry* picked = nullptr;
    if( pimpl->mccs != nullptr )
    {
        picked = pimpl->mccs->pick( ev->x(), ev->y() ).get();
    }

    /* Initiate camera rotation if nothing was picked.
     */
    if( picked == nullptr )
    {
        pimpl->mousepos = ev->pos();
        pimpl->mouseInteraction = true;
        ev->accept();
    }
    else
    if( pimpl->renderer != nullptr && hasCamera() && hasCameraControl() )
    {
        /* Picking was successful! Initiate spatial movement.
         */
        base::Geometry& pickedGeometry = const_cast< base::Geometry& >( *picked );
        pimpl->spatialMovement.reset
            ( new base::SpatialMovement( pickedGeometry, ev->x(), ev->y(), pimpl->renderer->viewport(), *pimpl->cam ) );
        pimpl->mouseInteraction = true;
        ev->accept();
    }
}

void Display::mouseMoveEvent( QMouseEvent* ev )
{
    if( pimpl->mouseInteraction )
    {
        if( pimpl->spatialMovement.get() == nullptr && hasCamera() && hasCameraControl() )
        {
            /* Camera rotation is going on.
             */
            const int dx = ( ev->x() - pimpl->mousepos.x() );
            const int dy = ( ev->y() - pimpl->mousepos.y() );
            pimpl->mousepos = ev->pos();

            if( dx != 0 || dy != 0 )
            {
                cameraControl().rotateHorizontally( dx * pimpl->radiansPerPixel );
                cameraControl().rotateVertically  ( dx * pimpl->radiansPerPixel );
                updateGL();
                ev->accept();
            }
        }
        else
        if( pimpl->spatialMovement.get() != nullptr )
        {
            /* Spatial movement is going on. Update it.
             */
            if( pimpl->spatialMovement->update( ev->x(), ev->y() ) )
            {
                updateGL();
                ev->accept();
            }
        }
    }
}

void Display::mouseReleaseEvent( QMouseEvent* ev )
{
    pimpl->mouseInteraction = false;
    pimpl->spatialMovement.reset();
    ev->accept();
}

void Display::wheelEvent( QWheelEvent* ev )
{
    if( hasCamera() && hasCameraControl() )
    {
        cameraControl().moveAxially( ev->delta() * pimpl->axialMovementSpeed );
        ev->accept();
    }
}

void Display::setCameraControl( base::Aggregation< base::CameraControl >* camControl )
{
    pimpl->camControl.reset( camControl );
    if( hasCamera() )
    {
        cameraControl().setCamera( *pimpl->cam );
    }
}

bool Display::hasCameraControl() const
{
    return pimpl->camControl.get() != nullptr && pimpl->camControl->get() != nullptr;
}

base::CameraControl& Display::cameraControl()
{
    CARNA_ASSERT( hasCameraControl() );
    return **pimpl->camControl;
}

const base::CameraControl& Display::cameraControl() const
{
    CARNA_ASSERT( hasCameraControl() );
    return **pimpl->camControl;
}

void Display::setProjectionControl( base::Aggregation< base::ProjectionControl >* projControl )
{
    pimpl->projControl.reset( projControl );
    pimpl->updateProjection( *this );
}

bool Display::hasProjectionControl() const
{
    return pimpl->projControl.get() != nullptr && pimpl->projControl->get() != nullptr;
}

base::ProjectionControl& Display::projectionControl()
{
    CARNA_ASSERT( hasProjectionControl() );
    return **pimpl->projControl;
}

const base::ProjectionControl& Display::projectionControl() const
{
    CARNA_ASSERT( hasProjectionControl() );
    return **pimpl->projControl;
}

bool Display::hasRenderer() const
{
    return pimpl->renderer.get() != nullptr;
}

base::FrameRenderer& Display::renderer()
{
    CARNA_ASSERT( hasRenderer() );
    return *pimpl->renderer;
}

const base::FrameRenderer& Display::renderer() const
{
    CARNA_ASSERT( hasRenderer() );
    return *pimpl->renderer;
}

}  // namespace Carna :: qt

}  // namespace Carna

<commit_msg>fixed bug in Display<commit_after>/*
 *  Copyright (C) 2010 - 2015 Leonid Kostrykin
 *
 *  Chair of Medical Engineering (mediTEC)
 *  RWTH Aachen University
 *  Pauwelsstr. 20
 *  52074 Aachen
 *  Germany
 *
 */

#include <Carna/qt/Display.h>
#include <Carna/base/FrameRenderer.h>
#include <Carna/base/SpatialMovement.h>
#include <Carna/base/GLContext.h>
#include <Carna/base/Camera.h>
#include <Carna/base/Log.h>
#include <Carna/base/ProjectionControl.h>
#include <Carna/base/CameraControl.h>
#include <Carna/presets/MeshColorCodingStage.h>
#include <QGLContext>
#include <QMouseEvent>
#include <QWheelEvent>
#include <set>

namespace Carna
{

namespace qt
{

// ----------------------------------------------------------------------------------
// Display :: Details
// ----------------------------------------------------------------------------------

struct Display::Details
{
    Details( const std::function< void( base::FrameRenderer& ) >& );
    const std::function< void( base::FrameRenderer& ) > setupRenderer;

    static std::set< const Display* > sharingDisplays;
    static const Display* pickSharingDisplay();

    typedef base::QGLContextAdapter< QGLContext > GLContext;

    std::unique_ptr< GLContext > glc;
    std::unique_ptr< base::FrameRenderer > renderer;
    bool glInitializationFinished;

    ViewportMode vpMode;

    base::Camera* cam;
    base::Node* root;
    std::unique_ptr< base::Aggregation< base::CameraControl > > camControl;
    std::unique_ptr< base::Aggregation< base::ProjectionControl > > projControl;

    bool mouseInteraction;
    QPoint mousepos;
    float radiansPerPixel;
    float axialMovementSpeed;

    presets::MeshColorCodingStage* mccs;
    std::unique_ptr< base::SpatialMovement > spatialMovement;

    void updateProjection( Display& );
    bool fitSquare() const;
};

std::set< const Display* > Display::Details::sharingDisplays = std::set< const Display* >();

Display::Details::Details( const std::function< void( base::FrameRenderer& ) >& setupRenderer )
    : setupRenderer( setupRenderer )
    , glInitializationFinished( false )
    , vpMode( fitAuto )
    , cam( nullptr )
    , root( nullptr )
    , radiansPerPixel( DEFAULT_ROTATION_SPEED )
    , axialMovementSpeed( DEFAULT_AXIAL_MOVEMENT_SPEED )
    , mccs( nullptr )
{
}

const Display* Display::Details::pickSharingDisplay()
{
    if( sharingDisplays.empty() )
    {
        return nullptr;
    }
    else
    {
        return *sharingDisplays.begin();
    }
}

void Display::Details::updateProjection( Display& display )
{
    if( display.hasCamera() && display.hasProjectionControl() && display.width() > 0 && display.height() > 0 )
    {
        const unsigned int width  = static_cast< unsigned int >( display. width() );
        const unsigned int height = static_cast< unsigned int >( display.height() );

        /* Update projection parameters.
         */
        if( fitSquare() )
        {
            const unsigned int vpSize = std::min( width, height );
            display.projectionControl().setViewportWidth ( vpSize );
            display.projectionControl().setViewportHeight( vpSize );
        }
        else
        {
            display.projectionControl().setViewportWidth (  width );
            display.projectionControl().setViewportHeight( height );
        }

        /* Fetch new projection matrix.
         */
        base::math::Matrix4f projection;
        display.projectionControl().updateProjection( projection );
        display.camera().setProjection( projection );

        /* Redraw if required.
         */
        if( glInitializationFinished )
        {
            display.updateGL();
        }
    }
}

bool Display::Details::fitSquare() const
{
    switch( vpMode )
    {

    case Display::fitSquare:
        return true;

    case Display::fitFrame:
        return false;

    case Display::fitAuto:
        return projControl.get() == nullptr || projControl->get() == nullptr;

    default:
        CARNA_FAIL( "Unknown ViewportMode value." );

    }
}

// ----------------------------------------------------------------------------------
// Display
// ----------------------------------------------------------------------------------

const float Display::DEFAULT_ROTATION_SPEED       = 1e-2f;
const float Display::DEFAULT_AXIAL_MOVEMENT_SPEED = 1e+0f;

Display::Display( const std::function< void( base::FrameRenderer& ) >& setupRenderer, QWidget* parent )
    : QGLWidget( parent, Details::pickSharingDisplay() )
    , pimpl( new Details( setupRenderer ) )
{
    Details::sharingDisplays.insert( this );
}

Display::~Display()
{
    Details::sharingDisplays.erase( this );
    if( pimpl->glc.get() != nullptr )
    {
        pimpl->glc->makeActive();
    }
}

void Display::setViewportMode( ViewportMode vpMode )
{
    if( vpMode != pimpl->vpMode )
    {
        pimpl->vpMode = vpMode;
        if( pimpl->renderer.get() != nullptr )
        {
            resizeGL( width(), height() );
        }
    }
}

void Display::setCamera( base::Camera& cam )
{
    pimpl->cam = &cam;
    pimpl->updateProjection( *this );
    if( hasCameraControl() )
    {
        cameraControl().setCamera( cam );
    }

    /* We will search the root node the first time we render the frame.
     */
    pimpl->root = nullptr;
}

bool Display::hasCamera() const
{
    return pimpl->cam != nullptr;
}

base::Camera& Display::camera()
{
    return *pimpl->cam;
}

const base::Camera& Display::camera() const
{
    return *pimpl->cam;
}

void Display::initializeGL()
{
    CARNA_ASSERT( pimpl->glc.get() == nullptr );
    pimpl->glc.reset( new Details::GLContext() );
}

void Display::resizeGL( int w, int h )
{
    CARNA_ASSERT( pimpl->glc.get() != nullptr );
    const unsigned int width  = static_cast< unsigned int >( w );
    const unsigned int height = static_cast< unsigned int >( h );
    if( pimpl->renderer == nullptr )
    {
        pimpl->renderer.reset( new base::FrameRenderer( *pimpl->glc, width, height, pimpl->fitSquare() ) );
        pimpl->setupRenderer( *pimpl->renderer );
        pimpl->mccs = pimpl->renderer->findStage< presets::MeshColorCodingStage >().get();
        pimpl->updateProjection( *this );
        pimpl->glInitializationFinished = true;
    }
    else
    {
        pimpl->renderer->reshape( width, height, pimpl->fitSquare() );
        pimpl->updateProjection( *this );
    }
}

void Display::paintGL()
{
    CARNA_ASSERT( pimpl->renderer.get() != nullptr );

    if( pimpl->cam == nullptr )
    {
        base::Log::instance().record( base::Log::debug, "Display has no camera but should render." );
    }
    else
    {
        if( pimpl->root == nullptr )
        {
            pimpl->root = &pimpl->cam->findRoot();
        }
        pimpl->renderer->render( *pimpl->cam, *pimpl->root );
    }
}

void Display::setRotationSpeed( float radiansPerPixel )
{
    pimpl->radiansPerPixel = radiansPerPixel;
}

void Display::setAxialMovementSpeed( float axialMovementSpeed )
{
    pimpl->axialMovementSpeed = axialMovementSpeed;
}

void Display::mousePressEvent( QMouseEvent* ev )
{
    /* First, try to pick object at clicked location.
     */
    const base::Geometry* picked = nullptr;
    if( pimpl->mccs != nullptr )
    {
        picked = pimpl->mccs->pick( ev->x(), ev->y() ).get();
    }

    /* Initiate camera rotation if nothing was picked.
     */
    if( picked == nullptr )
    {
        pimpl->mousepos = ev->pos();
        pimpl->mouseInteraction = true;
        ev->accept();
    }
    else
    if( pimpl->renderer != nullptr && hasCamera() && hasCameraControl() )
    {
        /* Picking was successful! Initiate spatial movement.
         */
        base::Geometry& pickedGeometry = const_cast< base::Geometry& >( *picked );
        pimpl->spatialMovement.reset
            ( new base::SpatialMovement( pickedGeometry, ev->x(), ev->y(), pimpl->renderer->viewport(), *pimpl->cam ) );
        pimpl->mouseInteraction = true;
        ev->accept();
    }
}

void Display::mouseMoveEvent( QMouseEvent* ev )
{
    if( pimpl->mouseInteraction )
    {
        if( pimpl->spatialMovement.get() == nullptr && hasCamera() && hasCameraControl() )
        {
            /* Camera rotation is going on.
             */
            const int dx = ( ev->x() - pimpl->mousepos.x() );
            const int dy = ( ev->y() - pimpl->mousepos.y() );
            pimpl->mousepos = ev->pos();

            if( dx != 0 || dy != 0 )
            {
                cameraControl().rotateHorizontally( dx * pimpl->radiansPerPixel );
                cameraControl().rotateVertically  ( dx * pimpl->radiansPerPixel );
                updateGL();
                ev->accept();
            }
        }
        else
        if( pimpl->spatialMovement.get() != nullptr )
        {
            /* Spatial movement is going on. Update it.
             */
            if( pimpl->spatialMovement->update( ev->x(), ev->y() ) )
            {
                updateGL();
                ev->accept();
            }
        }
    }
}

void Display::mouseReleaseEvent( QMouseEvent* ev )
{
    pimpl->mouseInteraction = false;
    pimpl->spatialMovement.reset();
    ev->accept();
}

void Display::wheelEvent( QWheelEvent* ev )
{
    if( hasCamera() && hasCameraControl() )
    {
        cameraControl().moveAxially( ev->delta() * pimpl->axialMovementSpeed );
        ev->accept();
    }
}

void Display::setCameraControl( base::Aggregation< base::CameraControl >* camControl )
{
    pimpl->camControl.reset( camControl );
    if( hasCamera() )
    {
        cameraControl().setCamera( *pimpl->cam );
    }
}

bool Display::hasCameraControl() const
{
    return pimpl->camControl.get() != nullptr && pimpl->camControl->get() != nullptr;
}

base::CameraControl& Display::cameraControl()
{
    CARNA_ASSERT( hasCameraControl() );
    return **pimpl->camControl;
}

const base::CameraControl& Display::cameraControl() const
{
    CARNA_ASSERT( hasCameraControl() );
    return **pimpl->camControl;
}

void Display::setProjectionControl( base::Aggregation< base::ProjectionControl >* projControl )
{
    pimpl->projControl.reset( projControl );
    pimpl->updateProjection( *this );
}

bool Display::hasProjectionControl() const
{
    return pimpl->projControl.get() != nullptr && pimpl->projControl->get() != nullptr;
}

base::ProjectionControl& Display::projectionControl()
{
    CARNA_ASSERT( hasProjectionControl() );
    return **pimpl->projControl;
}

const base::ProjectionControl& Display::projectionControl() const
{
    CARNA_ASSERT( hasProjectionControl() );
    return **pimpl->projControl;
}

bool Display::hasRenderer() const
{
    return pimpl->renderer.get() != nullptr;
}

base::FrameRenderer& Display::renderer()
{
    CARNA_ASSERT( hasRenderer() );
    return *pimpl->renderer;
}

const base::FrameRenderer& Display::renderer() const
{
    CARNA_ASSERT( hasRenderer() );
    return *pimpl->renderer;
}

}  // namespace Carna :: qt

}  // namespace Carna

<|endoftext|>


<commit_before>/* -*- mode:linux -*- */
/**
 * \file main.cc
 *
 *
 *
 * \author Ethan Burns
 * \date 2008-10-08
 */

#include <math.h>
#include <string.h>
#include <stdlib.h>

#include <iostream>
#include <vector>

#include "state.h"
#include "a_star.h"
#include "breadth_first_search.h"
#include "cost_bound_dfs.h"
#include "ida_star.h"
#include "kbfs.h"
#include "psdd_search.h"
#include "dynamic_bounded_psdd.h"
#include "pbnf_search.h"
#include "h_zero.h"
#include "grid/grid_world.h"

using namespace std;

// Globals -- ick.
unsigned int threads = 1;
float cost_bound = INFINITY;
float ratio = 1.0;
float weight = 1.0;

Search *get_search(int argc, char *argv[])
{
	if (argc > 1 && strcmp(argv[1], "astar") == 0) {
		return new AStar();
	} else if (argc > 1 && strcmp(argv[1], "idastar") == 0) {
		return new IDAStar();
	} else if (argc > 1 && strcmp(argv[1], "bfs") == 0) {
		return new BreadthFirstSearch();
	} else if (argc > 1
		   && sscanf(argv[1], "costbounddfs-%f", &cost_bound) == 1) {
		return new CostBoundDFS(cost_bound);
	} else if (argc > 1 && sscanf(argv[1], "kbfs-%u", &threads) == 1) {
		return new KBFS(threads);
	} else if (argc > 1
		   && sscanf(argv[1], "psdd-%u-%f", &threads, &ratio) == 2) {
		return new PSDDSearch(threads);
	} else if (argc > 1
		   && sscanf(argv[1], "dynpsdd-%u-%f-%f",
			     &threads, &ratio, &weight) == 3) {
		return new DynamicBoundedPSDD(threads, weight);
	} else if (argc > 1
		   && sscanf(argv[1], "pbnf-%u-%f", &threads, &ratio) == 2) {
		return new PBNFSearch(threads);
	} else {
		cout << "Must supply a search algorithm:" << endl;
		cout << "\tastar" << endl
		     << "\tidastar" << endl
		     << "\tbfs" << endl
		     << "\tcostbounddfs-<cost>" << endl
		     << "\tkbfs-<threads>" << endl
		     << "\tpsdd-<threads>-<nblocks/thread>" << endl
		     << "\tdynpsdd-<threads>-<nblocks/thread>-<weight>" << endl
		     << endl;
		exit(EXIT_FAILURE);
	}
}

int main(int argc, char *argv[])
{
	vector<const State *> *path;
	Search *search = get_search(argc, argv);
	GridWorld g(cin);

	if (ratio == 0)
		ratio = 1.0;
	int denom = g.get_height() / ((int) ratio * threads);
	unsigned int nblocks = g.get_height() / denom;
	GridWorld::RowModProject project(&g, nblocks);
	g.set_projection(&project);

//	HZero hzero(&g);
//	g.set_heuristic(&hzero);
	GridWorld::ManhattanDist manhattan(&g);
	g.set_heuristic(&manhattan);

	path = search->search(g.initial_state());

	/* Print the graph to the terminal */
//	g.print(cout, path);

	cout << "generated: " << search->get_generated() << endl;
	cout << "expanded: " << search->get_expanded() << endl;
	if (path) {
		cout << "cost: " << (int) path->at(0)->get_g() << endl;
		cout << "length: " << path->size() << endl;
		for (unsigned int i = 0; i < path->size(); i += 1)
			delete path->at(i);
		delete path;
	} else {
		cout << "No Solution" << endl;
	}

	delete search;

#if ENABLE_IMAGES
	g.export_eps("output.eps");
#endif	// ENABLE_IMAGES

	return EXIT_SUCCESS;
}
<commit_msg>added usage info for pbnf<commit_after>/* -*- mode:linux -*- */
/**
 * \file main.cc
 *
 *
 *
 * \author Ethan Burns
 * \date 2008-10-08
 */

#include <math.h>
#include <string.h>
#include <stdlib.h>

#include <iostream>
#include <vector>

#include "state.h"
#include "a_star.h"
#include "breadth_first_search.h"
#include "cost_bound_dfs.h"
#include "ida_star.h"
#include "kbfs.h"
#include "psdd_search.h"
#include "dynamic_bounded_psdd.h"
#include "pbnf_search.h"
#include "h_zero.h"
#include "grid/grid_world.h"

using namespace std;

// Globals -- ick.
unsigned int threads = 1;
float cost_bound = INFINITY;
float ratio = 1.0;
float weight = 1.0;

Search *get_search(int argc, char *argv[])
{
	if (argc > 1 && strcmp(argv[1], "astar") == 0) {
		return new AStar();
	} else if (argc > 1 && strcmp(argv[1], "idastar") == 0) {
		return new IDAStar();
	} else if (argc > 1 && strcmp(argv[1], "bfs") == 0) {
		return new BreadthFirstSearch();
	} else if (argc > 1
		   && sscanf(argv[1], "costbounddfs-%f", &cost_bound) == 1) {
		return new CostBoundDFS(cost_bound);
	} else if (argc > 1 && sscanf(argv[1], "kbfs-%u", &threads) == 1) {
		return new KBFS(threads);
	} else if (argc > 1
		   && sscanf(argv[1], "psdd-%u-%f", &threads, &ratio) == 2) {
		return new PSDDSearch(threads);
	} else if (argc > 1
		   && sscanf(argv[1], "dynpsdd-%u-%f-%f",
			     &threads, &ratio, &weight) == 3) {
		return new DynamicBoundedPSDD(threads, weight);
	} else if (argc > 1
		   && sscanf(argv[1], "pbnf-%u-%f", &threads, &ratio) == 2) {
		return new PBNFSearch(threads);
	} else {
		cout << "Must supply a search algorithm:" << endl;
		cout << "\tastar" << endl
		     << "\tidastar" << endl
		     << "\tbfs" << endl
		     << "\tcostbounddfs-<cost>" << endl
		     << "\tkbfs-<threads>" << endl
		     << "\tpsdd-<threads>-<nblocks/thread>" << endl
		     << "\tdynpsdd-<threads>-<nblocks/thread>-<weight>" << endl
		     << "\pbnf-<threads>-<nblocks/thread>" << endl
		     << endl;
		exit(EXIT_FAILURE);
	}
}

int main(int argc, char *argv[])
{
	vector<const State *> *path;
	Search *search = get_search(argc, argv);
	GridWorld g(cin);

	if (ratio == 0)
		ratio = 1.0;
	int denom = g.get_height() / ((int) ratio * threads);
	unsigned int nblocks = g.get_height() / denom;
	GridWorld::RowModProject project(&g, nblocks);
	g.set_projection(&project);

//	HZero hzero(&g);
//	g.set_heuristic(&hzero);
	GridWorld::ManhattanDist manhattan(&g);
	g.set_heuristic(&manhattan);

	path = search->search(g.initial_state());

	/* Print the graph to the terminal */
//	g.print(cout, path);

	cout << "generated: " << search->get_generated() << endl;
	cout << "expanded: " << search->get_expanded() << endl;
	if (path) {
		cout << "cost: " << (int) path->at(0)->get_g() << endl;
		cout << "length: " << path->size() << endl;
		for (unsigned int i = 0; i < path->size(); i += 1)
			delete path->at(i);
		delete path;
	} else {
		cout << "No Solution" << endl;
	}

	delete search;

#if ENABLE_IMAGES
	g.export_eps("output.eps");
#endif	// ENABLE_IMAGES

	return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>#include <algorithm>
#include <random>
#include <vector>

#include <transformer.hh>

allrgb::Transformer::Transformer(cv::Mat& input, const bool random)
  : img_(input)
  , colors_(9)
  , random_(random)
{}

cv::Mat&
allrgb::Transformer::img_get() const
{
  return img_;
}

void
allrgb::Transformer::operator()()
{
  assert(colors_.at(0) == 4096 * 4096);

  if (random_)
  {
    std::vector<cv::Point> points;
    points.reserve(4096 * 4096);
    for (size_t i = 0; i < 4096 * 4096; ++i)
      points.emplace_back(i % 4096, i / 4096);

    std::random_device rd;
    std::shuffle(points.begin(), points.end(), std::mt19937(rd()));

    for (const auto& point : points)
      replace_color_(img_.at<cv::Vec3b>(point));
  }
  else
  {
    for (auto it = img_.begin<cv::Vec3b>(); it != img_.end<cv::Vec3b>(); ++it)
      replace_color_(*it);
  }

  assert(colors_.at(0) == 0);
}

void
allrgb::Transformer::replace_color_(cv::Vec3b& color)
{
  const uchar blue = color[0];
  const uchar green = color[1];
  const uchar red = color[2];

  uchar new_red = 0;
  uchar new_green = 0;
  uchar new_blue = 0;

  size_t octree_index = 0;

  for (unsigned i = 0; i < 8; ++i)
  {
    const size_t perfect_child = ocnode_index_(red, green, blue, i);
    const size_t prev_octree_index = octree_index;
    octree_index = colors_.index_child(prev_octree_index, perfect_child);

    size_t last_chosen = perfect_child;
    size_t nb_try = 0;
    while (colors_.at(octree_index) <= 0)
    {
      last_chosen = next_lookup_(perfect_child, ++nb_try);
      octree_index = colors_.index_child(prev_octree_index, last_chosen);
    }
    choose_child_(new_red, new_green, new_blue, last_chosen);
  }

  colors_.delete_leaf(octree_index);

  color[0] = new_blue;
  color[1] = new_green;
  color[2] = new_red;
}

// bit_index = 0 is the LSB
uchar
allrgb::Transformer::bdigit_(const uchar value, const size_t bit_index)
{
  assert(bit_index < 8);

  const uchar mask = 0b00000001 << bit_index;
  return (value & mask) >> bit_index;
}

size_t
allrgb::Transformer::ocnode_index_(const uchar r, const uchar g, const uchar b,
                                   const size_t ocnode_depth)
{
  assert(ocnode_depth < 8);

  const size_t bindex = 7 - ocnode_depth;

  size_t res = 0;
  res = bdigit_(r, bindex);
  res = (res << 1) | bdigit_(g, bindex);
  res = (res << 1) | bdigit_(b, bindex);
  return res;
}

void
allrgb::Transformer::choose_child_(uchar& r, uchar& g, uchar& b,
                                   const size_t nb_child)
{
  assert(nb_child < 8);

  const uchar red_digit = (nb_child & 0b00000100) >> 2;
  const uchar green_digit = (nb_child & 0b00000010) >> 1;
  const uchar blue_digit = nb_child & 0b00000001;

  r = (r << 1) | red_digit;
  g = (g << 1) | green_digit;
  b = (b << 1) | blue_digit;
}

size_t
allrgb::Transformer::next_lookup_(const size_t perfect, const size_t nb_try)
{
  assert(perfect < 8 && nb_try < 8);

  const static size_t lookup_table[8][8] = {
    {0, 1, 4, 5, 2, 3, 6, 7},
    {1, 0, 5, 4, 3, 2, 7, 6},
    {2, 3, 6, 7, 0, 1, 4, 5},
    {3, 2, 7, 6, 1, 0, 5, 4},
    {4, 5, 0, 1, 6, 7, 2, 3},
    {5, 4, 1, 0, 7, 6, 3, 2},
    {6, 7, 2, 3, 4, 5, 0, 1},
    {7, 6, 3, 2, 5, 4, 1, 0}
  };

  return lookup_table[perfect][nb_try];
}
<commit_msg>[TRANSFORM] Allow rectangles as input for random traversal<commit_after>#include <algorithm>
#include <random>
#include <vector>

#include <transformer.hh>

allrgb::Transformer::Transformer(cv::Mat& input, const bool random)
  : img_(input)
  , colors_(9)
  , random_(random)
{}

cv::Mat&
allrgb::Transformer::img_get() const
{
  return img_;
}

void
allrgb::Transformer::operator()()
{
  assert(colors_.at(0) == 4096 * 4096);

  if (random_)
  {
    std::vector<cv::Point> points;
    points.reserve(4096 * 4096);
    for (int y = 0; y < img_.rows; ++y)
      for (int x = 0; x < img_.cols; ++x)
        points.emplace_back(x, y);

    std::random_device rd;
    std::shuffle(points.begin(), points.end(), std::mt19937(rd()));

    for (const auto& point : points)
      replace_color_(img_.at<cv::Vec3b>(point));
  }
  else
  {
    for (auto it = img_.begin<cv::Vec3b>(); it != img_.end<cv::Vec3b>(); ++it)
      replace_color_(*it);
  }

  assert(colors_.at(0) == 0);
}

void
allrgb::Transformer::replace_color_(cv::Vec3b& color)
{
  const uchar blue = color[0];
  const uchar green = color[1];
  const uchar red = color[2];

  uchar new_red = 0;
  uchar new_green = 0;
  uchar new_blue = 0;

  size_t octree_index = 0;

  for (unsigned i = 0; i < 8; ++i)
  {
    const size_t perfect_child = ocnode_index_(red, green, blue, i);
    const size_t prev_octree_index = octree_index;
    octree_index = colors_.index_child(prev_octree_index, perfect_child);

    size_t last_chosen = perfect_child;
    size_t nb_try = 0;
    while (colors_.at(octree_index) <= 0)
    {
      last_chosen = next_lookup_(perfect_child, ++nb_try);
      octree_index = colors_.index_child(prev_octree_index, last_chosen);
    }
    choose_child_(new_red, new_green, new_blue, last_chosen);
  }

  colors_.delete_leaf(octree_index);

  color[0] = new_blue;
  color[1] = new_green;
  color[2] = new_red;
}

// bit_index = 0 is the LSB
uchar
allrgb::Transformer::bdigit_(const uchar value, const size_t bit_index)
{
  assert(bit_index < 8);

  const uchar mask = 0b00000001 << bit_index;
  return (value & mask) >> bit_index;
}

size_t
allrgb::Transformer::ocnode_index_(const uchar r, const uchar g, const uchar b,
                                   const size_t ocnode_depth)
{
  assert(ocnode_depth < 8);

  const size_t bindex = 7 - ocnode_depth;

  size_t res = 0;
  res = bdigit_(r, bindex);
  res = (res << 1) | bdigit_(g, bindex);
  res = (res << 1) | bdigit_(b, bindex);
  return res;
}

void
allrgb::Transformer::choose_child_(uchar& r, uchar& g, uchar& b,
                                   const size_t nb_child)
{
  assert(nb_child < 8);

  const uchar red_digit = (nb_child & 0b00000100) >> 2;
  const uchar green_digit = (nb_child & 0b00000010) >> 1;
  const uchar blue_digit = nb_child & 0b00000001;

  r = (r << 1) | red_digit;
  g = (g << 1) | green_digit;
  b = (b << 1) | blue_digit;
}

size_t
allrgb::Transformer::next_lookup_(const size_t perfect, const size_t nb_try)
{
  assert(perfect < 8 && nb_try < 8);

  const static size_t lookup_table[8][8] = {
    {0, 1, 4, 5, 2, 3, 6, 7},
    {1, 0, 5, 4, 3, 2, 7, 6},
    {2, 3, 6, 7, 0, 1, 4, 5},
    {3, 2, 7, 6, 1, 0, 5, 4},
    {4, 5, 0, 1, 6, 7, 2, 3},
    {5, 4, 1, 0, 7, 6, 3, 2},
    {6, 7, 2, 3, 4, 5, 0, 1},
    {7, 6, 3, 2, 5, 4, 1, 0}
  };

  return lookup_table[perfect][nb_try];
}
<|endoftext|>


<commit_before>// Copyright (c) 2019 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <vector>
#include <assert.h>
#include <crypto/common.h>

namespace {

uint32_t DecodeBits(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos, uint8_t minval, const std::vector<uint8_t> &bit_sizes)
{
    uint32_t val = minval;
    bool bit;
    for (std::vector<uint8_t>::const_iterator bit_sizes_it = bit_sizes.begin();
        bit_sizes_it != bit_sizes.end(); ++bit_sizes_it) {
        if (bit_sizes_it + 1 != bit_sizes.end()) {
            if (bitpos == endpos) break;
            bit = *bitpos;
            bitpos++;
        } else {
            bit = 0;
        }
        if (bit) {
            val += (1 << *bit_sizes_it);
        } else {
            for (int b = 0; b < *bit_sizes_it; b++) {
                if (bitpos == endpos) break;
                bit = *bitpos;
                bitpos++;
                val += bit << (*bit_sizes_it - 1 - b);
            }
            return val;
        }
    }
    return -1;
}

const std::vector<uint8_t> TYPE_BIT_SIZES{0, 0, 1};
uint32_t DecodeType(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos)
{
    return DecodeBits(bitpos, endpos, 0, TYPE_BIT_SIZES);
}

const std::vector<uint8_t> ASN_BIT_SIZES{15, 16, 17, 18, 19, 20, 21, 22, 23, 24};
uint32_t DecodeASN(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos)
{
    return DecodeBits(bitpos, endpos, 1, ASN_BIT_SIZES);
}

const std::vector<uint8_t> MATCH_BIT_SIZES{1, 2, 3, 4, 5, 6, 7, 8};
uint32_t DecodeMatch(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos)
{
    return DecodeBits(bitpos, endpos, 2, MATCH_BIT_SIZES);
}

const std::vector<uint8_t> JUMP_BIT_SIZES{5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30};
uint32_t DecodeJump(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos)
{
    return DecodeBits(bitpos, endpos, 17, JUMP_BIT_SIZES);
}

}

uint32_t Interpret(const std::vector<bool> &asmap, const std::vector<bool> &ip)
{
    std::vector<bool>::const_iterator pos = asmap.begin();
    const std::vector<bool>::const_iterator endpos = asmap.end();
    uint8_t bits = ip.size();
    uint32_t default_asn = 0;
    uint32_t opcode, jump, match, matchlen;
    while (pos != endpos) {
        opcode = DecodeType(pos, endpos);
        if (opcode == 0) {
            return DecodeASN(pos, endpos);
        } else if (opcode == 1) {
            jump = DecodeJump(pos, endpos);
            if (bits == 0) break;
            if (ip[ip.size() - bits]) {
                if (jump >= endpos - pos) break;
                pos += jump;
            }
            bits--;
        } else if (opcode == 2) {
            match = DecodeMatch(pos, endpos);
            matchlen = CountBits(match) - 1;
            for (uint32_t bit = 0; bit < matchlen; bit++) {
                if (bits == 0) break;
                if ((ip[ip.size() - bits]) != ((match >> (matchlen - 1 - bit)) & 1)) {
                    return default_asn;
                }
                bits--;
            }
        } else if (opcode == 3) {
            default_asn = DecodeASN(pos, endpos);
        } else {
            break;
        }
    }
    return 0; // 0 is not a valid ASN
}
<commit_msg>Introduce Instruction enum in asmap<commit_after>// Copyright (c) 2019 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <vector>
#include <assert.h>
#include <crypto/common.h>

namespace {

uint32_t DecodeBits(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos, uint8_t minval, const std::vector<uint8_t> &bit_sizes)
{
    uint32_t val = minval;
    bool bit;
    for (std::vector<uint8_t>::const_iterator bit_sizes_it = bit_sizes.begin();
        bit_sizes_it != bit_sizes.end(); ++bit_sizes_it) {
        if (bit_sizes_it + 1 != bit_sizes.end()) {
            if (bitpos == endpos) break;
            bit = *bitpos;
            bitpos++;
        } else {
            bit = 0;
        }
        if (bit) {
            val += (1 << *bit_sizes_it);
        } else {
            for (int b = 0; b < *bit_sizes_it; b++) {
                if (bitpos == endpos) break;
                bit = *bitpos;
                bitpos++;
                val += bit << (*bit_sizes_it - 1 - b);
            }
            return val;
        }
    }
    return -1;
}

enum class Instruction : uint32_t
{
    RETURN = 0,
    JUMP = 1,
    MATCH = 2,
    DEFAULT = 3,
};

const std::vector<uint8_t> TYPE_BIT_SIZES{0, 0, 1};
Instruction DecodeType(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos)
{
    return Instruction(DecodeBits(bitpos, endpos, 0, TYPE_BIT_SIZES));
}

const std::vector<uint8_t> ASN_BIT_SIZES{15, 16, 17, 18, 19, 20, 21, 22, 23, 24};
uint32_t DecodeASN(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos)
{
    return DecodeBits(bitpos, endpos, 1, ASN_BIT_SIZES);
}

const std::vector<uint8_t> MATCH_BIT_SIZES{1, 2, 3, 4, 5, 6, 7, 8};
uint32_t DecodeMatch(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos)
{
    return DecodeBits(bitpos, endpos, 2, MATCH_BIT_SIZES);
}

const std::vector<uint8_t> JUMP_BIT_SIZES{5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30};
uint32_t DecodeJump(std::vector<bool>::const_iterator& bitpos, const std::vector<bool>::const_iterator& endpos)
{
    return DecodeBits(bitpos, endpos, 17, JUMP_BIT_SIZES);
}

}

uint32_t Interpret(const std::vector<bool> &asmap, const std::vector<bool> &ip)
{
    std::vector<bool>::const_iterator pos = asmap.begin();
    const std::vector<bool>::const_iterator endpos = asmap.end();
    uint8_t bits = ip.size();
    uint32_t default_asn = 0;
    uint32_t jump, match, matchlen;
    Instruction opcode;
    while (pos != endpos) {
        opcode = DecodeType(pos, endpos);
        if (opcode == Instruction::RETURN) {
            return DecodeASN(pos, endpos);
        } else if (opcode == Instruction::JUMP) {
            jump = DecodeJump(pos, endpos);
            if (bits == 0) break;
            if (ip[ip.size() - bits]) {
                if (jump >= endpos - pos) break;
                pos += jump;
            }
            bits--;
        } else if (opcode == Instruction::MATCH) {
            match = DecodeMatch(pos, endpos);
            matchlen = CountBits(match) - 1;
            for (uint32_t bit = 0; bit < matchlen; bit++) {
                if (bits == 0) break;
                if ((ip[ip.size() - bits]) != ((match >> (matchlen - 1 - bit)) & 1)) {
                    return default_asn;
                }
                bits--;
            }
        } else if (opcode == Instruction::DEFAULT) {
            default_asn = DecodeASN(pos, endpos);
        } else {
            break;
        }
    }
    return 0; // 0 is not a valid ASN
}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkFollower.cc
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 1993-1995 Ken Martin, Will Schroeder, Bill Lorensen.

This software is copyrighted by Ken Martin, Will Schroeder and Bill Lorensen.
The following terms apply to all files associated with the software unless
explicitly disclaimed in individual files. This copyright specifically does
not apply to the related textbook "The Visualization Toolkit" ISBN
013199837-4 published by Prentice Hall which is covered by its own copyright.

The authors hereby grant permission to use, copy, and distribute this
software and its documentation for any purpose, provided that existing
copyright notices are retained in all copies and that this notice is included
verbatim in any distributions. Additionally, the authors grant permission to
modify this software and its documentation for any purpose, provided that
such modifications are not distributed without the explicit consent of the
authors and that existing copyright notices are retained in all copies. Some
of the algorithms implemented by this software are patented, observe all
applicable patent law.

IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY FOR
DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY DERIVATIVES THEREOF,
EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE IS PROVIDED ON AN
"AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE NO OBLIGATION TO PROVIDE
MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

=========================================================================*/
#include <stdlib.h>
#include <math.h>
#include "vtkMath.hh"
#include "vtkFollower.hh"
#include "vtkCamera.hh"

// Description:
// Creates an follower with no camera set
vtkFollower::vtkFollower()
{
  this->Camera = NULL;
}

vtkFollower::~vtkFollower()
{
}

// Description:
// Copy the Follower's composite 4x4 matrix into the matrix provided.
void vtkFollower::GetMatrix(vtkMatrix4x4& result)
{
  float *pos;
  vtkMatrix4x4 matrix;

  this->GetOrientation();
  this->Transform.Push();
  this->Transform.Identity();
  this->Transform.PreMultiply();

  // first translate
  this->Transform.Translate(this->Position[0],
			    this->Position[1],
			    this->Position[2]);

  // shift to origin
  this->Transform.Translate(this->Origin[0],
			    this->Origin[1],
			    this->Origin[2]);

  // add the rotation to follow the camera
  if (this->Camera)
    {
    float distance, distance_old;
    float *vup;
    float twist = 0;
    float v1[3], v2[3], y_axis[3];
    double theta, dot, mag;
    double cosang;
    vtkMath math;
    float vn[3];

    // calc the direction
    pos = this->Camera->GetPosition();
    vup = this->Camera->GetViewUp();

    // first rotate y
    distance = sqrt((this->Position[0]-pos[0])*(this->Position[0]-pos[0]) +
		    (this->Position[2]-pos[2])*(this->Position[2]-pos[2]));
    vn[0] = (pos[0] - this->Position[0])/distance;
    vn[1] = (pos[1] - this->Position[1])/distance;
    vn[2] = (pos[2] - this->Position[2])/distance;

    // rotate y
    if (distance > 0.0)
      {
      matrix[0][0] = (this->Position[2]-pos[2])/distance;
      matrix[0][2] = -1.0*(pos[0] - this->Position[0])/distance;
      }
    else
      {
      if (this->Position[1] < pos[1])
	{
	matrix[0][0] = -1.0;
	}
      else
	{
	matrix[0][0] = 1.0;
	}
      matrix[0][2] = 0.0;
      }
    matrix[0][1] = matrix[0][3] = 0.0;
    matrix[1][1] = 1.0;
    matrix[1][0] = matrix[1][2] = matrix[1][3] = 0.0;
    matrix[2][0] = -1.0*matrix[0][2];
    matrix[2][2] = matrix[0][0];
    matrix[2][3] = 0.0;
    matrix[2][1] = 0.0;
    matrix[3][3] = 1.0;
    matrix[3][0] = matrix[3][1] = matrix[3][2] = 0.0;
    this->Transform.Concatenate(matrix);

    // now rotate x
    distance_old = distance;
    distance = sqrt((this->Position[0]-pos[0])*(this->Position[0]-pos[0]) +
		    (this->Position[1]-pos[1])*(this->Position[1]-pos[1]) +
		    (this->Position[2]-pos[2])*(this->Position[2]-pos[2]));
    matrix[0][0] = 1.0;
    matrix[0][1] = matrix[0][2] = matrix[0][3] = 0.0;
    matrix[1][1] = distance_old/distance;
    matrix[1][2] = (this->Position[1] - pos[1])/distance;
    matrix[1][0] = matrix[1][3] = 0.0;
    matrix[2][1] = -1.0*matrix[1][2];
    matrix[2][2] = matrix[1][1];
    matrix[2][3] = 0.0;
    matrix[2][0] = 0.0;
    matrix[3][3] = 1.0;
    matrix[3][0] = matrix[3][1] = matrix[3][2] = 0.0;
    this->Transform.Concatenate(matrix);

    // calc the twist
    // compute: vn X ( vup X vn)
    // and:     vn X ( y-axis X vn)
    // then find the angle between the two projected vectors
    //
    y_axis[0] = y_axis[2] = 0.0; y_axis[1] = 1.0;

    // bump the view normal if it is parallel to the y-axis
    //
    if ((vn[0] == 0.0) && (vn[2] == 0.0))
      vn[2] = 0.01*vn[1];

    // first project the view_up onto the view_plane
    //
    math.Cross(vup, vn, v1);
    math.Cross(vn, v1, v1);

    // then project the y-axis onto the view plane
    //
    math.Cross(y_axis, vn, v2);
    math.Cross(vn, v2, v2);

    // then find the angle between the two projected vectors
    //
    dot = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
    mag = sqrt((double)(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]));
    mag *= sqrt((double)(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]));

    // make sure we dont divide by 0
    if (mag != 0.0)
      {
      cosang = dot / mag;
      if (cosang < -1.0) cosang = -1.0;
      if (cosang > 1.0) cosang = 1.0;
      theta = acos(cosang);
      }
    else
      theta = 0.0;

    // now see if the angle is positive or negative
    //
    math.Cross(v1, v2, v1);
    dot = v1[0]*vn[0] + v1[1]*vn[1] + v1[2]*vn[2];

    twist = (theta);
    if (dot < 0.0)
      twist = -twist;

    // now rotate z (twist)
    matrix[0][0] = cos(-twist);
    matrix[0][1] = sin(-twist);
    matrix[0][2] = matrix[0][3] = 0.0;
    matrix[1][0] = -1.0*matrix[0][1];
    matrix[1][1] = matrix[0][0];
    matrix[1][2] = matrix[1][3] = 0.0;
    matrix[2][1] = 0.0;
    matrix[2][2] = 1.0;
    matrix[2][3] = 0.0;
    matrix[2][0] = 0.0;
    matrix[3][3] = 1.0;
    matrix[3][0] = matrix[3][1] = matrix[3][2] = 0.0;
    this->Transform.Concatenate(matrix);

    // rotate y by 180 to get the positive zaxis instead of negative
    this->Transform.RotateY(180);
    }

  // rotate
  this->Transform.RotateZ(this->Orientation[2]);
  this->Transform.RotateX(this->Orientation[0]);
  this->Transform.RotateY(this->Orientation[1]);

  // scale
  this->Transform.Scale(this->Scale[0],
			this->Scale[1],
			this->Scale[2]);

  // shift back from origin
  this->Transform.Translate(-this->Origin[0],
			    -this->Origin[1],
			    -this->Origin[2]);

  result = this->Transform.GetMatrix();

  this->Transform.Pop();
}

void vtkFollower::PrintSelf(ostream& os, vtkIndent indent)
{
  vtkActor::PrintSelf(os,indent);

  if ( this->Camera )
    {
    os << indent << "Camera:\n";
    this->Camera->PrintSelf(os,indent.GetNextIndent());
    }
  else
    {
    os << indent << "Camera: (none)\n";
    }
}

<commit_msg>minor fix<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkFollower.cc
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 1993-1995 Ken Martin, Will Schroeder, Bill Lorensen.

This software is copyrighted by Ken Martin, Will Schroeder and Bill Lorensen.
The following terms apply to all files associated with the software unless
explicitly disclaimed in individual files. This copyright specifically does
not apply to the related textbook "The Visualization Toolkit" ISBN
013199837-4 published by Prentice Hall which is covered by its own copyright.

The authors hereby grant permission to use, copy, and distribute this
software and its documentation for any purpose, provided that existing
copyright notices are retained in all copies and that this notice is included
verbatim in any distributions. Additionally, the authors grant permission to
modify this software and its documentation for any purpose, provided that
such modifications are not distributed without the explicit consent of the
authors and that existing copyright notices are retained in all copies. Some
of the algorithms implemented by this software are patented, observe all
applicable patent law.

IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY FOR
DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY DERIVATIVES THEREOF,
EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE IS PROVIDED ON AN
"AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE NO OBLIGATION TO PROVIDE
MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

=========================================================================*/
#include <stdlib.h>
#include <math.h>
#include "vtkMath.hh"
#include "vtkFollower.hh"
#include "vtkCamera.hh"

// Description:
// Creates an follower with no camera set
vtkFollower::vtkFollower()
{
  this->Camera = NULL;
}

vtkFollower::~vtkFollower()
{
}

// Description:
// Copy the Follower's composite 4x4 matrix into the matrix provided.
void vtkFollower::GetMatrix(vtkMatrix4x4& result)
{
  float *pos;
  vtkMatrix4x4 matrix;

  this->GetOrientation();
  this->Transform.Push();
  this->Transform.Identity();
  this->Transform.PreMultiply();

  // apply user defined matrix last if there is one
  if (this->UserMatrix)
    {
    this->Transform.Concatenate(*this->UserMatrix);
    }

  // first translate
  this->Transform.Translate(this->Position[0],
			    this->Position[1],
			    this->Position[2]);

  // shift to origin
  this->Transform.Translate(this->Origin[0],
			    this->Origin[1],
			    this->Origin[2]);

  // add the rotation to follow the camera
  if (this->Camera)
    {
    float distance, distance_old;
    float *vup;
    float twist = 0;
    float v1[3], v2[3], y_axis[3];
    double theta, dot, mag;
    double cosang;
    vtkMath math;
    float vn[3];

    // calc the direction
    pos = this->Camera->GetPosition();
    vup = this->Camera->GetViewUp();

    // first rotate y
    distance = sqrt((this->Position[0]-pos[0])*(this->Position[0]-pos[0]) +
		    (this->Position[2]-pos[2])*(this->Position[2]-pos[2]));
    vn[0] = (pos[0] - this->Position[0])/distance;
    vn[1] = (pos[1] - this->Position[1])/distance;
    vn[2] = (pos[2] - this->Position[2])/distance;

    // rotate y
    if (distance > 0.0)
      {
      matrix[0][0] = (this->Position[2]-pos[2])/distance;
      matrix[0][2] = -1.0*(pos[0] - this->Position[0])/distance;
      }
    else
      {
      if (this->Position[1] < pos[1])
	{
	matrix[0][0] = -1.0;
	}
      else
	{
	matrix[0][0] = 1.0;
	}
      matrix[0][2] = 0.0;
      }
    matrix[0][1] = matrix[0][3] = 0.0;
    matrix[1][1] = 1.0;
    matrix[1][0] = matrix[1][2] = matrix[1][3] = 0.0;
    matrix[2][0] = -1.0*matrix[0][2];
    matrix[2][2] = matrix[0][0];
    matrix[2][3] = 0.0;
    matrix[2][1] = 0.0;
    matrix[3][3] = 1.0;
    matrix[3][0] = matrix[3][1] = matrix[3][2] = 0.0;
    this->Transform.Concatenate(matrix);

    // now rotate x
    distance_old = distance;
    distance = sqrt((this->Position[0]-pos[0])*(this->Position[0]-pos[0]) +
		    (this->Position[1]-pos[1])*(this->Position[1]-pos[1]) +
		    (this->Position[2]-pos[2])*(this->Position[2]-pos[2]));
    matrix[0][0] = 1.0;
    matrix[0][1] = matrix[0][2] = matrix[0][3] = 0.0;
    matrix[1][1] = distance_old/distance;
    matrix[1][2] = (this->Position[1] - pos[1])/distance;
    matrix[1][0] = matrix[1][3] = 0.0;
    matrix[2][1] = -1.0*matrix[1][2];
    matrix[2][2] = matrix[1][1];
    matrix[2][3] = 0.0;
    matrix[2][0] = 0.0;
    matrix[3][3] = 1.0;
    matrix[3][0] = matrix[3][1] = matrix[3][2] = 0.0;
    this->Transform.Concatenate(matrix);

    // calc the twist
    // compute: vn X ( vup X vn)
    // and:     vn X ( y-axis X vn)
    // then find the angle between the two projected vectors
    //
    y_axis[0] = y_axis[2] = 0.0; y_axis[1] = 1.0;

    // bump the view normal if it is parallel to the y-axis
    //
    if ((vn[0] == 0.0) && (vn[2] == 0.0))
      vn[2] = 0.01*vn[1];

    // first project the view_up onto the view_plane
    //
    math.Cross(vup, vn, v1);
    math.Cross(vn, v1, v1);

    // then project the y-axis onto the view plane
    //
    math.Cross(y_axis, vn, v2);
    math.Cross(vn, v2, v2);

    // then find the angle between the two projected vectors
    //
    dot = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
    mag = sqrt((double)(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]));
    mag *= sqrt((double)(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]));

    // make sure we dont divide by 0
    if (mag != 0.0)
      {
      cosang = dot / mag;
      if (cosang < -1.0) cosang = -1.0;
      if (cosang > 1.0) cosang = 1.0;
      theta = acos(cosang);
      }
    else
      theta = 0.0;

    // now see if the angle is positive or negative
    //
    math.Cross(v1, v2, v1);
    dot = v1[0]*vn[0] + v1[1]*vn[1] + v1[2]*vn[2];

    twist = (theta);
    if (dot < 0.0)
      twist = -twist;

    // now rotate z (twist)
    matrix[0][0] = cos(-twist);
    matrix[0][1] = sin(-twist);
    matrix[0][2] = matrix[0][3] = 0.0;
    matrix[1][0] = -1.0*matrix[0][1];
    matrix[1][1] = matrix[0][0];
    matrix[1][2] = matrix[1][3] = 0.0;
    matrix[2][1] = 0.0;
    matrix[2][2] = 1.0;
    matrix[2][3] = 0.0;
    matrix[2][0] = 0.0;
    matrix[3][3] = 1.0;
    matrix[3][0] = matrix[3][1] = matrix[3][2] = 0.0;
    this->Transform.Concatenate(matrix);

    // rotate y by 180 to get the positive zaxis instead of negative
    this->Transform.RotateY(180);
    }

  // rotate
  this->Transform.RotateZ(this->Orientation[2]);
  this->Transform.RotateX(this->Orientation[0]);
  this->Transform.RotateY(this->Orientation[1]);

  // scale
  this->Transform.Scale(this->Scale[0],
			this->Scale[1],
			this->Scale[2]);

  // shift back from origin
  this->Transform.Translate(-this->Origin[0],
			    -this->Origin[1],
			    -this->Origin[2]);

  result = this->Transform.GetMatrix();

  this->Transform.Pop();
}

void vtkFollower::PrintSelf(ostream& os, vtkIndent indent)
{
  vtkActor::PrintSelf(os,indent);

  if ( this->Camera )
    {
    os << indent << "Camera:\n";
    this->Camera->PrintSelf(os,indent.GetNextIndent());
    }
  else
    {
    os << indent << "Camera: (none)\n";
    }
}

<|endoftext|>


<commit_before>#pragma once

#include <algorithm>
#include <string>

namespace cu
{

/// Tells whether the @c txt string ends with the @c end string.
template <std::size_t N>
bool endsWith( const std::string & txt, const char (&end)[N] )
{
  // The actual length of the `end` string is N-1, because of the
  // terminating '\0' character.
  const auto endLength = N-1;
  if ( txt.size() < endLength )
    return false;
  return std::equal( end, end + endLength,
                     txt.end()- endLength );
}

/// Tells whether the @c txt string ends with the @c end string.
inline bool endsWith( const std::string & txt, const std::string & end )
{
  if ( txt.size() < end.size() )
    return false;
  return std::equal( end.begin(), end.end(),
                     txt.end() - end.size() );
}

} // namespace cu
<commit_msg>Added cu::trim() for std::string.<commit_after>#pragma once

#include <algorithm>
#include <string>

namespace cu
{

/// Tells whether the @c txt string ends with the @c end string.
template <std::size_t N>
bool endsWith( const std::string & txt, const char (&end)[N] )
{
  // The actual length of the `end` string is N-1, because of the
  // terminating '\0' character.
  const auto endLength = N-1;
  if ( txt.size() < endLength )
    return false;
  return std::equal( end, end + endLength,
                     txt.end()- endLength );
}

/// Tells whether the @c txt string ends with the @c end string.
inline bool endsWith( const std::string & txt, const std::string & end )
{
  if ( txt.size() < end.size() )
    return false;
  return std::equal( end.begin(), end.end(),
                     txt.end() - end.size() );
}

/// Removes whitespaces from the beginning and end of a string.
inline std::string trim( const std::string & s )
{
  auto first = s.begin();
  auto last = s.end();
  while ( first < last && std::isspace( *first) )
    ++first;
  while ( last > first && std::isspace( *std::prev(last) ) )
    --last; // NOOP

  return { first, last };
}

} // namespace cu
<|endoftext|>


<commit_before>#include <cstring>
#include <cstdio>

void dfpair(FILE *, const char *key, const char *fmt, ...);

template <class Elm> struct HtableEntry {
	Elm *nxt;
};

template <class Ops, class Key, class Elm> class Htable {
public:

	enum {
		Defsz = 1024,
		Growfact = 2,
	};

	static const float Fillfact = 1.5;

	Htable(unsigned int sz = Defsz)
		: fill(0), ncollide(0), nresize(0), nbins(0), bins(NULL) {
		resize(sz);
	}

	~Htable(void) {
		if (bins)
			delete[] bins;
	}

	void add(Elm *e) {
		add(e, Ops::hash(Ops::key(e)));
	}

	void add(Elm *e, unsigned long h) {
		if (fill * Fillfact >= nbins)
			resize(nbins == 0 ? Defsz : nbins * Growfact);

		add(bins, nbins, e, h);
		fill++;
	}

	Elm *find(Key k) {
		return find(k, Ops::hash(k));
	}

	Elm *find(Key k, unsigned long h) {
		unsigned int i = h % nbins;

		for (Elm *p = bins[i]; p; p = Ops::entry(p).nxt) {
			if (Ops::eq(Ops::key(p), k))
				return p;
		}

		return NULL;
	}

	void prstats(FILE *f, const char *prefix) {
		char key[strlen(prefix) + strlen("collisions") + 1];
		strcpy(key, prefix);

		strcat(key+strlen(prefix), "fill");
		dfpair(f, key, "%lu", fill);

		strcpy(key+strlen(prefix), "collisions");
		dfpair(f, key, "%lu", ncollide);

		strcpy(key+strlen(prefix), "resizes");
		dfpair(f, key, "%lu", nresize);
	}

private:

	void add(Elm *b[], unsigned int n, Elm *e, unsigned long h) {
		unsigned int i = h % n;
		if (b[i])
			ncollide++;
		Ops::entry(e).nxt = b[i];
		b[i] = e;
	}

	void resize(unsigned int sz) {
		Elm **b = new Elm*[sz];

		for (unsigned int i = 0; i < sz; i++)
			b[i] = NULL;

		for (unsigned int i = 0; i < nbins; i++) {
		for (Elm *p = bins[i]; p; p = Ops::entry(p).nxt)
			add(b, sz, p, Ops::hash(Ops::key(p)));
		}

		if (bins)
			delete[] bins;

		bins = b;
		nbins = sz;
		nresize++;
	}

	friend bool htable_add_test(void);

	unsigned long fill, ncollide;
	unsigned int nresize, nbins;
	Elm **bins;
};<commit_msg>Don't count the initial hash table resize().<commit_after>#include <cstring>
#include <cstdio>

void dfpair(FILE *, const char *key, const char *fmt, ...);

template <class Elm> struct HtableEntry {
	Elm *nxt;
};

template <class Ops, class Key, class Elm> class Htable {
public:

	enum {
		Defsz = 1024,
		Growfact = 2,
	};

	static const float Fillfact = 1.5;

	Htable(unsigned int sz = Defsz)
		: fill(0), ncollide(0), nbins(0), bins(NULL) {
		resize(sz);
		nresize = 0;	// don't count the initial resize
	}

	~Htable(void) {
		if (bins)
			delete[] bins;
	}

	void add(Elm *e) {
		add(e, Ops::hash(Ops::key(e)));
	}

	void add(Elm *e, unsigned long h) {
		if (fill * Fillfact >= nbins)
			resize(nbins == 0 ? Defsz : nbins * Growfact);

		add(bins, nbins, e, h);
		fill++;
	}

	Elm *find(Key k) {
		return find(k, Ops::hash(k));
	}

	Elm *find(Key k, unsigned long h) {
		unsigned int i = h % nbins;

		for (Elm *p = bins[i]; p; p = Ops::entry(p).nxt) {
			if (Ops::eq(Ops::key(p), k))
				return p;
		}

		return NULL;
	}

	void prstats(FILE *f, const char *prefix) {
		char key[strlen(prefix) + strlen("collisions") + 1];
		strcpy(key, prefix);

		strcat(key+strlen(prefix), "fill");
		dfpair(f, key, "%lu", fill);

		strcpy(key+strlen(prefix), "collisions");
		dfpair(f, key, "%lu", ncollide);

		strcpy(key+strlen(prefix), "resizes");
		dfpair(f, key, "%lu", nresize);
	}

private:

	void add(Elm *b[], unsigned int n, Elm *e, unsigned long h) {
		unsigned int i = h % n;
		if (b[i])
			ncollide++;
		Ops::entry(e).nxt = b[i];
		b[i] = e;
	}

	void resize(unsigned int sz) {
		Elm **b = new Elm*[sz];

		for (unsigned int i = 0; i < sz; i++)
			b[i] = NULL;

		for (unsigned int i = 0; i < nbins; i++) {
		for (Elm *p = bins[i]; p; p = Ops::entry(p).nxt)
			add(b, sz, p, Ops::hash(Ops::key(p)));
		}

		if (bins)
			delete[] bins;

		bins = b;
		nbins = sz;
		nresize++;
	}

	friend bool htable_add_test(void);

	unsigned long fill, ncollide;
	unsigned int nresize, nbins;
	Elm **bins;
};<|endoftext|>


<commit_before>#include "Eigen/Eigen"
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <limits>
#include <vector>
#include <queue>
#include <algorithm>

using namespace std;

template<typename T>
T dist2(const typename Eigen::Matrix<T, Eigen::Dynamic, 1> v0, const typename Eigen::Matrix<T, Eigen::Dynamic, 1> v1)
{
	return (v0 - v1).dot(v0 - v1);
}

template<typename T>
struct KDTree
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, 1> Vector;
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;

protected:
	struct BuildPoint
	{
		Vector pos;
		size_t index;
		BuildPoint(const Vector& pos =  Vector(), const size_t index = 0): pos(pos), index(index) {}
	};
	typedef vector<BuildPoint> BuildPoints;
	typedef typename BuildPoints::iterator BuildPointsIt;
	typedef typename BuildPoints::const_iterator BuildPointsCstIt;

	struct CompareDim
	{
		size_t dim;
		CompareDim(const size_t dim):dim(dim){}
		bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
	};

	struct SearchElement
	{
		size_t index;
		T minDist;

		SearchElement(const size_t index, const T minDist): index(index), minDist(minDist) {}
		friend bool operator<(const SearchElement& e0, const SearchElement& e1) { return e0.minDist < e1.minDist; }
	};

	// TODO: add index
	struct Node
	{
		Vector pos;
		int dim; // -1 == leaf, -2 == invalid
		Node(const Vector& pos = Vector(), const int dim = -2):pos(pos), dim(dim) {}
	};
	typedef vector<Node> Nodes;

	const size_t dim;
public:
	Vector minBound;
	Vector maxBound;
protected:
	Nodes nodes;

	inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
	inline size_t childRight(size_t pos) const { return 2*pos + 2; }
	inline size_t parent(size_t pos) const { return (pos-1)/2; }
	size_t getTreeSize(size_t size) const;
	size_t argMax(const Vector& v) const;
	void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos);
	void dump(const Vector minValues, const Vector maxValues, const size_t pos) const;

public:
	KDTree(const Matrix& cloud);
	Vector nn(const Vector& query);
};

template<typename T>
size_t KDTree<T>::getTreeSize(size_t elCount) const
{
	// FIXME: 64 bits safe stuff, only work for 2^32 elements right now
	size_t count = 0;
	int i = 31;
	for (; i >= 0; --i)
	{
		if (elCount & (1 << i))
			break;
	}
	for (int j = 0; j <= i; ++j)
		count |= (1 << j);
	//cerr << "tree size " << count << " (" << elCount << " elements)\n";
	return count;
}

template<typename T>
size_t KDTree<T>::argMax(const Vector& v) const
{
	T maxVal(0);
	size_t maxIdx;
	for (size_t i = 0; i < dim; ++i)
	{
		if (v[i] > maxVal)
		{
			maxVal = v[i];
			maxIdx = i;
		}
	}
	return maxIdx;
}

template<typename T>
void KDTree<T>::buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos)
{
	const size_t count(last - first);
	//cerr << count << endl;
	if (count == 1)
	{
		nodes[pos] = Node(first->pos, -1);
		return;
	}

	// estimate variance
	// get mean
	Vector mean(Vector::Zero(dim));
	for (BuildPointsCstIt it(first); it != last; ++it)
		mean += it->pos;
	mean /= last - first;
	// get sum of variance
	Vector var(Vector::Zero(dim));
	for (BuildPointsCstIt it(first); it != last; ++it)
		var += (it->pos - mean).cwise() * (it->pos - mean);
	// get dimension of maxmial variance
	const size_t cutDim = argMax(var);

	// sort
	sort(first, last, CompareDim(cutDim));

	// set node
	const size_t recurseCount(count-1);
	const size_t rightCount(recurseCount/2);
	const size_t leftCount(recurseCount-rightCount);
	assert(last - rightCount == first + leftCount + 1);

	nodes[pos] = Node((first+leftCount)->pos, cutDim);

	//cerr << pos << " cutting on " << cutDim << " at " << (first+leftCount)->pos[cutDim] << endl;

	// recurse
	if (count > 2)
	{
		buildNodes(first, first + leftCount, childLeft(pos));
		buildNodes(first + leftCount + 1, last, childRight(pos));
	}
	else
	{
		nodes[childLeft(pos)] = Node(first->pos, -1);
		nodes[childRight(pos)] = Node(Vector(), -2);
	}
}

template<typename T>
void KDTree<T>::dump(const Vector minValues, const Vector maxValues, const size_t pos) const
{
	const Node& node(nodes[pos]);

	if (node.dim >= -1)
	{
		if (dim == 2)
			cout << "<circle cx=\"" << 100*node.pos(0) << "\" cy=\"" << 100*node.pos(1) << "\" r=\"1\" stroke=\"black\" stroke-width=\"0.2\" fill=\"red\"/>" << endl;
		else
			cout << "pt at\n" << node.pos << endl;
	}
	if (node.dim >= 0)
	{
		//cerr << "in bounds:\n" << minValues << "\nto\n" << maxValues << endl;

		// update bounds for left
		Vector leftMaxValues(maxValues);
		leftMaxValues[node.dim] = node.pos[node.dim];
		// update bounds for right
		Vector rightMinValues(minValues);
		rightMinValues[node.dim] = node.pos[node.dim];

		// print line
		if (dim == 2)
			cout << "<line x1=\"" << 100*rightMinValues(0) << "\" y1=\"" << 100*rightMinValues(1) << "\" x2=\"" << 100*leftMaxValues(0) << "\" y2=\"" << 100*leftMaxValues(1) << "\" style=\"stroke:rgb(0,0,0);stroke-width:0.2\"/>" << endl;
		else
			cout << "cut from\n" << rightMinValues << "\nto\n" << leftMaxValues << endl;
		// recurs
		dump(minValues, leftMaxValues, childLeft(pos));
		dump(rightMinValues, maxValues, childRight(pos));
	}
}

template<typename T>
KDTree<T>::KDTree(const typename KDTree<T>::Matrix& cloud):
	dim(cloud.rows()),
	minBound(Vector::Constant(dim, numeric_limits<T>::max())),
	maxBound(Vector::Constant(dim, numeric_limits<T>::min()))
{
	// build point vector and compute bounds
	vector<BuildPoint> buildPoints;
	for (size_t i = 0; i < cloud.cols(); ++i)
	{
		const Vector& v(cloud.col(i));
		buildPoints.push_back(BuildPoint(v, i));
		minBound = minBound.cwise().min(v);
		maxBound = maxBound.cwise().max(v);
	}

	// create nodes
	nodes.resize(getTreeSize(cloud.cols()));
	buildNodes(buildPoints.begin(), buildPoints.end(), 0);

	// dump nodes
	//dump(minBound, maxBound, 0);
}

template<typename T>
typename KDTree<T>::Vector KDTree<T>::nn(const Vector& query)
{
	typedef priority_queue<SearchElement> Queue;
	Queue queue;

	queue.push(SearchElement(0, 0));

	T bestDist(numeric_limits<T>::max());
	size_t bestIndex(0);
	size_t visitCount(0);
	while (!queue.empty())
	{
		SearchElement el(queue.top());
		queue.pop();

		// nothing is closer, we found best
		if (el.minDist > bestDist)
			break;

		const Node& node(nodes[el.index]);
		// TODO: optimise, do not push these in first place
		if (node.dim == -2)
			continue;

		const T dist(dist2(node.pos, query));
		if (dist < bestDist)
		{
			bestIndex = el.index;
			bestDist = dist;
		}

		if (node.dim < 0)
			continue;

		const T offset(query[node.dim] - node.pos[node.dim]);
		const T offset2(offset * offset);
		if (offset > 0)
		{
			if (offset2 < bestDist)
				queue.push(SearchElement(childLeft(el.index), offset2));
			queue.push(SearchElement(childRight(el.index), 0));
		}
		else
		{
			queue.push(SearchElement(childLeft(el.index), 0));
			if (offset2 < bestDist)
				queue.push(SearchElement(childRight(el.index), offset2));
		}
		// TODO: optimise using loop there to relieve queue use

		++visitCount;
	}
	cerr << "visit count kdtree: " << visitCount << endl;
	return nodes[bestIndex].pos;
}

// brute force nearest neighbor for comparison
template<typename T>
typename KDTree<T>::Vector bfnn(const typename KDTree<T>::Vector& query, const typename KDTree<T>::Matrix data)
{
	T bestDist(numeric_limits<T>::max());
	size_t bestIndex(0);

	for (size_t i = 0; i < data.cols(); ++i)
	{
		const T dist(dist2<float>(data.col(i), query));
		if (dist < bestDist)
		{
			bestIndex = i;
			bestDist = dist;
		}
	}

	cerr << "visit count kdtree: " << data.cols() << endl;

	return data.col(bestIndex);
}

template<typename T>
typename KDTree<T>::Matrix load(const char *fileName)
{
	typedef typename KDTree<T>::Matrix Matrix;

	ifstream ifs(fileName);
	if (!ifs.good())
		throw runtime_error(string("Cannot open file ") + fileName);

	vector<T> data;
	int dim(0);
	bool firstLine(true);

	while (!ifs.eof())
	{
		char line[1024];
		ifs.getline(line, sizeof(line));
		line[sizeof(line)-1] = 0;

		char *token = strtok(line, " \t,;");
		while (token)
		{
			if (firstLine)
				++dim;
			data.push_back(atof(token));
			//cout << atof(token) << " ";
			token = strtok(NULL, " \t,;"); // FIXME: non reentrant, use strtok_r
		}
		firstLine = false;
	}

	return Matrix::Map(&data[0], dim, data.size() / dim);
}

int main(int argc, char* argv[])
{
	if (argc != 2)
	{
		cerr << "Usage " << argv[0] << " DATA" << endl;
		return 1;
	}

	/*cout << "<?xml version=\"1.0\" standalone=\"no\"?>" << endl;
	cout << "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">" << endl;

	cout << "<svg width=\"100%\" height=\"100%\" version=\"1.1\" xmlns=\"http://www.w3.org/2000/svg\">" << endl;
	*/

	KDTree<float>::Matrix d(load<float>(argv[1]));
	//cerr << d.rows() << " " << d.cols() << endl;
	KDTree<float> kdtree(d);
	typedef KDTree<float>::Vector Vector;

	for (int i = 0; i < 10; ++i)
	{
		Vector q(kdtree.minBound + Vector::Random(2).cwise() * (kdtree.maxBound - kdtree.minBound));
		Vector v_bf(bfnn<float>(q, d));
		Vector v_kdtree(kdtree.nn(q));
		cout << "query:\n" << q << "\nbf:\n" << v_bf << "\nkdtree:\n" << v_kdtree << "\n\n";
		assert(dist2(v_bf, v_kdtree) < numeric_limits<float>::epsilon());
	}

	//cout << "</svg>" << endl;

	return 0;
}<commit_msg>Fixed arbitrary dimensions.<commit_after>#include "Eigen/Eigen"
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <limits>
#include <vector>
#include <queue>
#include <algorithm>

using namespace std;

template<typename T>
T dist2(const typename Eigen::Matrix<T, Eigen::Dynamic, 1> v0, const typename Eigen::Matrix<T, Eigen::Dynamic, 1> v1)
{
	return (v0 - v1).squaredNorm();
}

template<typename T>
struct KDTree
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, 1> Vector;
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;

protected:
	struct BuildPoint
	{
		Vector pos;
		size_t index;
		BuildPoint(const Vector& pos =  Vector(), const size_t index = 0): pos(pos), index(index) {}
	};
	typedef vector<BuildPoint> BuildPoints;
	typedef typename BuildPoints::iterator BuildPointsIt;
	typedef typename BuildPoints::const_iterator BuildPointsCstIt;

	struct CompareDim
	{
		size_t dim;
		CompareDim(const size_t dim):dim(dim){}
		bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
	};

	struct SearchElement
	{
		size_t index;
		T minDist;

		SearchElement(const size_t index, const T minDist): index(index), minDist(minDist) {}
		friend bool operator<(const SearchElement& e0, const SearchElement& e1) { return e0.minDist < e1.minDist; }
	};

	// TODO: add index
	struct Node
	{
		Vector pos;
		int dim; // -1 == leaf, -2 == invalid
		Node(const Vector& pos = Vector(), const int dim = -2):pos(pos), dim(dim) {}
	};
	typedef vector<Node> Nodes;

	const size_t dim;
public:
	Vector minBound;
	Vector maxBound;
protected:
	Nodes nodes;

	inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
	inline size_t childRight(size_t pos) const { return 2*pos + 2; }
	inline size_t parent(size_t pos) const { return (pos-1)/2; }
	size_t getTreeSize(size_t size) const;
	size_t argMax(const Vector& v) const;
	void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos);
	void dump(const Vector minValues, const Vector maxValues, const size_t pos) const;

public:
	KDTree(const Matrix& cloud);
	Vector nn(const Vector& query);
};

template<typename T>
size_t KDTree<T>::getTreeSize(size_t elCount) const
{
	// FIXME: 64 bits safe stuff, only work for 2^32 elements right now
	size_t count = 0;
	int i = 31;
	for (; i >= 0; --i)
	{
		if (elCount & (1 << i))
			break;
	}
	for (int j = 0; j <= i; ++j)
		count |= (1 << j);
	//cerr << "tree size " << count << " (" << elCount << " elements)\n";
	return count;
}

template<typename T>
size_t KDTree<T>::argMax(const Vector& v) const
{
	T maxVal(0);
	size_t maxIdx;
	for (size_t i = 0; i < dim; ++i)
	{
		if (v[i] > maxVal)
		{
			maxVal = v[i];
			maxIdx = i;
		}
	}
	return maxIdx;
}

template<typename T>
void KDTree<T>::buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos)
{
	const size_t count(last - first);
	//cerr << count << endl;
	if (count == 1)
	{
		nodes[pos] = Node(first->pos, -1);
		return;
	}

	// estimate variance
	// get mean
	Vector mean(Vector::Zero(dim));
	for (BuildPointsCstIt it(first); it != last; ++it)
		mean += it->pos;
	mean /= last - first;
	// get sum of variance
	Vector var(Vector::Zero(dim));
	for (BuildPointsCstIt it(first); it != last; ++it)
		var += (it->pos - mean).cwise() * (it->pos - mean);
	// get dimension of maxmial variance
	const size_t cutDim = argMax(var);

	// sort
	sort(first, last, CompareDim(cutDim));

	// set node
	const size_t recurseCount(count-1);
	const size_t rightCount(recurseCount/2);
	const size_t leftCount(recurseCount-rightCount);
	assert(last - rightCount == first + leftCount + 1);

	nodes[pos] = Node((first+leftCount)->pos, cutDim);

	//cerr << pos << " cutting on " << cutDim << " at " << (first+leftCount)->pos[cutDim] << endl;

	// recurse
	if (count > 2)
	{
		buildNodes(first, first + leftCount, childLeft(pos));
		buildNodes(first + leftCount + 1, last, childRight(pos));
	}
	else
	{
		nodes[childLeft(pos)] = Node(first->pos, -1);
		nodes[childRight(pos)] = Node(Vector(), -2);
	}
}

template<typename T>
void KDTree<T>::dump(const Vector minValues, const Vector maxValues, const size_t pos) const
{
	const Node& node(nodes[pos]);

	if (node.dim >= -1)
	{
		if (dim == 2)
			cout << "<circle cx=\"" << 100*node.pos(0) << "\" cy=\"" << 100*node.pos(1) << "\" r=\"1\" stroke=\"black\" stroke-width=\"0.2\" fill=\"red\"/>" << endl;
		else
			cout << "pt at\n" << node.pos << endl;
	}
	if (node.dim >= 0)
	{
		//cerr << "in bounds:\n" << minValues << "\nto\n" << maxValues << endl;

		// update bounds for left
		Vector leftMaxValues(maxValues);
		leftMaxValues[node.dim] = node.pos[node.dim];
		// update bounds for right
		Vector rightMinValues(minValues);
		rightMinValues[node.dim] = node.pos[node.dim];

		// print line
		if (dim == 2)
			cout << "<line x1=\"" << 100*rightMinValues(0) << "\" y1=\"" << 100*rightMinValues(1) << "\" x2=\"" << 100*leftMaxValues(0) << "\" y2=\"" << 100*leftMaxValues(1) << "\" style=\"stroke:rgb(0,0,0);stroke-width:0.2\"/>" << endl;
		else
			cout << "cut from\n" << rightMinValues << "\nto\n" << leftMaxValues << endl;
		// recurs
		dump(minValues, leftMaxValues, childLeft(pos));
		dump(rightMinValues, maxValues, childRight(pos));
	}
}

template<typename T>
KDTree<T>::KDTree(const typename KDTree<T>::Matrix& cloud):
	dim(cloud.rows()),
	minBound(Vector::Constant(dim, numeric_limits<T>::max())),
	maxBound(Vector::Constant(dim, numeric_limits<T>::min()))
{
	// build point vector and compute bounds
	vector<BuildPoint> buildPoints;
	for (size_t i = 0; i < cloud.cols(); ++i)
	{
		const Vector& v(cloud.col(i));
		buildPoints.push_back(BuildPoint(v, i));
		minBound = minBound.cwise().min(v);
		maxBound = maxBound.cwise().max(v);
	}

	// create nodes
	nodes.resize(getTreeSize(cloud.cols()));
	buildNodes(buildPoints.begin(), buildPoints.end(), 0);

	// dump nodes
	//dump(minBound, maxBound, 0);
}

template<typename T>
typename KDTree<T>::Vector KDTree<T>::nn(const Vector& query)
{
	typedef priority_queue<SearchElement> Queue;
	Queue queue;

	queue.push(SearchElement(0, 0));

	T bestDist(numeric_limits<T>::max());
	size_t bestIndex(0);
	size_t visitCount(0);
	while (!queue.empty())
	{
		SearchElement el(queue.top());
		queue.pop();

		// nothing is closer, we found best
		if (el.minDist > bestDist)
			break;

		const Node& node(nodes[el.index]);
		// TODO: optimise, do not push these in first place
		if (node.dim == -2)
			continue;

		const T dist(dist2(node.pos, query));
		if (dist < bestDist)
		{
			bestIndex = el.index;
			bestDist = dist;
		}

		if (node.dim < 0)
			continue;

		const T offset(query[node.dim] - node.pos[node.dim]);
		const T offset2(offset * offset);
		if (offset > 0)
		{
			if (offset2 < bestDist)
				queue.push(SearchElement(childLeft(el.index), offset2));
			queue.push(SearchElement(childRight(el.index), 0));
		}
		else
		{
			queue.push(SearchElement(childLeft(el.index), 0));
			if (offset2 < bestDist)
				queue.push(SearchElement(childRight(el.index), offset2));
		}
		// TODO: optimise using loop there to relieve queue use

		++visitCount;
	}
	cerr << "visit count kdtree: " << visitCount << endl;
	return nodes[bestIndex].pos;
}

// brute force nearest neighbor for comparison
template<typename T>
typename KDTree<T>::Vector bfnn(const typename KDTree<T>::Vector& query, const typename KDTree<T>::Matrix data)
{
	T bestDist(numeric_limits<T>::max());
	size_t bestIndex(0);

	for (size_t i = 0; i < data.cols(); ++i)
	{
		const T dist(dist2<float>(data.col(i), query));
		if (dist < bestDist)
		{
			bestIndex = i;
			bestDist = dist;
		}
	}

	cerr << "visit count bf: " << data.cols() << endl;

	return data.col(bestIndex);
}

template<typename T>
typename KDTree<T>::Matrix load(const char *fileName)
{
	typedef typename KDTree<T>::Matrix Matrix;

	ifstream ifs(fileName);
	if (!ifs.good())
		throw runtime_error(string("Cannot open file ") + fileName);

	vector<T> data;
	int dim(0);
	bool firstLine(true);

	while (!ifs.eof())
	{
		char line[1024];
		ifs.getline(line, sizeof(line));
		line[sizeof(line)-1] = 0;

		char *token = strtok(line, " \t,;");
		while (token)
		{
			if (firstLine)
				++dim;
			data.push_back(atof(token));
			//cout << atof(token) << " ";
			token = strtok(NULL, " \t,;"); // FIXME: non reentrant, use strtok_r
		}
		firstLine = false;
	}

	return Matrix::Map(&data[0], dim, data.size() / dim);
}

int main(int argc, char* argv[])
{
	if (argc != 2)
	{
		cerr << "Usage " << argv[0] << " DATA" << endl;
		return 1;
	}

	/*cout << "<?xml version=\"1.0\" standalone=\"no\"?>" << endl;
	cout << "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">" << endl;

	cout << "<svg width=\"100%\" height=\"100%\" version=\"1.1\" xmlns=\"http://www.w3.org/2000/svg\">" << endl;
	*/

	KDTree<float>::Matrix d(load<float>(argv[1]));
	//cerr << d.rows() << " " << d.cols() << endl;
	KDTree<float> kdtree(d);
	typedef KDTree<float>::Vector Vector;

	for (int i = 0; i < 10; ++i)
	{
		Vector q(kdtree.minBound + Vector::Random(kdtree.minBound.size()).cwise() * (kdtree.maxBound - kdtree.minBound));
		Vector v_bf(bfnn<float>(q, d));
		Vector v_kdtree(kdtree.nn(q));
		cout << "query:\n" << q << "\nbf:\n" << v_bf << "\nkdtree:\n" << v_kdtree << "\n\n";
		assert(dist2(v_bf, v_kdtree) < numeric_limits<float>::epsilon());
	}

	//cout << "</svg>" << endl;

	return 0;
}<|endoftext|>


<commit_before>/*

   nsjail - logging
   -----------------------------------------

   Copyright 2014 Google Inc. All Rights Reserved.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

*/

#include "logs.h"

#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <limits.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/syscall.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#include "macros.h"
#include "util.h"

namespace logs {

static int _log_fd = STDERR_FILENO;
static bool _log_fd_isatty = true;
static enum llevel_t _log_level = INFO;
static bool _log_set = false;

static void setDupLogFdOr(int fd, int orfd) {
	int saved_errno = errno;
	_log_fd = fcntl(fd, F_DUPFD_CLOEXEC, 0);
	if (_log_fd == -1) {
		_log_fd = fcntl(orfd, F_DUPFD_CLOEXEC, 0);
	}
	if (_log_fd == -1) {
		_log_fd = orfd;
	}
	_log_fd_isatty = (isatty(_log_fd) == 1);
	errno = saved_errno;
}

/*
 * Log to stderr by default. Use a dup()d fd, because in the future we'll associate the
 * connection socket with fd (0, 1, 2).
 */
__attribute__((constructor)) static void log_init(void) {
	setDupLogFdOr(STDERR_FILENO, STDERR_FILENO);
}

bool logSet() {
	return _log_set;
}

void logLevel(enum llevel_t ll) {
	_log_level = ll;
}

void logFile(const std::string& log_file, int log_fd) {
	_log_set = true;
	int newlogfd = -1;
	if (!log_file.empty()) {
		newlogfd = TEMP_FAILURE_RETRY(
		    open(log_file.c_str(), O_CREAT | O_RDWR | O_APPEND | O_CLOEXEC, 0640));
		if (newlogfd == -1) {
			PLOG_W("Couldn't open('%s')", log_file.c_str());
		}
	}
	/* Close previous log_fd */
	if (_log_fd > STDERR_FILENO) {
		close(_log_fd);
	}
	setDupLogFdOr(newlogfd, log_fd);
	close(newlogfd);
}

void logMsg(enum llevel_t ll, const char* fn, int ln, bool perr, const char* fmt, ...) {
	if (ll < _log_level) {
		return;
	}

	char strerr[512];
	if (perr) {
		snprintf(strerr, sizeof(strerr), "%s", strerror(errno));
	}
	struct {
		const char* const descr;
		const char* const prefix;
		const bool print_funcline;
		const bool print_time;
	} static const logLevels[] = {
	    {"D", "\033[0;4m", true, true},
	    {"I", "\033[1m", false, true},
	    {"W", "\033[0;33m", true, true},
	    {"E", "\033[1;31m", true, true},
	    {"F", "\033[7;35m", true, true},
	    {"HR", "\033[0m", false, false},
	    {"HB", "\033[1m", false, false},
	};

	/* Start printing logs */
	std::string msg;
	if (_log_fd_isatty) {
		msg.append(logLevels[ll].prefix);
	}
	if (ll != HELP && ll != HELP_BOLD) {
		msg.append("[").append(logLevels[ll].descr).append("]");
	}
	if (logLevels[ll].print_time) {
		msg.append("[").append(util::timeToStr(time(NULL))).append("]");
	}
	if (logLevels[ll].print_funcline) {
		msg.append("[")
		    .append(std::to_string(getpid()))
		    .append("] ")
		    .append(fn)
		    .append("():")
		    .append(std::to_string(ln));
	}

	char* strp;
	va_list args;
	va_start(args, fmt);
	int ret = vasprintf(&strp, fmt, args);
	va_end(args);
	if (ret == -1) {
		msg.append(" [logs internal]: MEMORY ALLOCATION ERROR");
	} else {
		msg.append(" ").append(strp);
		free(strp);
	}
	if (perr) {
		msg.append(": ").append(strerr);
	}
	if (_log_fd_isatty) {
		msg.append("\033[0m");
	}
	msg.append("\n");
	/* End printing logs */

	if (write(_log_fd, msg.c_str(), msg.size()) == -1) {
		dprintf(_log_fd, "%s", msg.c_str());
	}

	if (ll == FATAL) {
		exit(0xff);
	}
}

void logStop(int sig) {
	LOG_I("Server stops due to fatal signal (%d) caught. Exiting", sig);
}

}  // namespace logs
<commit_msg>log: use TEMP_FAILURE_RETRY instead of fallback to dprintf<commit_after>/*

   nsjail - logging
   -----------------------------------------

   Copyright 2014 Google Inc. All Rights Reserved.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

*/

#include "logs.h"

#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <limits.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/syscall.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#include "macros.h"
#include "util.h"

namespace logs {

static int _log_fd = STDERR_FILENO;
static bool _log_fd_isatty = true;
static enum llevel_t _log_level = INFO;
static bool _log_set = false;

static void setDupLogFdOr(int fd, int orfd) {
	int saved_errno = errno;
	_log_fd = fcntl(fd, F_DUPFD_CLOEXEC, 0);
	if (_log_fd == -1) {
		_log_fd = fcntl(orfd, F_DUPFD_CLOEXEC, 0);
	}
	if (_log_fd == -1) {
		_log_fd = orfd;
	}
	_log_fd_isatty = (isatty(_log_fd) == 1);
	errno = saved_errno;
}

/*
 * Log to stderr by default. Use a dup()d fd, because in the future we'll associate the
 * connection socket with fd (0, 1, 2).
 */
__attribute__((constructor)) static void log_init(void) {
	setDupLogFdOr(STDERR_FILENO, STDERR_FILENO);
}

bool logSet() {
	return _log_set;
}

void logLevel(enum llevel_t ll) {
	_log_level = ll;
}

void logFile(const std::string& log_file, int log_fd) {
	_log_set = true;
	int newlogfd = -1;
	if (!log_file.empty()) {
		newlogfd = TEMP_FAILURE_RETRY(
		    open(log_file.c_str(), O_CREAT | O_RDWR | O_APPEND | O_CLOEXEC, 0640));
		if (newlogfd == -1) {
			PLOG_W("Couldn't open('%s')", log_file.c_str());
		}
	}
	/* Close previous log_fd */
	if (_log_fd > STDERR_FILENO) {
		close(_log_fd);
	}
	setDupLogFdOr(newlogfd, log_fd);
	close(newlogfd);
}

void logMsg(enum llevel_t ll, const char* fn, int ln, bool perr, const char* fmt, ...) {
	if (ll < _log_level) {
		return;
	}

	char strerr[512];
	if (perr) {
		snprintf(strerr, sizeof(strerr), "%s", strerror(errno));
	}
	struct {
		const char* const descr;
		const char* const prefix;
		const bool print_funcline;
		const bool print_time;
	} static const logLevels[] = {
	    {"D", "\033[0;4m", true, true},
	    {"I", "\033[1m", false, true},
	    {"W", "\033[0;33m", true, true},
	    {"E", "\033[1;31m", true, true},
	    {"F", "\033[7;35m", true, true},
	    {"HR", "\033[0m", false, false},
	    {"HB", "\033[1m", false, false},
	};

	/* Start printing logs */
	std::string msg;
	if (_log_fd_isatty) {
		msg.append(logLevels[ll].prefix);
	}
	if (ll != HELP && ll != HELP_BOLD) {
		msg.append("[").append(logLevels[ll].descr).append("]");
	}
	if (logLevels[ll].print_time) {
		msg.append("[").append(util::timeToStr(time(NULL))).append("]");
	}
	if (logLevels[ll].print_funcline) {
		msg.append("[")
		    .append(std::to_string(getpid()))
		    .append("] ")
		    .append(fn)
		    .append("():")
		    .append(std::to_string(ln));
	}

	char* strp;
	va_list args;
	va_start(args, fmt);
	int ret = vasprintf(&strp, fmt, args);
	va_end(args);
	if (ret == -1) {
		msg.append(" [logs internal]: MEMORY ALLOCATION ERROR");
	} else {
		msg.append(" ").append(strp);
		free(strp);
	}
	if (perr) {
		msg.append(": ").append(strerr);
	}
	if (_log_fd_isatty) {
		msg.append("\033[0m");
	}
	msg.append("\n");
	/* End printing logs */

	TEMP_FAILURE_RETRY(write(_log_fd, msg.c_str(), msg.size()));

	if (ll == FATAL) {
		exit(0xff);
	}
}

void logStop(int sig) {
	LOG_I("Server stops due to fatal signal (%d) caught. Exiting", sig);
}

}  // namespace logs
<|endoftext|>