<commit_before>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
 * This file is part of the LibreOffice project.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * This file incorporates work covered by the following license notice:
 *
 *   Licensed to the Apache Software Foundation (ASF) under one or more
 *   contributor license agreements. See the NOTICE file distributed
 *   with this work for additional information regarding copyright
 *   ownership. The ASF licenses this file to you 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 .
 */

#include <com/sun/star/awt/XWindow.hpp>
#include <com/sun/star/lang/XInitialization.hpp>
#include <com/sun/star/lang/XServiceInfo.hpp>
#include <com/sun/star/task/XInteractionHandler2.hpp>

#include <iahndl.hxx>
#include <comphelper/namedvaluecollection.hxx>
#include <cppuhelper/implbase3.hxx>
#include <cppuhelper/supportsservice.hxx>

using namespace com::sun::star;

namespace {

class UUIInteractionHandler:
    public cppu::WeakImplHelper3< com::sun::star::lang::XServiceInfo,
                                  com::sun::star::lang::XInitialization,
                                  com::sun::star::task::XInteractionHandler2 >
{
private:
    UUIInteractionHelper * m_pImpl;

    UUIInteractionHandler(UUIInteractionHandler &); // not implemented
    void operator =(UUIInteractionHandler); // not implemented

public:
    UUIInteractionHandler(com::sun::star::uno::Reference<
                  com::sun::star::uno::XComponentContext >
                  const & rxContext)
        SAL_THROW(());

    virtual ~UUIInteractionHandler() SAL_THROW(());

    virtual OUString SAL_CALL getImplementationName()
        throw (com::sun::star::uno::RuntimeException, std::exception) SAL_OVERRIDE;

    virtual sal_Bool SAL_CALL supportsService(OUString const &
                          rServiceName)
        throw (com::sun::star::uno::RuntimeException, std::exception) SAL_OVERRIDE;

    virtual com::sun::star::uno::Sequence< OUString > SAL_CALL
    getSupportedServiceNames()
        throw (com::sun::star::uno::RuntimeException, std::exception) SAL_OVERRIDE;

    virtual void SAL_CALL
    initialize(
        com::sun::star::uno::Sequence< com::sun::star::uno::Any > const &
            rArguments)
        throw (com::sun::star::uno::Exception, std::exception) SAL_OVERRIDE;

    virtual void SAL_CALL
    handle(com::sun::star::uno::Reference<
           com::sun::star::task::XInteractionRequest > const &
       rRequest)
        throw (com::sun::star::uno::RuntimeException, std::exception) SAL_OVERRIDE;

    virtual sal_Bool SAL_CALL
        handleInteractionRequest(
            const ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionRequest >& _Request
        )   throw ( ::com::sun::star::uno::RuntimeException, std::exception ) SAL_OVERRIDE;
};

UUIInteractionHandler::UUIInteractionHandler(
    uno::Reference< uno::XComponentContext > const &
        rxContext)
    SAL_THROW(())
        : m_pImpl(new UUIInteractionHelper(rxContext))
{
}

UUIInteractionHandler::~UUIInteractionHandler()
{
    delete m_pImpl;
}

OUString SAL_CALL UUIInteractionHandler::getImplementationName()
    throw (uno::RuntimeException, std::exception)
{
    return OUString("com.sun.star.comp.uui.UUIInteractionHandler");
}

sal_Bool SAL_CALL
UUIInteractionHandler::supportsService(OUString const & rServiceName)
    throw (uno::RuntimeException, std::exception)
{
    return cppu::supportsService(this, rServiceName);
}

uno::Sequence< OUString > SAL_CALL
UUIInteractionHandler::getSupportedServiceNames()
    throw (uno::RuntimeException, std::exception)
{
    uno::Sequence< OUString > aNames(3);
    aNames[0] = "com.sun.star.task.InteractionHandler";
    // added to indicate support for configuration.backend.MergeRecoveryRequest
    aNames[1] = "com.sun.star.configuration.backend.InteractionHandler";
    aNames[2] = "com.sun.star.uui.InteractionHandler";
    // for backwards compatibility
    return aNames;
}

void SAL_CALL
UUIInteractionHandler::initialize(
    uno::Sequence< uno::Any > const & rArguments)
    throw (uno::Exception, std::exception)
{
    uno::Reference<uno::XComponentContext> xContext = m_pImpl->getORB();
    delete m_pImpl;

    // The old-style InteractionHandler service supported a sequence of
    // PropertyValue, while the new-style service now uses constructors to pass
    // in Parent and Context values; for backwards compatibility, keep support
    // for a PropertyValue sequence, too:
    uno::Reference< awt::XWindow > xWindow;
    OUString aContext;
    if (!((rArguments.getLength() == 1 && (rArguments[0] >>= xWindow)) ||
          (rArguments.getLength() == 2 && (rArguments[0] >>= xWindow) &&
           (rArguments[1] >>= aContext))))
    {
        ::comphelper::NamedValueCollection aProperties( rArguments );
        if ( aProperties.has( "Parent" ) )
        {
            OSL_VERIFY( aProperties.get( "Parent" ) >>= xWindow );
        }
        if ( aProperties.has( "Context" ) )
        {
            OSL_VERIFY( aProperties.get( "Context" ) >>= aContext );
        }
    }

    m_pImpl = new UUIInteractionHelper(xContext, xWindow, aContext);
}

void SAL_CALL
UUIInteractionHandler::handle(
    uno::Reference< task::XInteractionRequest > const & rRequest)
    throw (uno::RuntimeException, std::exception)
{
    try
    {
        m_pImpl->handleRequest(rRequest);
    }
    catch (uno::RuntimeException const & ex)
    {
        throw uno::RuntimeException(ex.Message, *this);
    }
}

sal_Bool SAL_CALL UUIInteractionHandler::handleInteractionRequest(
    const uno::Reference< task::XInteractionRequest >& _Request ) throw ( uno::RuntimeException, std::exception )
{
    try
    {
        return m_pImpl->handleRequest( _Request );
    }
    catch (uno::RuntimeException const & ex)
    {
        throw uno::RuntimeException( ex.Message, *this );
    }
}

}

extern "C" SAL_DLLPUBLIC_EXPORT css::uno::XInterface * SAL_CALL
com_sun_star_comp_uui_UUIInteractionHandler_get_implementation(
    css::uno::XComponentContext *context,
    css::uno::Sequence<css::uno::Any> const &)
{
    return cppu::acquire(new UUIInteractionHandler(context));
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<commit_msg>Clean up function declarations<commit_after>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
 * This file is part of the LibreOffice project.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * This file incorporates work covered by the following license notice:
 *
 *   Licensed to the Apache Software Foundation (ASF) under one or more
 *   contributor license agreements. See the NOTICE file distributed
 *   with this work for additional information regarding copyright
 *   ownership. The ASF licenses this file to you 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 .
 */

#include <sal/config.h>

#include <boost/noncopyable.hpp>
#include <com/sun/star/awt/XWindow.hpp>
#include <com/sun/star/lang/XInitialization.hpp>
#include <com/sun/star/lang/XServiceInfo.hpp>
#include <com/sun/star/task/XInteractionHandler2.hpp>

#include <iahndl.hxx>
#include <comphelper/namedvaluecollection.hxx>
#include <cppuhelper/implbase3.hxx>
#include <cppuhelper/supportsservice.hxx>

using namespace com::sun::star;

namespace {

class UUIInteractionHandler:
    public cppu::WeakImplHelper3< com::sun::star::lang::XServiceInfo,
                                  com::sun::star::lang::XInitialization,
                                  com::sun::star::task::XInteractionHandler2 >,
    private boost::noncopyable
{
private:
    UUIInteractionHelper * m_pImpl;

public:
    UUIInteractionHandler(com::sun::star::uno::Reference<
                  com::sun::star::uno::XComponentContext >
                  const & rxContext)
        SAL_THROW(());

    virtual ~UUIInteractionHandler() SAL_THROW(());

    virtual OUString SAL_CALL getImplementationName()
        throw (com::sun::star::uno::RuntimeException, std::exception) SAL_OVERRIDE;

    virtual sal_Bool SAL_CALL supportsService(OUString const &
                          rServiceName)
        throw (com::sun::star::uno::RuntimeException, std::exception) SAL_OVERRIDE;

    virtual com::sun::star::uno::Sequence< OUString > SAL_CALL
    getSupportedServiceNames()
        throw (com::sun::star::uno::RuntimeException, std::exception) SAL_OVERRIDE;

    virtual void SAL_CALL
    initialize(
        com::sun::star::uno::Sequence< com::sun::star::uno::Any > const &
            rArguments)
        throw (com::sun::star::uno::Exception, std::exception) SAL_OVERRIDE;

    virtual void SAL_CALL
    handle(com::sun::star::uno::Reference<
           com::sun::star::task::XInteractionRequest > const &
       rRequest)
        throw (com::sun::star::uno::RuntimeException, std::exception) SAL_OVERRIDE;

    virtual sal_Bool SAL_CALL
        handleInteractionRequest(
            const ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionRequest >& _Request
        )   throw ( ::com::sun::star::uno::RuntimeException, std::exception ) SAL_OVERRIDE;
};

UUIInteractionHandler::UUIInteractionHandler(
    uno::Reference< uno::XComponentContext > const &
        rxContext)
    SAL_THROW(())
        : m_pImpl(new UUIInteractionHelper(rxContext))
{
}

UUIInteractionHandler::~UUIInteractionHandler()
{
    delete m_pImpl;
}

OUString SAL_CALL UUIInteractionHandler::getImplementationName()
    throw (uno::RuntimeException, std::exception)
{
    return OUString("com.sun.star.comp.uui.UUIInteractionHandler");
}

sal_Bool SAL_CALL
UUIInteractionHandler::supportsService(OUString const & rServiceName)
    throw (uno::RuntimeException, std::exception)
{
    return cppu::supportsService(this, rServiceName);
}

uno::Sequence< OUString > SAL_CALL
UUIInteractionHandler::getSupportedServiceNames()
    throw (uno::RuntimeException, std::exception)
{
    uno::Sequence< OUString > aNames(3);
    aNames[0] = "com.sun.star.task.InteractionHandler";
    // added to indicate support for configuration.backend.MergeRecoveryRequest
    aNames[1] = "com.sun.star.configuration.backend.InteractionHandler";
    aNames[2] = "com.sun.star.uui.InteractionHandler";
    // for backwards compatibility
    return aNames;
}

void SAL_CALL
UUIInteractionHandler::initialize(
    uno::Sequence< uno::Any > const & rArguments)
    throw (uno::Exception, std::exception)
{
    uno::Reference<uno::XComponentContext> xContext = m_pImpl->getORB();
    delete m_pImpl;

    // The old-style InteractionHandler service supported a sequence of
    // PropertyValue, while the new-style service now uses constructors to pass
    // in Parent and Context values; for backwards compatibility, keep support
    // for a PropertyValue sequence, too:
    uno::Reference< awt::XWindow > xWindow;
    OUString aContext;
    if (!((rArguments.getLength() == 1 && (rArguments[0] >>= xWindow)) ||
          (rArguments.getLength() == 2 && (rArguments[0] >>= xWindow) &&
           (rArguments[1] >>= aContext))))
    {
        ::comphelper::NamedValueCollection aProperties( rArguments );
        if ( aProperties.has( "Parent" ) )
        {
            OSL_VERIFY( aProperties.get( "Parent" ) >>= xWindow );
        }
        if ( aProperties.has( "Context" ) )
        {
            OSL_VERIFY( aProperties.get( "Context" ) >>= aContext );
        }
    }

    m_pImpl = new UUIInteractionHelper(xContext, xWindow, aContext);
}

void SAL_CALL
UUIInteractionHandler::handle(
    uno::Reference< task::XInteractionRequest > const & rRequest)
    throw (uno::RuntimeException, std::exception)
{
    try
    {
        m_pImpl->handleRequest(rRequest);
    }
    catch (uno::RuntimeException const & ex)
    {
        throw uno::RuntimeException(ex.Message, *this);
    }
}

sal_Bool SAL_CALL UUIInteractionHandler::handleInteractionRequest(
    const uno::Reference< task::XInteractionRequest >& _Request ) throw ( uno::RuntimeException, std::exception )
{
    try
    {
        return m_pImpl->handleRequest( _Request );
    }
    catch (uno::RuntimeException const & ex)
    {
        throw uno::RuntimeException( ex.Message, *this );
    }
}

}

extern "C" SAL_DLLPUBLIC_EXPORT css::uno::XInterface * SAL_CALL
com_sun_star_comp_uui_UUIInteractionHandler_get_implementation(
    css::uno::XComponentContext *context,
    css::uno::Sequence<css::uno::Any> const &)
{
    return cppu::acquire(new UUIInteractionHandler(context));
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<|endoftext|>


<commit_before><commit_msg>Add TODO comment for when liblangtag gets updated<commit_after><|endoftext|>


<commit_before>/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_GL_FILTERING_FILTER_BILATERAL_2DAS_HPP
#define PIC_GL_FILTERING_FILTER_BILATERAL_2DAS_HPP

#include "../../util/vec.hpp"
#include "../../util/std_util.hpp"
#include "../../gl/filtering/filter.hpp"
#include "../../gl/filtering/filter_sampling_map.hpp"
#include "../../gl/point_samplers/sampler_random_m.hpp"

namespace pic {

class FilterGLBilateral2DAS: public FilterGL
{
protected:
    float sigma_s, sigma_r;
    MRSamplersGL<2> *ms;

    //Random numbers tile
    ImageGL *imageRand;

    //Sampling map
    FilterGLSamplingMap *fGLsm;
    ImageGL *imgTmp;

    /**
     * @brief initShaders
     */
    void initShaders();

    /**
     * @brief FragmentShader
     */
    void FragmentShader();

public:

    /**
     * @brief FilterGLBilateral2DAS
     */
    FilterGLBilateral2DAS();

    ~FilterGLBilateral2DAS();

    /**
     * @brief releaseAux
     */
    void releaseAux()
    {
        delete_s(ms);
        delete_s(imageRand);
        delete_s(fGLsm);
        delete_s(imgTmp);
    }

    /**
     * @brief FilterGLBilateral2DAS
     * @param sigma_s
     * @param sigma_r
     */
    FilterGLBilateral2DAS(float sigma_s, float sigma_r);

    /**
     * @brief update
     * @param sigma_s
     * @param sigma_r
     */
    void update(float sigma_s, float sigma_r);

    /**
     * @brief Process
     * @param imgIn
     * @param imgOut
     * @return
     */
    ImageGL *Process(ImageGLVec imgIn, ImageGL *imgOut);

    /**
     * @brief execute
     * @param imgIn
     * @param sigma_s
     * @param sigma_r
     * @return
     */
    static ImageGL *execute(ImageGL *imgIn, float sigma_s, float sigma_r)
    {
        FilterGLBilateral2DAS *filter = new FilterGLBilateral2DAS(sigma_s, sigma_r);

        ImageGL *imgOut = filter->Process(SingleGL(imgIn), NULL);

        delete filter;
        return imgOut;
    }

};

PIC_INLINE FilterGLBilateral2DAS::FilterGLBilateral2DAS(): FilterGL()
{
    ms = NULL;
    imageRand = NULL;
    fGLsm = NULL;
    imgTmp = NULL;
}

PIC_INLINE FilterGLBilateral2DAS::~FilterGLBilateral2DAS()
{
    release();
}

PIC_INLINE FilterGLBilateral2DAS::FilterGLBilateral2DAS(float sigma_s,
        float sigma_r): FilterGL()
{
    //protected values are assigned/computed
    this->sigma_s = sigma_s;
    this->sigma_r = sigma_r;

    fGLsm = new FilterGLSamplingMap(sigma_s);
    imgTmp = NULL;

    int nRand = 32;

    Image tmp_image_rand(1, 128, 128, 1);
    tmp_image_rand.setRand(1);
    tmp_image_rand *= float(nRand - 1);

    imageRand = new ImageGL(&tmp_image_rand, true);
    imageRand->generateTextureGL(GL_TEXTURE_2D, GL_INT, false);

    //Precomputation of the Gaussian Kernel
    int kernelSize = PrecomputedGaussian::getKernelSize(sigma_s);
    int halfKernelSize = kernelSize >> 1;

    //Poisson samples
#ifdef PIC_DEBUG
    printf("Window: %d\n", halfKernelSize);
#endif

    Vec2i window = Vec2i(halfKernelSize, halfKernelSize);
    ms = new MRSamplersGL<2>(ST_BRIDSON, window, halfKernelSize, 3, nRand);

    ms->generateTexture();
    ms->generateLevelsRTexture();

    FragmentShader();
    initShaders();
}

PIC_INLINE void FilterGLBilateral2DAS::FragmentShader()
{
    fragment_source = MAKE_STRING
                      (
                          uniform sampler2D	u_tex;
                          uniform isampler2D	u_poisson;
                          uniform sampler2D	u_sample;
                          uniform isampler2D	u_rand;
                          uniform isampler2D	u_levelsR;
                          uniform float		sigmas2;
                          uniform float		sigmar2;
                          uniform int           levelsR_Size;
                          out     vec4          f_color;

                          //Calculate the number of samples
    int CalculateSamples(int shifter, ivec2 tSize) {
        //Fetch to the sampling map
        float levelVal = dot(texture(u_sample, gl_FragCoord.xy / tSize.xy).xyz, vec3(1.0)) / 3.0;
        levelVal = clamp(1.0f - levelVal, 0.0, 1.0) * float(levelsR_Size);

        int levelInt = int(floor(levelVal));

        int nSamples = texelFetch(u_levelsR, ivec2(levelInt, shifter), 0).x;

        if(levelInt < (levelsR_Size - 1)) {
            float tmp = (levelVal - float(levelInt));

            if(tmp > 0.0) {
                int nSamples1 = texelFetch(u_levelsR, ivec2(levelInt + 1, shifter), 0).x;

                nSamples += int(float(nSamples1 - nSamples) * tmp);
            }
        }

        return nSamples / 2;
    }

    void main(void) {
        ivec2 coordsFrag = ivec2(gl_FragCoord.xy);

        //Coordinates
        int shifter = texelFetch(u_rand, coordsFrag.xy % 128, 0).x;

        //Calculating the number of samples
        ivec2 tSize =  textureSize(u_tex, 0);

        int nSamples = CalculateSamples(shifter, tSize);

        //Filtering
        vec3 tmpCol;
        vec3 colRef = texelFetch(u_tex, coordsFrag, 0).xyz;
        vec3  color = vec3(0.0);
        float weight = 0.0;

        for(int i = 0; i < nSamples; i++) {
            ivec4 coords = texelFetch(u_poisson, ivec2(i, shifter), 0);

            //Texture fetch
            tmpCol = texelFetch(u_tex, coordsFrag.xy + coords.xy, 0).xyz;
            vec3 tmpCol2 = tmpCol - colRef;
            float dstR = dot(tmpCol2.xyz, tmpCol2.xyz);
            float tmp = exp(-dstR / sigmar2 - float(coords.z) / sigmas2);
            color += tmpCol * tmp;
            weight += tmp;
        }

        color = weight > 0.0 ? color / weight : colRef;
        f_color = vec4(color, 1.0);
    }
                      );
}

PIC_INLINE void FilterGLBilateral2DAS::initShaders()
{
#ifdef PIC_DEBUG
    printf("Number of samples: %d\n", ms->nSamples / 2);
#endif

    technique.initStandard("330", vertex_source, fragment_source, "FilterGLBilateral2DAS");

    update(-1.0f, -1.0f);
}

PIC_INLINE void FilterGLBilateral2DAS::update(float sigma_s, float sigma_r)
{
    bool flag = false;

    if(sigma_s > 0.0f) {
        flag = (this->sigma_s == sigma_s);
        this->sigma_s = sigma_s;
    }

    if(sigma_r > 0.0f) {
        flag = flag || (this->sigma_r == sigma_r);
        this->sigma_r = sigma_r;
    }

    if(!flag) {
    }

    //shader update
    int kernelSize = PrecomputedGaussian::getKernelSize(this->sigma_s);
    int halfKernelSize = kernelSize >> 1;

    Vec2i window = Vec2i(halfKernelSize, halfKernelSize);
    ms->updateGL(window, halfKernelSize);

    //shader update
    float sigmas2 = 2.0f * this->sigma_s * this->sigma_s;
    float sigmar2 = 2.0f * this->sigma_r * this->sigma_r;

    technique.bind();
    technique.setUniform1i("u_tex",      0);
    technique.setUniform1i("u_poisson",  1);
    technique.setUniform1i("u_rand",	 2);
    technique.setUniform1i("u_sample",	 3);
    technique.setUniform1i("u_levelsR",	 4);
    technique.setUniform1f("sigmas2",  sigmas2);
    technique.setUniform1f("sigmar2",  sigmar2);
    technique.setUniform1i("levelsR_Size", ms->nLevels);
    technique.unbind();
}

PIC_INLINE ImageGL *FilterGLBilateral2DAS::Process(ImageGLVec imgIn,
        ImageGL *imgOut)
{
    if(imgIn[0] == NULL) {
        return imgOut;
    }

    int w = imgIn[0]->width;
    int h = imgIn[0]->height;

    if(imgOut == NULL) {
        imgOut = new ImageGL(1, w, h, imgIn[0]->channels, IMG_GPU, GL_TEXTURE_2D);
    }

    if(fbo == NULL) {
        fbo = new Fbo();
        fbo->create(w, h, 1, false, imgOut->getTexture());
    }

    if(imgTmp == NULL) {
        float scale = fGLsm->getScale();
        imgTmp = new ImageGL(    1,
                                    int(imgIn[0]->widthf  * scale),
                                    int(imgIn[0]->heightf * scale),
                                    1, IMG_GPU, GL_TEXTURE_2D);
    }

    ImageGL *edge, *base;

    if(imgIn.size() == 2) {
        //Joint/Cross Bilateral Filtering
        base = imgIn[0];
        edge = imgIn[1];
    } else {
        base = imgIn[0];
        edge = imgIn[0];
    }

    //Calculating the sampling map
    imgTmp = fGLsm->Process(imgIn, imgTmp);

    //Rendering
    fbo->bind();
    glViewport(0, 0, (GLsizei)w, (GLsizei)h);

    //Shaders
    technique.bind();

    //Textures
    glActiveTexture(GL_TEXTURE4);
    glBindTexture(GL_TEXTURE_2D, ms->getLevelsRTexture());

    glActiveTexture(GL_TEXTURE3);
    imgTmp->bindTexture();

    glActiveTexture(GL_TEXTURE2);
    imageRand->bindTexture();

    glActiveTexture(GL_TEXTURE1);
    glBindTexture(GL_TEXTURE_2D, ms->getTexture());

    glActiveTexture(GL_TEXTURE0);
    base->bindTexture();

    //Rendering aligned quad
    quad->Render();

    //Fbo
    fbo->unbind();

    //Shaders
    technique.unbind();

    //Textures
    glActiveTexture(GL_TEXTURE4);
    glBindTexture(GL_TEXTURE_2D, 0);

    glActiveTexture(GL_TEXTURE3);
    imgTmp->unBindTexture();

    glActiveTexture(GL_TEXTURE2);
    imageRand->unBindTexture();

    glActiveTexture(GL_TEXTURE1);
    glBindTexture(GL_TEXTURE_2D, 0);

    glActiveTexture(GL_TEXTURE0);
    base->unBindTexture();

    return imgOut;
}

} // end namespace pic

#endif /* PIC_GL_FILTERING_FILTER_BILATERAL_2DAS_HPP */

<commit_msg>Update filter_bilateral_2das.hpp<commit_after>/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_GL_FILTERING_FILTER_BILATERAL_2DAS_HPP
#define PIC_GL_FILTERING_FILTER_BILATERAL_2DAS_HPP

#include "../../util/vec.hpp"
#include "../../util/std_util.hpp"
#include "../../gl/filtering/filter.hpp"
#include "../../gl/filtering/filter_sampling_map.hpp"
#include "../../gl/point_samplers/sampler_random_m.hpp"

namespace pic {

class FilterGLBilateral2DAS: public FilterGL
{
protected:
    float sigma_s, sigma_r;
    MRSamplersGL<2> *ms;

    //Random numbers tile
    ImageGL *imageRand;

    //Sampling map
    FilterGLSamplingMap *fGLsm;
    ImageGL *imgTmp;

    /**
     * @brief initShaders
     */
    void initShaders();

    /**
     * @brief FragmentShader
     */
    void FragmentShader();

public:

    /**
     * @brief FilterGLBilateral2DAS
     */
    FilterGLBilateral2DAS();

    ~FilterGLBilateral2DAS();

    /**
     * @brief releaseAux
     */
    void releaseAux()
    {
        delete_s(ms);
        delete_s(imageRand);
        delete_s(fGLsm);
        delete_s(imgTmp);
    }

    /**
     * @brief FilterGLBilateral2DAS
     * @param sigma_s
     * @param sigma_r
     */
    FilterGLBilateral2DAS(float sigma_s, float sigma_r);

    /**
     * @brief update
     * @param sigma_s
     * @param sigma_r
     */
    void update(float sigma_s, float sigma_r);

    /**
     * @brief Process
     * @param imgIn
     * @param imgOut
     * @return
     */
    ImageGL *Process(ImageGLVec imgIn, ImageGL *imgOut);

    /**
     * @brief execute
     * @param imgIn
     * @param sigma_s
     * @param sigma_r
     * @return
     */
    static ImageGL *execute(ImageGL *imgIn, float sigma_s, float sigma_r)
    {
        FilterGLBilateral2DAS *filter = new FilterGLBilateral2DAS(sigma_s, sigma_r);

        ImageGL *imgOut = filter->Process(SingleGL(imgIn), NULL);

        delete filter;
        return imgOut;
    }

};

PIC_INLINE FilterGLBilateral2DAS::FilterGLBilateral2DAS(): FilterGL()
{
    ms = NULL;
    imageRand = NULL;
    fGLsm = NULL;
    imgTmp = NULL;
}

PIC_INLINE FilterGLBilateral2DAS::~FilterGLBilateral2DAS()
{
    release();
}

PIC_INLINE FilterGLBilateral2DAS::FilterGLBilateral2DAS(float sigma_s,
        float sigma_r): FilterGL()
{
    //protected values are assigned/computed
    this->sigma_s = sigma_s;
    this->sigma_r = sigma_r;

    fGLsm = new FilterGLSamplingMap(sigma_s);
    imgTmp = NULL;

    int nRand = 32;

    Image tmp_image_rand(1, 128, 128, 1);
    tmp_image_rand.setRand(1);
    tmp_image_rand *= float(nRand - 1);

    imageRand = new ImageGL(&tmp_image_rand, true);
    imageRand->generateTextureGL(GL_TEXTURE_2D, GL_INT, false);

    //Precomputation of the Gaussian Kernel
    int kernelSize = PrecomputedGaussian::getKernelSize(sigma_s);
    int halfKernelSize = kernelSize >> 1;

    //Poisson samples
#ifdef PIC_DEBUG
    printf("Window: %d\n", halfKernelSize);
#endif

    Vec2i window = Vec2i(halfKernelSize, halfKernelSize);
    ms = new MRSamplersGL<2>(ST_BRIDSON, window, halfKernelSize, 3, nRand);

    ms->generateTexture();
    ms->generateLevelsRTexture();

    FragmentShader();
    initShaders();
}

PIC_INLINE void FilterGLBilateral2DAS::FragmentShader()
{
    fragment_source = MAKE_STRING
                      (
                          uniform sampler2D	u_tex;
                          uniform isampler2D	u_poisson;
                          uniform sampler2D	u_sample;
                          uniform isampler2D	u_rand;
                          uniform isampler2D	u_levelsR;
                          uniform float		sigmas2;
                          uniform float		sigmar2;
                          uniform int           levelsR_Size;
                          out     vec4          f_color;

                          //Calculate the number of samples
    int CalculateSamples(int shifter, ivec2 tSize) {
        //Fetch to the sampling map
        float levelVal = dot(texture(u_sample, gl_FragCoord.xy / tSize.xy).xyz, vec3(1.0)) / 3.0;
        levelVal = clamp(1.0f - levelVal, 0.0, 1.0) * float(levelsR_Size);

        int levelInt = int(floor(levelVal));

        int nSamples = texelFetch(u_levelsR, ivec2(levelInt, shifter), 0).x;

        if(levelInt < (levelsR_Size - 1)) {
            float tmp = (levelVal - float(levelInt));

            if(tmp > 0.0) {
                int nSamples1 = texelFetch(u_levelsR, ivec2(levelInt + 1, shifter), 0).x;

                nSamples += int(float(nSamples1 - nSamples) * tmp);
            }
        }

        return nSamples / 2;
    }

    void main(void) {
        ivec2 coordsFrag = ivec2(gl_FragCoord.xy);

        //Coordinates
        int shifter = texelFetch(u_rand, coordsFrag.xy % 128, 0).x;

        //Calculating the number of samples
        ivec2 tSize =  textureSize(u_tex, 0);

        int nSamples = CalculateSamples(shifter, tSize);

        //Filtering
        vec3 tmpCol;
        vec3 colRef = texelFetch(u_tex, coordsFrag, 0).xyz;
        vec3  color = vec3(0.0);
        float weight = 0.0;

        for(int i = 0; i < nSamples; i++) {
            ivec4 coords = texelFetch(u_poisson, ivec2(i, shifter), 0);

            //Texture fetch
            tmpCol = texelFetch(u_tex, coordsFrag.xy + coords.xy, 0).xyz;
            vec3 tmpCol2 = tmpCol - colRef;
            float dstR = dot(tmpCol2.xyz, tmpCol2.xyz);
            float tmp = exp(-dstR / sigmar2 - float(coords.z) / sigmas2);
            color += tmpCol * tmp;
            weight += tmp;
        }

        color = weight > 0.0 ? color / weight : colRef;
        f_color = vec4(color, 1.0);
    }
                      );
}

PIC_INLINE void FilterGLBilateral2DAS::initShaders()
{
#ifdef PIC_DEBUG
    printf("Number of samples: %d\n", ms->nSamples / 2);
#endif

    technique.initStandard("330", vertex_source, fragment_source, "FilterGLBilateral2DAS");

    update(-1.0f, -1.0f);
}

PIC_INLINE void FilterGLBilateral2DAS::update(float sigma_s, float sigma_r)
{
    bool flag = false;

    if(sigma_s > 0.0f) {
        flag = (this->sigma_s == sigma_s);
        this->sigma_s = sigma_s;
    }

    if(sigma_r > 0.0f) {
        flag = flag || (this->sigma_r == sigma_r);
        this->sigma_r = sigma_r;
    }

    if(!flag) {
    }

    //shader update
    int kernelSize = PrecomputedGaussian::getKernelSize(this->sigma_s);
    int halfKernelSize = kernelSize >> 1;

    Vec2i window = Vec2i(halfKernelSize, halfKernelSize);
    ms->updateGL(window, halfKernelSize);

    //shader update
    float sigmas2 = 2.0f * this->sigma_s * this->sigma_s;
    float sigmar2 = 2.0f * this->sigma_r * this->sigma_r;

    technique.bind();
    technique.setUniform1i("u_tex",      0);
    technique.setUniform1i("u_poisson",  1);
    technique.setUniform1i("u_rand",	 2);
    technique.setUniform1i("u_sample",	 3);
    technique.setUniform1i("u_levelsR",	 4);
    technique.setUniform1f("sigmas2",  sigmas2);
    technique.setUniform1f("sigmar2",  sigmar2);
    technique.setUniform1i("levelsR_Size", ms->nLevels);
    technique.unbind();
}

PIC_INLINE ImageGL *FilterGLBilateral2DAS::Process(ImageGLVec imgIn,
        ImageGL *imgOut)
{
    if(imgIn[0] == NULL) {
        return imgOut;
    }

    int w = imgIn[0]->width;
    int h = imgIn[0]->height;

    if(imgOut == NULL) {
        imgOut = new ImageGL(1, w, h, imgIn[0]->channels, IMG_GPU, GL_TEXTURE_2D);
    }

    if(fbo == NULL) {
        fbo = new Fbo();
        fbo->create(w, h, 1, false, imgOut->getTexture());
    }

    ImageGL *edge, *base;

    if(imgIn.size() == 2) {
        //Joint/Cross Bilateral Filtering
        base = imgIn[0];
        edge = imgIn[1];
    } else {
        base = imgIn[0];
        edge = imgIn[0];
    }

    //Calculating the sampling map
    imgTmp = fGLsm->Process(imgIn, imgTmp);

    //Rendering
    fbo->bind();
    glViewport(0, 0, (GLsizei)w, (GLsizei)h);

    //Shaders
    technique.bind();

    //Textures
    glActiveTexture(GL_TEXTURE4);
    glBindTexture(GL_TEXTURE_2D, ms->getLevelsRTexture());

    glActiveTexture(GL_TEXTURE3);
    imgTmp->bindTexture();

    glActiveTexture(GL_TEXTURE2);
    imageRand->bindTexture();

    glActiveTexture(GL_TEXTURE1);
    glBindTexture(GL_TEXTURE_2D, ms->getTexture());

    glActiveTexture(GL_TEXTURE0);
    base->bindTexture();

    //Rendering aligned quad
    quad->Render();

    //Fbo
    fbo->unbind();

    //Shaders
    technique.unbind();

    //Textures
    glActiveTexture(GL_TEXTURE4);
    glBindTexture(GL_TEXTURE_2D, 0);

    glActiveTexture(GL_TEXTURE3);
    imgTmp->unBindTexture();

    glActiveTexture(GL_TEXTURE2);
    imageRand->unBindTexture();

    glActiveTexture(GL_TEXTURE1);
    glBindTexture(GL_TEXTURE_2D, 0);

    glActiveTexture(GL_TEXTURE0);
    base->unBindTexture();

    return imgOut;
}

} // end namespace pic

#endif /* PIC_GL_FILTERING_FILTER_BILATERAL_2DAS_HPP */

<|endoftext|>


<commit_before>//
// Copyright (c) 2013 Christoph Malek
// See LICENSE for more information.
//

#ifndef RJ_GAME_COMPONENTS_PLATFORM_HPP
#define RJ_GAME_COMPONENTS_PLATFORM_HPP

#include <rectojump/game/entity_rect.hpp>

namespace rj
{
	class platform : public entity_rect
	{
	protected:
		void update(dur duration) override
		{m_render_object.move(m_velocity * duration);}

		void init() override
		{ }

	public:
		platform(const sf::Vector2f& pos, const sf::Vector2f& size = {20.f, 20.f}) :
			entity_rect{pos, size, {-0.3f, 0.f}}
		{m_render_object.setOrigin(size.x / 2, size.y / 2);}

		~platform() = default;
	};
}

#endif // RJ_GAME_COMPONENTS_PLATFORM_HPP
<commit_msg>cleanup platform<commit_after>//
// Copyright (c) 2013 Christoph Malek
// See LICENSE for more information.
//

#ifndef RJ_GAME_COMPONENTS_PLATFORM_HPP
#define RJ_GAME_COMPONENTS_PLATFORM_HPP

#include <rectojump/game/entity_rect.hpp>

namespace rj
{
	class platform : public entity_rect
	{
	public:
		platform(const sf::Vector2f& pos, const sf::Vector2f& size = {20.f, 20.f}) :
			entity_rect{pos, size, {-0.3f, 0.f}}
		{m_render_object.setOrigin(size.x / 2, size.y / 2);}

		~platform() = default;

	private:
		void update(dur duration) override
		{m_render_object.move(m_velocity * duration);}
	};
}

#endif // RJ_GAME_COMPONENTS_PLATFORM_HPP
<|endoftext|>


<commit_before>#include <catch.hpp>
#include <rapidcheck-catch.h>

#include "rapidcheck/Gen.h"
#include "rapidcheck/shrinkable/Create.h"

#include "util/ArbitraryRandom.h"

using namespace rc;
using namespace rc::detail;
using namespace rc::newgen::detail;

struct MockGenerationHandler : public GenerationHandler
{
    Any onGenerate(const Gen<Any> &gen) override
    {
        wasCalled = true;
        passedGenerator = gen;
        return returnValue;
    }

    bool wasCalled = false;
    Gen<Any> passedGenerator = Gen<Any>(
        fn::constant(shrinkable::just(Any::of(0))));
    Any returnValue;
};

TEST_CASE("Gen") {
    SECTION("operator()") {
        prop("passes the arguments to the functor",
             [](const Random &random, int size) {
                 bool called = false;
                 Random passedRandom;
                 int passedSize;
                 Gen<int> gen([&](const Random &random, int size) {
                     called = true;
                     passedRandom = random;
                     passedSize = size;
                     return shrinkable::just(0);
                 });

                 gen(random, size);
                 RC_ASSERT(called);
                 RC_ASSERT(passedRandom == random);
                 RC_ASSERT(passedSize == size);
             });

        prop("returns the value returned by the functor",
             [](const Random &random, int size, int x) {
                 Gen<int> gen([=](const Random &random, int size) {
                     return shrinkable::just(x);
                 });

                 RC_ASSERT(gen(random, size) == shrinkable::just(x));
             });

        prop("if exception is thrown in generation function, shrinkable is"
             " returned that rethrows the exception on call to value()",
             [](const std::string &message) {
                 Gen<int> gen([=](const Random &random, int size) -> Shrinkable<int> {
                     throw GenerationFailure(message);
                 });

                 const auto shrinkable = gen(Random(), 0);
                 try {
                     shrinkable.value();
                 } catch (const GenerationFailure &e) {
                     RC_ASSERT(e.what() == message);
                     RC_SUCCEED("Threw correct exception");
                 }
                 RC_FAIL("Did not throw correct exception");
             });
    }

    SECTION("operator*") {
        ImplicitScope scope;

        SECTION("by default throws exception") {
            Gen<int> gen(fn::constant(shrinkable::just(0)));
            REQUIRE_THROWS(*gen);
        }

        MockGenerationHandler handler;
        handler.returnValue = Any::of(456);
        ImplicitParam<rc::newgen::detail::param::CurrentHandler> letHandler(
            &handler);
        Gen<int> gen(fn::constant(shrinkable::just(1337)));
        int x = *gen;

        SECTION("passes erased self to onGenerate") {
            auto result = shrinkable::map(
                handler.passedGenerator(Random(), 0),
                [](Any &&any) { return std::move(any.get<int>()); });
            REQUIRE(result == shrinkable::just(1337));
            REQUIRE(handler.wasCalled);
        }

        SECTION("returns what is returned by onGenerate") {
            RC_ASSERT(x == 456);
        }
    }
}
<commit_msg>Move GenTests to new framework<commit_after>#include <catch.hpp>
#include <rapidcheck-catch.h>

#include "rapidcheck/Gen.h"
#include "rapidcheck/shrinkable/Create.h"

#include "util/ArbitraryRandom.h"

using namespace rc;
using namespace rc::detail;
using namespace rc::newgen::detail;

struct MockGenerationHandler : public GenerationHandler
{
    Any onGenerate(const Gen<Any> &gen) override
    {
        wasCalled = true;
        passedGenerator = gen;
        return returnValue;
    }

    bool wasCalled = false;
    Gen<Any> passedGenerator = Gen<Any>(
        fn::constant(shrinkable::just(Any::of(0))));
    Any returnValue;
};

TEST_CASE("Gen") {
    SECTION("operator()") {
        newprop(
            "passes the arguments to the functor",
            [](const Random &random, int size) {
                bool called = false;
                Random passedRandom;
                int passedSize;
                Gen<int> gen([&](const Random &random, int size) {
                    called = true;
                    passedRandom = random;
                    passedSize = size;
                    return shrinkable::just(0);
                });

                gen(random, size);
                RC_ASSERT(called);
                RC_ASSERT(passedRandom == random);
                RC_ASSERT(passedSize == size);
            });

        newprop(
            "returns the value returned by the functor",
            [](const Random &random, int size, int x) {
                Gen<int> gen([=](const Random &random, int size) {
                    return shrinkable::just(x);
                });

                RC_ASSERT(gen(random, size) == shrinkable::just(x));
            });

        newprop(
            "if exception is thrown in generation function, shrinkable is"
            " returned that rethrows the exception on call to value()",
            [](const std::string &message) {
                Gen<int> gen([=](const Random &random, int size) -> Shrinkable<int> {
                    throw GenerationFailure(message);
                });

                const auto shrinkable = gen(Random(), 0);
                try {
                    shrinkable.value();
                } catch (const GenerationFailure &e) {
                    RC_ASSERT(e.what() == message);
                    RC_SUCCEED("Threw correct exception");
                }
                RC_FAIL("Did not throw correct exception");
            });
    }

    SECTION("operator*") {
        ImplicitScope scope;

        SECTION("by default throws exception") {
            Gen<int> gen(fn::constant(shrinkable::just(0)));
            REQUIRE_THROWS(*gen);
        }

        MockGenerationHandler handler;
        handler.returnValue = Any::of(456);
        ImplicitParam<rc::newgen::detail::param::CurrentHandler> letHandler(
            &handler);
        Gen<int> gen(fn::constant(shrinkable::just(1337)));
        int x = *gen;

        SECTION("passes erased self to onGenerate") {
            auto result = shrinkable::map(
                handler.passedGenerator(Random(), 0),
                [](Any &&any) { return std::move(any.get<int>()); });
            REQUIRE(result == shrinkable::just(1337));
            REQUIRE(handler.wasCalled);
        }

        SECTION("returns what is returned by onGenerate") {
            RC_ASSERT(x == 456);
        }
    }
}
<|endoftext|>


<commit_before>/*
 * Open Source Movement Analysis Library
 * Copyright (C) 2016, Moveck Solution 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(s) of the copyright holders 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 "openma/body/inversedynamicsmatrix.h"
#include "openma/body/inversedynamicsprocessor_p.h"

#include "openma/body/anchor.h"
#include "openma/body/chain.h"
#include "openma/body/inertialparameters.h"
#include "openma/body/joint.h"
#include "openma/body/model.h"
#include "openma/body/segment.h"
#include "openma/body/utils.h"
#include "openma/base/trial.h"
#include "openma/instrument/forceplate.h"
#include "openma/math.h"

// -------------------------------------------------------------------------- //
//                                 PRIVATE API                                //
// -------------------------------------------------------------------------- //

#ifndef DOXYGEN_SHOULD_SKIP_THIS

namespace ma
{
namespace body
{
  class InverseDynamicMatrixPrivate : public InverseDynamicProcessorPrivate
  {
    OPENMA_DECLARE_PINT_ACCESSOR(InverseDynamicMatrix)

  public:
    InverseDynamicMatrixPrivate(InverseDynamicMatrix* pint);
    ~InverseDynamicMatrixPrivate();
  };

  InverseDynamicMatrixPrivate::InverseDynamicMatrixPrivate(InverseDynamicMatrix* pint)
  : InverseDynamicProcessorPrivate(pint,"InverseDynamicMatrix")
  {};

  InverseDynamicMatrixPrivate::~InverseDynamicMatrixPrivate() = default;
};
};

#endif

// -------------------------------------------------------------------------- //
//                                 PUBLIC API                                 //
// -------------------------------------------------------------------------- //

OPENMA_INSTANCE_STATIC_TYPEID(ma::body::InverseDynamicMatrix);

namespace ma
{
namespace body
{
  /**
   * @class InverseDynamicMatrix openma/body/matrixinversedynamics.h
   * @brief Algorithm to compute joint kinetics expressed in the global frame
   *
   * @TODO Explain in details the required element to use this algorithm (model configure, external wrench, etc.).
   *
   * @ingroup openma_body
   */

  /**
   * Constructor
   */
  InverseDynamicMatrix::InverseDynamicMatrix(Node* parent)
  : InverseDynamicProcessor(*new InverseDynamicMatrixPrivate(this), parent)
  {};

  /**
   * Destructor
   */
  InverseDynamicMatrix::~InverseDynamicMatrix() _OPENMA_NOEXCEPT = default;

  /**
   * Internally this method is a combination of two algorithms published in the litterature [1,2].
   * Both of these algorithms are known to be less sensitive to noise [3]. The reason to use this is only to reduce computation time.
   *  - No need to convert to quaternion
   *  - No need to create sparse 6x6 matrices
   *  - Angular velocity and angular acceleration computed directly from rotation matrices
   *  - All the results are express in the inertial (global) coordinate system
   *
   * @par References
   *  1. Dumas et al., 2004, A 3D generic inverse dynamic method using wrench notation and quaternion algebra, Computer methods in Biomechanics and Biomedical Engineering, 7 (3), pp. 159-166
   *  2. Doriot & Chèze, 2004, A Three-Dimensional Kinematic and Dynamic Study of the Lower Limb During the Stance Phase of Gait Using an Homogeneous Matrix Approach, IEEE Transactions on Biomedical Engineering, 51 (1), pp. 21-27
   *  3. Dumas et al., 2007, Influence of the 3D Inverse Dynamic Method on the Joint Forces and Moments During Gait, ￼Journal of￼Biomechanical￼Engineering, 129, pp. 786-790
   */
  bool InverseDynamicMatrix::run(Node* inout)
  {
#if !defined(_MSC_VER)
#warning WE NEED TO MANAGE UNITS FOR FORCES AND MOMENTS
#endif

    auto models = inout->findChildren<Model*>({},{},false);
    for (auto& model : models)
    {
      auto chains = model->chains()->findChildren<Chain*>({},{},false);
      for (auto& chain : chains)
      {
        auto joints = chain->findChildren<Joint*>({},{},false);
        if (joints.empty())
        {
          warning("No joint found in the chain '%s'. Inverse dynamics for this chain aborted.", chain->name().c_str());
          continue;
        }
        // Properties check
        std::vector<TimeSequence*> tss(joints.size());
        unsigned inc = 0;
        for (const auto& joint : joints)
          tss[inc++] = joint->distalSegment()->pose();
        double rate = 0.0, start = 0.0;
        unsigned samples = 0;
        if (!compare_timesequences_properties(tss, rate, start, samples))
        {
          error("At least one segment's pose does not have the same sample rate, start time or number of samples in the chain '%s'.", chain->name().c_str());
          continue;
        }
        assert(rate > 0.);
        double dt = 1. / rate;
        double gres = 0.;
        math::Vector Fd, Md, pd;
        math::Vector g = math::Map<const math::Vector>(1, model->gravity(), &gres).replicate(samples);
        // Iteration computation of joint kinetics
        for (auto itJ = joints.rbegin() ; itJ != joints.rend() ; ++itJ)
        {
          auto jnt = *itJ;
          auto seg = jnt->distalSegment();
          auto bsip = seg->findChild<InertialParameters*>();
          if (bsip == nullptr)
          {
            error("The segment '%s' does not have body segment inertial parameters. Inverse dynamics for the chain '%s' aborted.", seg->name().c_str(), chain->name().c_str());
            break;
          }

          // Internal variables used
          // -----------------------

          // - Pose of the segment
          auto pose = math::to_pose(seg->pose());
          // - Rotation component of the pose in the Inertial Coordinate System (ICS)
          auto R = pose.block<9>(0);
          // - Tensor of inertia expressed in the ICS
          math::Array<9> I = transform_relative_inertia(bsip, seg, pose);
          // - Centre of mass expressed in the ICS
          math::Position com = transform_relative_com(bsip, seg, pose);
          // - Mass of the segment
          double m = bsip->mass();
          // - Proximal end point of the segment
          auto pp = math::to_position(jnt->proximalAnchor()->position());
          if (!pp.isValid())
          {
            error("Unexpected error in the computation of proximal anchor position for the joint '%s'. Inverse dynamics for the chain '%s' aborted.", jnt->name().c_str(), chain->name().c_str());
            break;
          }
          // - Lever arm between the proximal end point and the centre of mass
          math::Position c = com - pp;

          // Derivatives computation
          // -----------------------
          // TODO Would it be possible to reduce the computation time by compute the angular velocity for the lower triangle only (same for the angular acceleration)

          // - Angular velocity of the segment in the ICS
          math::Vector omega = R.derivative<1>(dt).transform(R.transpose()).skewRedux();
          // - Angular acceleration of the segment in the ICS
          math::Vector alpha = R.derivative<2>(dt).transform(R.transpose()).skewRedux();
          // - Linear acceleration of the CoM in the ICS
          auto a = com.derivative<2>(dt);

          // Forces and moments computed at the proximal end point
          // -----------------------------------------------------

          // - External contact (ground, etc.)
          math::Vector Fext(samples); Fext.values().setZero(); Fext.residuals().setZero();
          math::Vector Mext(samples); Mext.values().setZero(); Mext.residuals().setZero();
          auto externals = seg->findChildren<const TimeSequence*>({}, {{"type", TimeSequence::Wrench}});
          for (const auto& external : externals)
          {
            auto wrench = math::to_wrench(external);
            if (!wrench.isValid())
            {
              warning("The external wrench '%s' is not valid or does not have the same sample rate, start time or number of sample than the rest of the chain '%s'.", external->name().c_str(), chain->name().c_str());
              continue;
            }
            Fext += wrench.block<3>(0);
            Mext += wrench.block<3>(3) + (wrench.block<3>(6) - pp).cross(wrench.block<3>(0));
          }
          // - Add the forces and moments of distal joint (if any)
          // NOTE: The opposite sign (-=) is because of the use of the action forces and moments computed for the previous segment. In the current segment, they represent reaction forces and moments.
          if (pd.isValid())
          {
            Fext -= Fd;
            Mext -= Md + (pd - pp).cross(Fd);
          }
          // - Weight
          math::Vector Fwei = m * g / 1000.0;
          auto Mwei = c.cross(Fwei);
          // - Dynamics
          math::Vector Fdyn = m * a / 1000.0;
          auto Mdyn = (I.transform(alpha) + omega.cross(I.transform(omega))) / 1000.0 + c.cross(Fdyn);
          // - Proximal joint (result)
          math::Vector Fp = Fdyn - Fwei - Fext;
          math::Vector Mp = Mdyn - Mwei - Mext;
          math::to_timesequence(Fp, jnt->name() + ".Force", rate, start, TimeSequence::Force, "N", jnt);
          math::to_timesequence(Mp, jnt->name() + ".Moment", rate, start, TimeSequence::Moment, "Nmm" , jnt);

          // Set the next (reaction) distal joint variables
          Fd = Fp;
          Md = Mp;
          pd = pp;
        }
      }
    }
    return true;
  };

  /**
   * Create a deep copy of the object and return it as another object.
   */
  InverseDynamicMatrix* InverseDynamicMatrix::clone(Node* parent) const
  {
    auto dest = new InverseDynamicMatrix;
    dest->copy(this);
    dest->addParent(parent);
    return dest;
  };

  /**
   * Do a deep copy of the the given @a source. The previous content is replaced.
   */
  void InverseDynamicMatrix::copy(const Node* source) _OPENMA_NOEXCEPT
  {
    auto src = node_cast<const InverseDynamicMatrix*>(source);
    if (src == nullptr)
      return;
    this->InverseDynamicProcessor::copy(src);
  };

 };
};<commit_msg>Save segments angular velocity for later use.<commit_after>/*
 * Open Source Movement Analysis Library
 * Copyright (C) 2016, Moveck Solution 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(s) of the copyright holders 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 "openma/body/inversedynamicsmatrix.h"
#include "openma/body/inversedynamicsprocessor_p.h"

#include "openma/body/anchor.h"
#include "openma/body/chain.h"
#include "openma/body/inertialparameters.h"
#include "openma/body/joint.h"
#include "openma/body/model.h"
#include "openma/body/segment.h"
#include "openma/body/utils.h"
#include "openma/base/trial.h"
#include "openma/instrument/forceplate.h"
#include "openma/math.h"

// -------------------------------------------------------------------------- //
//                                 PRIVATE API                                //
// -------------------------------------------------------------------------- //

#ifndef DOXYGEN_SHOULD_SKIP_THIS

namespace ma
{
namespace body
{
  class InverseDynamicMatrixPrivate : public InverseDynamicProcessorPrivate
  {
    OPENMA_DECLARE_PINT_ACCESSOR(InverseDynamicMatrix)

  public:
    InverseDynamicMatrixPrivate(InverseDynamicMatrix* pint);
    ~InverseDynamicMatrixPrivate();
  };

  InverseDynamicMatrixPrivate::InverseDynamicMatrixPrivate(InverseDynamicMatrix* pint)
  : InverseDynamicProcessorPrivate(pint,"InverseDynamicMatrix")
  {};

  InverseDynamicMatrixPrivate::~InverseDynamicMatrixPrivate() = default;
};
};

#endif

// -------------------------------------------------------------------------- //
//                                 PUBLIC API                                 //
// -------------------------------------------------------------------------- //

OPENMA_INSTANCE_STATIC_TYPEID(ma::body::InverseDynamicMatrix);

namespace ma
{
namespace body
{
  /**
   * @class InverseDynamicMatrix openma/body/matrixinversedynamics.h
   * @brief Algorithm to compute joint kinetics expressed in the global frame
   *
   * @TODO Explain in details the required element to use this algorithm (model configure, external wrench, etc.).
   *
   * @ingroup openma_body
   */

  /**
   * Constructor
   */
  InverseDynamicMatrix::InverseDynamicMatrix(Node* parent)
  : InverseDynamicProcessor(*new InverseDynamicMatrixPrivate(this), parent)
  {};

  /**
   * Destructor
   */
  InverseDynamicMatrix::~InverseDynamicMatrix() _OPENMA_NOEXCEPT = default;

  /**
   * Internally this method is a combination of two algorithms published in the litterature [1,2].
   * Both of these algorithms are known to be less sensitive to noise [3]. The reason to use this is only to reduce computation time.
   *  - No need to convert to quaternion
   *  - No need to create sparse 6x6 matrices
   *  - Angular velocity and angular acceleration computed directly from rotation matrices
   *  - All the results are express in the inertial (global) coordinate system
   *
   * @par References
   *  1. Dumas et al., 2004, A 3D generic inverse dynamic method using wrench notation and quaternion algebra, Computer methods in Biomechanics and Biomedical Engineering, 7 (3), pp. 159-166
   *  2. Doriot & Chèze, 2004, A Three-Dimensional Kinematic and Dynamic Study of the Lower Limb During the Stance Phase of Gait Using an Homogeneous Matrix Approach, IEEE Transactions on Biomedical Engineering, 51 (1), pp. 21-27
   *  3. Dumas et al., 2007, Influence of the 3D Inverse Dynamic Method on the Joint Forces and Moments During Gait, ￼Journal of￼Biomechanical￼Engineering, 129, pp. 786-790
   */
  bool InverseDynamicMatrix::run(Node* inout)
  {
#if !defined(_MSC_VER)
#warning WE NEED TO MANAGE UNITS FOR FORCES AND MOMENTS
#endif

    auto models = inout->findChildren<Model*>({},{},false);
    for (auto& model : models)
    {
      auto chains = model->chains()->findChildren<Chain*>({},{},false);
      for (auto& chain : chains)
      {
        auto joints = chain->findChildren<Joint*>({},{},false);
        if (joints.empty())
        {
          warning("No joint found in the chain '%s'. Inverse dynamics for this chain aborted.", chain->name().c_str());
          continue;
        }
        // Properties check
        std::vector<TimeSequence*> tss(joints.size());
        unsigned inc = 0;
        for (const auto& joint : joints)
          tss[inc++] = joint->distalSegment()->pose();
        double rate = 0.0, start = 0.0;
        unsigned samples = 0;
        if (!compare_timesequences_properties(tss, rate, start, samples))
        {
          error("At least one segment's pose does not have the same sample rate, start time or number of samples in the chain '%s'.", chain->name().c_str());
          continue;
        }
        assert(rate > 0.);
        double dt = 1. / rate;
        double gres = 0.;
        math::Vector Fd, Md, pd;
        math::Vector g = math::Map<const math::Vector>(1, model->gravity(), &gres).replicate(samples);
        // Iteration computation of joint kinetics
        for (auto itJ = joints.rbegin() ; itJ != joints.rend() ; ++itJ)
        {
          auto jnt = *itJ;
          auto seg = jnt->distalSegment();
          auto bsip = seg->findChild<InertialParameters*>();
          if (bsip == nullptr)
          {
            error("The segment '%s' does not have body segment inertial parameters. Inverse dynamics for the chain '%s' aborted.", seg->name().c_str(), chain->name().c_str());
            break;
          }

          // Internal variables used
          // -----------------------

          // - Pose of the segment
          auto spts = seg->pose();
          auto pose = math::to_pose(spts);
          // - Rotation component of the pose in the Inertial Coordinate System (ICS)
          auto R = pose.block<9>(0);
          // - Tensor of inertia expressed in the ICS
          math::Array<9> I = transform_relative_inertia(bsip, seg, pose);
          // - Centre of mass expressed in the ICS
          math::Position com = transform_relative_com(bsip, seg, pose);
          // - Mass of the segment
          double m = bsip->mass();
          // - Proximal end point of the segment
          auto pp = math::to_position(jnt->proximalAnchor()->position());
          if (!pp.isValid())
          {
            error("Unexpected error in the computation of proximal anchor position for the joint '%s'. Inverse dynamics for the chain '%s' aborted.", jnt->name().c_str(), chain->name().c_str());
            break;
          }
          // - Lever arm between the proximal end point and the centre of mass
          math::Position c = com - pp;

          // Derivatives computation
          // -----------------------
          // TODO Would it be possible to reduce the computation time by compute the angular velocity for the lower triangle only (same for the angular acceleration)

          // - Angular velocity of the segment in the ICS
          math::Vector omega = R.derivative<1>(dt).transform(R.transpose()).skewRedux();
          // - Angular acceleration of the segment in the ICS
          math::Vector alpha = R.derivative<2>(dt).transform(R.transpose()).skewRedux();
          // - Linear acceleration of the CoM in the ICS
          auto a = com.derivative<2>(dt);

          // Forces and moments computed at the proximal end point
          // -----------------------------------------------------

          // - External contact (ground, etc.)
          math::Vector Fext(samples); Fext.values().setZero(); Fext.residuals().setZero();
          math::Vector Mext(samples); Mext.values().setZero(); Mext.residuals().setZero();
          auto externals = seg->findChildren<const TimeSequence*>({}, {{"type", TimeSequence::Wrench}});
          for (const auto& external : externals)
          {
            auto wrench = math::to_wrench(external);
            if (!wrench.isValid())
            {
              warning("The external wrench '%s' is not valid or does not have the same sample rate, start time or number of sample than the rest of the chain '%s'.", external->name().c_str(), chain->name().c_str());
              continue;
            }
            Fext += wrench.block<3>(0);
            Mext += wrench.block<3>(3) + (wrench.block<3>(6) - pp).cross(wrench.block<3>(0));
          }
          // - Add the forces and moments of distal joint (if any)
          // NOTE: The opposite sign (-=) is because of the use of the action forces and moments computed for the previous segment. In the current segment, they represent reaction forces and moments.
          if (pd.isValid())
          {
            Fext -= Fd;
            Mext -= Md + (pd - pp).cross(Fd);
          }
          // - Weight
          math::Vector Fwei = m * g / 1000.0;
          auto Mwei = c.cross(Fwei);
          // - Dynamics
          math::Vector Fdyn = m * a / 1000.0;
          auto Mdyn = (I.transform(alpha) + omega.cross(I.transform(omega))) / 1000.0 + c.cross(Fdyn);
          // - Proximal joint (result)
          math::Vector Fp = Fdyn - Fwei - Fext;
          math::Vector Mp = Mdyn - Mwei - Mext;
          math::to_timesequence(Fp, jnt->name() + ".Force", rate, start, TimeSequence::Force, "N", jnt);
          math::to_timesequence(Mp, jnt->name() + ".Moment", rate, start, TimeSequence::Moment, "Nmm" , jnt);
          math::to_timesequence(omega, spts->name() + ".Omega", rate, start, TimeSequence::Angle | TimeSequence::Velocity | TimeSequence::Reconstructed, "rad/s" , seg);

          // Set the next (reaction) distal joint variables
          Fd = Fp;
          Md = Mp;
          pd = pp;
        }
      }
    }
    return true;
  };

  /**
   * Create a deep copy of the object and return it as another object.
   */
  InverseDynamicMatrix* InverseDynamicMatrix::clone(Node* parent) const
  {
    auto dest = new InverseDynamicMatrix;
    dest->copy(this);
    dest->addParent(parent);
    return dest;
  };

  /**
   * Do a deep copy of the the given @a source. The previous content is replaced.
   */
  void InverseDynamicMatrix::copy(const Node* source) _OPENMA_NOEXCEPT
  {
    auto src = node_cast<const InverseDynamicMatrix*>(source);
    if (src == nullptr)
      return;
    this->InverseDynamicProcessor::copy(src);
  };

 };
};<|endoftext|>


<commit_before>/*
 * Copyright 2015 - 2021 gary@drinkingtea.net
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#pragma once

#ifdef OX_USE_STDLIB
#include <array>
#endif

#include "bstring.hpp"
#include "fmt.hpp"
#include "hashmap.hpp"
#include "string.hpp"
#include "units.hpp"

extern "C" {

void oxTraceInitHook();

void oxTraceHook(const char *file, int line, const char *ch, const char *msg);

}

namespace ox::trace {

struct TraceMsg {
	static constexpr auto TypeName = "net.drinkingtea.ox.trace.TraceMsg";
	static constexpr auto Fields = 5;
	static constexpr auto TypeVersion = 1;
	const char *file = "";
	int line = 0;
	uint64_t time = 0;
	const char *ch = "";
	BasicString<100> msg;
};

template<typename T>
constexpr Error model(T *io, TraceMsg *obj) {
	io->template setTypeInfo<TraceMsg>();
	oxReturnError(io->field("ch", &obj->ch));
	oxReturnError(io->field("file", &obj->file));
	oxReturnError(io->field("line", &obj->line));
	oxReturnError(io->field("time", &obj->time));
	oxReturnError(io->field("msg", &obj->msg));
	return OxError(0);
}

class OutStream {

	protected:
		const char *m_delimiter = " ";
		TraceMsg m_msg;

	public:
		constexpr OutStream(const char *file, int line, const char *ch, const char *msg = "") noexcept {
			m_msg.file = file;
			m_msg.line = line;
			m_msg.ch = ch;
			m_msg.msg = msg;
		}

#ifdef OX_USE_STDLIB
		template<std::size_t fmtSegmentCnt, typename ...Args>
		constexpr OutStream(const char *file, int line, const char *ch, detail::Fmt<fmtSegmentCnt> fmtSegments, std::array<detail::FmtArg, fmtSegmentCnt - 1> elements) noexcept {
			//static_assert(sizeof...(args) == fmtSegmentCnt - 1, "Wrong number of trace arguments for format.");
			m_msg.file = file;
			m_msg.line = line;
			m_msg.ch = ch;
			const auto &firstSegment = fmtSegments.segments[0];
			oxIgnoreError(m_msg.msg.append(firstSegment.str, firstSegment.length));
			//const detail::FmtArg elements[sizeof...(args)] = {args...};
			for (auto i = 0u; i < fmtSegments.size - 1; ++i) {
				m_msg.msg += elements[i].out;
				const auto &s = fmtSegments.segments[i + 1];
				oxIgnoreError(m_msg.msg.append(s.str, s.length));
			}
		}
#else
		template<std::size_t fmtSegmentCnt, typename ...Args>
		constexpr OutStream(const char *file, int line, const char *ch, detail::Fmt<fmtSegmentCnt> fmtSegments, Args... args) noexcept {
			//static_assert(sizeof...(args) == fmtSegmentCnt - 1, "Wrong number of trace arguments for format.");
			m_msg.file = file;
			m_msg.line = line;
			m_msg.ch = ch;
			const auto &firstSegment = fmtSegments.segments[0];
			oxIgnoreError(m_msg.msg.append(firstSegment.str, firstSegment.length));
			const detail::FmtArg elements[sizeof...(args)] = {args...};
			for (auto i = 0u; i < fmtSegments.size - 1; ++i) {
				m_msg.msg += elements[i].out;
				const auto &s = fmtSegments.segments[i + 1];
				oxIgnoreError(m_msg.msg.append(s.str, s.length));
			}
		}
#endif

		inline ~OutStream() noexcept {
			oxTraceHook(m_msg.file, m_msg.line, m_msg.ch, m_msg.msg.c_str());
		}

		template<typename T>
		constexpr OutStream &operator<<(const typename enable_if<is_integral_v<T>>::type &v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			if constexpr(T(-1) < 0) {
				m_msg.msg += static_cast<int64_t>(v);
			} else {
				m_msg.msg += static_cast<uint64_t>(v);
			}
			return *this;
		}

		constexpr OutStream &operator<<(char *v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += v;
			return *this;
		}

		constexpr OutStream &operator<<(const char *v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += v;
			return *this;
		}

		template<std::size_t sz>
		constexpr OutStream &operator<<(const BasicString<sz> &v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += v.c_str();
			return *this;
		}

		template<std::size_t sz>
		constexpr OutStream &operator<<(const BString<sz> &v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += v.c_str();
			return *this;
		}

		constexpr OutStream &operator<<(char v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += v;
			return *this;
		}

		constexpr OutStream &operator<<(const Error &err) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += static_cast<int64_t>(err);
			return *this;
		}

		/**
		 * del sets the delimiter between log segments.
		 */
		constexpr OutStream &del(const char *delimiter) noexcept {
			m_delimiter = delimiter;
			return *this;
		}

};

class NullStream {

	public:
		constexpr NullStream(const char*, int, const char*, const char* = "") noexcept {
		}

#ifdef OX_USE_STDLIB
		template<std::size_t fmtSegmentCnt, typename ...Args>
		constexpr NullStream(const char*, int, const char*, detail::Fmt<fmtSegmentCnt>, std::array<detail::FmtArg, fmtSegmentCnt - 1>) noexcept {
		}
#else
		template<std::size_t fmtSegmentCnt, typename ...Args>
		constexpr NullStream(const char*, int, const char*, detail::Fmt<fmtSegmentCnt>, Args...) {
		}
#endif

		template<typename T>
		constexpr NullStream &operator<<(const T&) noexcept {
			return *this;
		}

		constexpr NullStream &del(const char*) noexcept {
			return *this;
		}

};

#if defined(DEBUG) && !defined(OX_BARE_METAL)
using TraceStream = OutStream;
#else
using TraceStream = NullStream;
#endif

void logError(const char *file, int line, const Error &err);

void init();

}

#define oxLogError(err) ox::trace::logError(__FILE__, __LINE__, err)

#define oxTrace(...) ox::trace::TraceStream(__FILE__, __LINE__, __VA_ARGS__)
#define oxOut(...) ox::trace::OutStream(__FILE__, __LINE__, "stdout", __VA_ARGS__)
#define oxErr(...) ox::trace::OutStream(__FILE__, __LINE__, "stderr", __VA_ARGS__)

#ifdef OX_USE_STDLIB
// Non-GCC compilers don't like ##__VA_ARGS__, so use initializer list, which relies on std lib
#define oxTracef(ch, fmt, ...) ox::trace::TraceStream(__FILE__, __LINE__, ch, ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), {__VA_ARGS__})
#define oxOutf(fmt, ...) ox::trace::OutStream(__FILE__, __LINE__, "stdout", ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), {__VA_ARGS__})
#define oxErrf(fmt, ...) ox::trace::OutStream(__FILE__, __LINE__, "stderr", ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), {__VA_ARGS__})
#else
#define oxTracef(ch, fmt, ...) ox::trace::TraceStream(__FILE__, __LINE__, ch, ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), ##__VA_ARGS__)
#define oxOutf(fmt, ...) ox::trace::OutStream(__FILE__, __LINE__, "stdout", ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), ##__VA_ARGS__)
#define oxErrf(fmt, ...) ox::trace::OutStream(__FILE__, __LINE__, "stderr", ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), ##__VA_ARGS__)
#endif

#define oxInfo(...) oxTrace("info", __VA_ARGS__)
#define oxInfof(...) oxTracef("info", __VA_ARGS__)
#define oxError(...) oxTrace("error", __VA_ARGS__)
#define oxErrorf(...) oxTracef("error", __VA_ARGS__)

#ifndef OX_NODEBUG
#define oxDebug(...) oxTrace("debug", __VA_ARGS__)
#define oxDebugf(...) oxTracef("debug", __VA_ARGS__)
#else
#define oxDebug(...) static_assert(false, "Debug prints were checked in."); oxTrace("debug", __VA_ARGS__)
#define oxDebugf(...) static_assert(false, "Debug prints were checked in."); oxTracef("debug", __VA_ARGS__)
#endif
<commit_msg>[ox/std] Remove template integer operators from OutStream<commit_after>/*
 * Copyright 2015 - 2021 gary@drinkingtea.net
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#pragma once

#ifdef OX_USE_STDLIB
#include <array>
#endif

#include "bstring.hpp"
#include "fmt.hpp"
#include "hashmap.hpp"
#include "string.hpp"
#include "units.hpp"

extern "C" {

void oxTraceInitHook();

void oxTraceHook(const char *file, int line, const char *ch, const char *msg);

}

namespace ox::trace {

struct TraceMsg {
	static constexpr auto TypeName = "net.drinkingtea.ox.trace.TraceMsg";
	static constexpr auto Fields = 5;
	static constexpr auto TypeVersion = 1;
	const char *file = "";
	int line = 0;
	uint64_t time = 0;
	const char *ch = "";
	BasicString<100> msg;
};

template<typename T>
constexpr Error model(T *io, TraceMsg *obj) {
	io->template setTypeInfo<TraceMsg>();
	oxReturnError(io->field("ch", &obj->ch));
	oxReturnError(io->field("file", &obj->file));
	oxReturnError(io->field("line", &obj->line));
	oxReturnError(io->field("time", &obj->time));
	oxReturnError(io->field("msg", &obj->msg));
	return OxError(0);
}

class OutStream {

	protected:
		const char *m_delimiter = " ";
		TraceMsg m_msg;

	public:
		constexpr OutStream(const char *file, int line, const char *ch, const char *msg = "") noexcept {
			m_msg.file = file;
			m_msg.line = line;
			m_msg.ch = ch;
			m_msg.msg = msg;
		}

#ifdef OX_USE_STDLIB
		template<std::size_t fmtSegmentCnt, typename ...Args>
		constexpr OutStream(const char *file, int line, const char *ch, detail::Fmt<fmtSegmentCnt> fmtSegments, std::array<detail::FmtArg, fmtSegmentCnt - 1> elements) noexcept {
			//static_assert(sizeof...(args) == fmtSegmentCnt - 1, "Wrong number of trace arguments for format.");
			m_msg.file = file;
			m_msg.line = line;
			m_msg.ch = ch;
			const auto &firstSegment = fmtSegments.segments[0];
			oxIgnoreError(m_msg.msg.append(firstSegment.str, firstSegment.length));
			//const detail::FmtArg elements[sizeof...(args)] = {args...};
			for (auto i = 0u; i < fmtSegments.size - 1; ++i) {
				m_msg.msg += elements[i].out;
				const auto &s = fmtSegments.segments[i + 1];
				oxIgnoreError(m_msg.msg.append(s.str, s.length));
			}
		}
#else
		template<std::size_t fmtSegmentCnt, typename ...Args>
		constexpr OutStream(const char *file, int line, const char *ch, detail::Fmt<fmtSegmentCnt> fmtSegments, Args... args) noexcept {
			//static_assert(sizeof...(args) == fmtSegmentCnt - 1, "Wrong number of trace arguments for format.");
			m_msg.file = file;
			m_msg.line = line;
			m_msg.ch = ch;
			const auto &firstSegment = fmtSegments.segments[0];
			oxIgnoreError(m_msg.msg.append(firstSegment.str, firstSegment.length));
			const detail::FmtArg elements[sizeof...(args)] = {args...};
			for (auto i = 0u; i < fmtSegments.size - 1; ++i) {
				m_msg.msg += elements[i].out;
				const auto &s = fmtSegments.segments[i + 1];
				oxIgnoreError(m_msg.msg.append(s.str, s.length));
			}
		}
#endif

		inline ~OutStream() noexcept {
			oxTraceHook(m_msg.file, m_msg.line, m_msg.ch, m_msg.msg.c_str());
		}

		constexpr OutStream &operator<<(short v) noexcept;

		constexpr OutStream &operator<<(int v) noexcept;

		constexpr OutStream &operator<<(long v) noexcept;

		constexpr OutStream &operator<<(long long v) noexcept;

		constexpr OutStream &operator<<(unsigned char v) noexcept;

		constexpr OutStream &operator<<(unsigned short v) noexcept;

		constexpr OutStream &operator<<(unsigned int v) noexcept;

		constexpr OutStream &operator<<(unsigned long v) noexcept;

		constexpr OutStream &operator<<(unsigned long long v) noexcept;

		constexpr OutStream &operator<<(char v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += v;
			return *this;
		}

		constexpr OutStream &operator<<(const char *v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += v;
			return *this;
		}

		template<std::size_t sz>
		constexpr OutStream &operator<<(const BString<sz> &v) noexcept {
			return operator<<(v.c_str());
		}

		template<std::size_t sz>
		constexpr OutStream &operator<<(const BasicString<sz> &v) noexcept {
			return operator<<(v.c_str());
		}

		constexpr OutStream &operator<<(const Error &err) noexcept {
			return appendSignedInt(static_cast<int64_t>(err));
		}

		/**
		 * del sets the delimiter between log segments.
		 */
		constexpr OutStream &del(const char *delimiter) noexcept {
			m_delimiter = delimiter;
			return *this;
		}

	private:
		constexpr OutStream &appendSignedInt(int64_t v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += static_cast<int64_t>(v);
			return *this;
		}

		constexpr OutStream &appendUnsignedInt(uint64_t v) noexcept {
			if (m_msg.msg.len()) {
				m_msg.msg += m_delimiter;
			}
			m_msg.msg += static_cast<int64_t>(v);
			return *this;
		}

};

constexpr OutStream &OutStream::operator<<(short v) noexcept {
	return appendSignedInt(v);
}

constexpr OutStream &OutStream::operator<<(int v) noexcept {
	return appendSignedInt(v);
}

constexpr OutStream &OutStream::operator<<(long v) noexcept {
	return appendSignedInt(v);
}

constexpr OutStream &OutStream::operator<<(long long v) noexcept {
	return appendSignedInt(v);
}

constexpr OutStream &OutStream::operator<<(unsigned char v) noexcept {
	return appendUnsignedInt(v);
}

constexpr OutStream &OutStream::operator<<(unsigned short v) noexcept {
	return appendUnsignedInt(v);
}

constexpr OutStream &OutStream::operator<<(unsigned int v) noexcept {
	return appendUnsignedInt(v);
}

constexpr OutStream &OutStream::operator<<(unsigned long v) noexcept {
	return appendUnsignedInt(v);
}

constexpr OutStream &OutStream::operator<<(unsigned long long v) noexcept {
	return appendUnsignedInt(v);
}

class NullStream {

	public:
		constexpr NullStream(const char*, int, const char*, const char* = "") noexcept {
		}

#ifdef OX_USE_STDLIB
		template<std::size_t fmtSegmentCnt, typename ...Args>
		constexpr NullStream(const char*, int, const char*, detail::Fmt<fmtSegmentCnt>, std::array<detail::FmtArg, fmtSegmentCnt - 1>) noexcept {
		}
#else
		template<std::size_t fmtSegmentCnt, typename ...Args>
		constexpr NullStream(const char*, int, const char*, detail::Fmt<fmtSegmentCnt>, Args...) {
		}
#endif

		template<typename T>
		constexpr NullStream &operator<<(const T&) noexcept {
			return *this;
		}

		constexpr NullStream &del(const char*) noexcept {
			return *this;
		}

};

#if defined(DEBUG) && !defined(OX_BARE_METAL)
using TraceStream = OutStream;
#else
using TraceStream = NullStream;
#endif

void logError(const char *file, int line, const Error &err);

void init();

}

#define oxLogError(err) ox::trace::logError(__FILE__, __LINE__, err)

#define oxTrace(...) ox::trace::TraceStream(__FILE__, __LINE__, __VA_ARGS__)
#define oxOut(...) ox::trace::OutStream(__FILE__, __LINE__, "stdout", __VA_ARGS__)
#define oxErr(...) ox::trace::OutStream(__FILE__, __LINE__, "stderr", __VA_ARGS__)

#ifdef OX_USE_STDLIB
// Non-GCC compilers don't like ##__VA_ARGS__, so use initializer list, which relies on std lib
#define oxTracef(ch, fmt, ...) ox::trace::TraceStream(__FILE__, __LINE__, ch, ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), {__VA_ARGS__})
#define oxOutf(fmt, ...) ox::trace::OutStream(__FILE__, __LINE__, "stdout", ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), {__VA_ARGS__})
#define oxErrf(fmt, ...) ox::trace::OutStream(__FILE__, __LINE__, "stderr", ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), {__VA_ARGS__})
#else
#define oxTracef(ch, fmt, ...) ox::trace::TraceStream(__FILE__, __LINE__, ch, ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), ##__VA_ARGS__)
#define oxOutf(fmt, ...) ox::trace::OutStream(__FILE__, __LINE__, "stdout", ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), ##__VA_ARGS__)
#define oxErrf(fmt, ...) ox::trace::OutStream(__FILE__, __LINE__, "stderr", ox::detail::fmtSegments<ox::detail::argCount(fmt)+1>(fmt), ##__VA_ARGS__)
#endif

#define oxInfo(...) oxTrace("info", __VA_ARGS__)
#define oxInfof(...) oxTracef("info", __VA_ARGS__)
#define oxError(...) oxTrace("error", __VA_ARGS__)
#define oxErrorf(...) oxTracef("error", __VA_ARGS__)

#ifndef OX_NODEBUG
#define oxDebug(...) oxTrace("debug", __VA_ARGS__)
#define oxDebugf(...) oxTracef("debug", __VA_ARGS__)
#else
#define oxDebug(...) static_assert(false, "Debug prints were checked in."); oxTrace("debug", __VA_ARGS__)
#define oxDebugf(...) static_assert(false, "Debug prints were checked in."); oxTracef("debug", __VA_ARGS__)
#endif
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 The Apollo Authors. 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 <fstream>
#include <string>
#include <vector>

#include "gflags/gflags.h"
#include "ros/include/ros/ros.h"
#include "ros/include/rosbag/bag.h"
#include "ros/include/rosbag/view.h"

#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/util/file.h"
#include "modules/common/util/string_util.h"

DEFINE_string(adapter_config_filename,
              "/apollo/modules/data/conf/event_collector_adapter.conf",
              "Path for adapter configuration.");

DEFINE_string(events_filename, "events.txt", "File to save event list.");

DEFINE_string(events_related_bags_filename, "events_related_bags.txt",
              "File to save events related bags list.");

DEFINE_double(event_time_backtrack_seconds, 20,
              "Backtrack from the event time to consider bags as related.");

namespace apollo {
namespace data {
namespace {
using apollo::common::adapter::AdapterManager;
using apollo::canbus::Chassis;

void OnSigInt(int32_t signal_num) {
  // only response for ctrl + c
  if (signal_num != SIGINT) {
    return;
  }
  AINFO << "EventCollector got signal: " << signal_num;

  // Only stop once.
  static bool is_stopping = false;
  if (!is_stopping) {
    is_stopping = true;
    ros::shutdown();
  }
}

class EventCollector {
 public:
  void Init(int32_t argc, char **argv) {
    signal(SIGINT, OnSigInt);

    ros::init(argc, argv, "EventCollector");
    AdapterManager::Init(FLAGS_adapter_config_filename);
  }

  void Start() {
    AdapterManager::AddDriveEventCallback(&EventCollector::OnDriveEvent, this);
    AdapterManager::AddChassisCallback(&EventCollector::OnChassis, this);
    AdapterManager::AddMonitorCallback(&EventCollector::OnMonitorMessage, this);
    AINFO << "Start spining...";
    ros::spin();
  }

  void Stop() {
    // Save events_related_bags.
    std::ofstream fout(FLAGS_events_related_bags_filename);
    if (!fout) {
      AERROR << "Failed to write " << FLAGS_events_related_bags_filename;
      return;
    }
    AINFO << "Saving info to " << FLAGS_events_related_bags_filename << "...";
    fout << std::fixed << std::setprecision(1);

    sort(events_.begin(), events_.end());
    for (const std::string& bag_filename :
         apollo::common::util::ListSubPaths("./", DT_REG)) {
      if (!apollo::common::util::EndWith(bag_filename, ".bag")) {
        continue;
      }

      rosbag::Bag bag(bag_filename);
      rosbag::View view(bag);
      const double begin_time = view.getBeginTime().toSec();
      const double end_time =
          view.getEndTime().toSec() + FLAGS_event_time_backtrack_seconds;

      bool started_line = false;
      const std::tuple<double, std::string> key(begin_time, "");
      for (auto iter = std::lower_bound(events_.begin(), events_.end(), key);
           iter != events_.end() && std::get<0>(*iter) < end_time; ++iter) {
        // event_time = std::get<0>(*iter)
        // event_message = std::get<1>(*iter)
        if (!started_line) {
          started_line = true;
          fout << bag_filename << "\n";
        }
        fout << "  Offset = " << std::get<0>(*iter) - begin_time << ", "
             << std::get<1>(*iter) << "\n";
      }
    }
  }

 private:
  // Event time and message.
  std::vector<std::tuple<double, std::string>> events_;
  Chassis::DrivingMode current_driving_mode_;

  void OnDriveEvent(const apollo::common::DriveEvent& event) {
    // The header time is the real event time.
    SaveEvent(event.header().timestamp_sec(), "DriveEvent", event.event());
  }

  void OnChassis(const Chassis& chassis) {
    // Save event when driving_mode changes from COMPLETE_AUTO_DRIVE to
    // EMERGENCY_MODE which is taken as a disengagement.
    if (current_driving_mode_ == Chassis::COMPLETE_AUTO_DRIVE &&
        chassis.driving_mode() == Chassis::EMERGENCY_MODE) {
      SaveEvent(chassis.header().timestamp_sec(), "Disengagement");
    }
    current_driving_mode_ = chassis.driving_mode();
  }

  void OnMonitorMessage(
      const apollo::common::monitor::MonitorMessage& monitor_msg) {
    using apollo::common::monitor::MonitorMessageItem;
    // Save all ERROR and FATAL monitor logs.
    const double time_sec = monitor_msg.header().timestamp_sec();
    for (const auto& item : monitor_msg.item()) {
      if (item.log_level() == MonitorMessageItem::ERROR ||
          item.log_level() == MonitorMessageItem::FATAL) {
        SaveEvent(time_sec, "MonitorErrorLog", item.msg());
      }
    }
  }

  void SaveEvent(const double timestamp_sec, const std::string type,
                 const std::string description = "") {
    const auto msg = apollo::common::util::StrCat(
        timestamp_sec, " [", type, "] ", description);

    AINFO << "SaveEvent: " << msg;
    events_.emplace_back(timestamp_sec, msg);

    // Append new event and flush.
    std::ofstream fout(FLAGS_events_filename, std::ofstream::app);
    if (fout) {
      fout << msg << std::endl;
    } else {
      AERROR << "Failed to write to " << FLAGS_events_filename;
    }
  }
};

}  // namespace
}  // namespace data
}  // namespace apollo

int main(int32_t argc, char **argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  apollo::data::EventCollector event_collector;
  event_collector.Init(argc, argv);
  event_collector.Start();
  event_collector.Stop();

  return 0;
}
<commit_msg>Data: Rename to last_driving_mode_ to make it clear.<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. 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 <fstream>
#include <string>
#include <vector>

#include "gflags/gflags.h"
#include "ros/include/ros/ros.h"
#include "ros/include/rosbag/bag.h"
#include "ros/include/rosbag/view.h"

#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/util/file.h"
#include "modules/common/util/string_util.h"

DEFINE_string(adapter_config_filename,
              "/apollo/modules/data/conf/event_collector_adapter.conf",
              "Path for adapter configuration.");

DEFINE_string(events_filename, "events.txt", "File to save event list.");

DEFINE_string(events_related_bags_filename, "events_related_bags.txt",
              "File to save events related bags list.");

DEFINE_double(event_time_backtrack_seconds, 20,
              "Backtrack from the event time to consider bags as related.");

namespace apollo {
namespace data {
namespace {
using apollo::common::adapter::AdapterManager;
using apollo::canbus::Chassis;

void OnSigInt(int32_t signal_num) {
  // only response for ctrl + c
  if (signal_num != SIGINT) {
    return;
  }
  AINFO << "EventCollector got signal: " << signal_num;

  // Only stop once.
  static bool is_stopping = false;
  if (!is_stopping) {
    is_stopping = true;
    ros::shutdown();
  }
}

class EventCollector {
 public:
  void Init(int32_t argc, char **argv) {
    signal(SIGINT, OnSigInt);

    ros::init(argc, argv, "EventCollector");
    AdapterManager::Init(FLAGS_adapter_config_filename);
  }

  void Start() {
    AdapterManager::AddDriveEventCallback(&EventCollector::OnDriveEvent, this);
    AdapterManager::AddChassisCallback(&EventCollector::OnChassis, this);
    AdapterManager::AddMonitorCallback(&EventCollector::OnMonitorMessage, this);
    AINFO << "Start spining...";
    ros::spin();
  }

  void Stop() {
    // Save events_related_bags.
    std::ofstream fout(FLAGS_events_related_bags_filename);
    if (!fout) {
      AERROR << "Failed to write " << FLAGS_events_related_bags_filename;
      return;
    }
    AINFO << "Saving info to " << FLAGS_events_related_bags_filename << "...";
    fout << std::fixed << std::setprecision(1);

    sort(events_.begin(), events_.end());
    for (const std::string& bag_filename :
         apollo::common::util::ListSubPaths("./", DT_REG)) {
      if (!apollo::common::util::EndWith(bag_filename, ".bag")) {
        continue;
      }

      rosbag::Bag bag(bag_filename);
      rosbag::View view(bag);
      const double begin_time = view.getBeginTime().toSec();
      const double end_time =
          view.getEndTime().toSec() + FLAGS_event_time_backtrack_seconds;

      bool started_line = false;
      const std::tuple<double, std::string> key(begin_time, "");
      for (auto iter = std::lower_bound(events_.begin(), events_.end(), key);
           iter != events_.end() && std::get<0>(*iter) < end_time; ++iter) {
        // event_time = std::get<0>(*iter)
        // event_message = std::get<1>(*iter)
        if (!started_line) {
          started_line = true;
          fout << bag_filename << "\n";
        }
        fout << "  Offset = " << std::get<0>(*iter) - begin_time << ", "
             << std::get<1>(*iter) << "\n";
      }
    }
  }

 private:
  // Event time and message.
  std::vector<std::tuple<double, std::string>> events_;
  Chassis::DrivingMode last_driving_mode_;

  void OnDriveEvent(const apollo::common::DriveEvent& event) {
    // The header time is the real event time.
    SaveEvent(event.header().timestamp_sec(), "DriveEvent", event.event());
  }

  void OnChassis(const Chassis& chassis) {
    // Save event when driving_mode changes from COMPLETE_AUTO_DRIVE to
    // EMERGENCY_MODE which is taken as a disengagement.
    if (last_driving_mode_ == Chassis::COMPLETE_AUTO_DRIVE &&
        chassis.driving_mode() == Chassis::EMERGENCY_MODE) {
      SaveEvent(chassis.header().timestamp_sec(), "Disengagement");
    }
    last_driving_mode_ = chassis.driving_mode();
  }

  void OnMonitorMessage(
      const apollo::common::monitor::MonitorMessage& monitor_msg) {
    using apollo::common::monitor::MonitorMessageItem;
    // Save all ERROR and FATAL monitor logs.
    const double time_sec = monitor_msg.header().timestamp_sec();
    for (const auto& item : monitor_msg.item()) {
      if (item.log_level() == MonitorMessageItem::ERROR ||
          item.log_level() == MonitorMessageItem::FATAL) {
        SaveEvent(time_sec, "MonitorErrorLog", item.msg());
      }
    }
  }

  void SaveEvent(const double timestamp_sec, const std::string type,
                 const std::string description = "") {
    const auto msg = apollo::common::util::StrCat(
        timestamp_sec, " [", type, "] ", description);

    AINFO << "SaveEvent: " << msg;
    events_.emplace_back(timestamp_sec, msg);

    // Append new event and flush.
    std::ofstream fout(FLAGS_events_filename, std::ofstream::app);
    if (fout) {
      fout << msg << std::endl;
    } else {
      AERROR << "Failed to write to " << FLAGS_events_filename;
    }
  }
};

}  // namespace
}  // namespace data
}  // namespace apollo

int main(int32_t argc, char **argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  apollo::data::EventCollector event_collector;
  event_collector.Init(argc, argv);
  event_collector.Start();
  event_collector.Stop();

  return 0;
}
<|endoftext|>