<commit_before>// Copyright 2010 Jens-Michael Hoffmann <jmho@c-xx.com>
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library. If not, see <http://www.gnu.org/licenses/>.

#include "BlendingAlgorithms.h"

#include "TextureTile.h"

#include <cmath>

#include <QtGui/QImage>
#include <QtGui/QPainter>

namespace Marble
{

void OverpaintBlending::blend( QImage * const bottom, TextureTile const * const top ) const
{
    Q_ASSERT( bottom );
    Q_ASSERT( top );
    Q_ASSERT( top->image() );
    Q_ASSERT( bottom->size() == top->image()->size() );
    Q_ASSERT( bottom->format() == QImage::Format_ARGB32_Premultiplied );

    QPainter painter( bottom );

    painter.drawImage( 0, 0, *top->image() );
}

// pre-conditions:
// - bottom and top image have the same size
// - bottom image format is ARGB32_Premultiplied
void IndependentChannelBlending::blend( QImage * const bottom,
                                        TextureTile const * const top ) const
{
    QImage const * const topImage = top->image();
    Q_ASSERT( topImage );
    Q_ASSERT( bottom->size() == topImage->size() );
    Q_ASSERT( bottom->format() == QImage::Format_ARGB32_Premultiplied );

    int const width = bottom->width();
    int const height = bottom->height();
    QImage const topImagePremult = topImage->convertToFormat( QImage::Format_ARGB32_Premultiplied );
    for ( int y = 0; y < height; ++y ) {
        for ( int x = 0; x < width; ++x ) {
            QRgb const bottomPixel = bottom->pixel( x, y );
            QRgb const topPixel = topImagePremult.pixel( x, y );
            qreal const resultRed = blendChannel( qRed( bottomPixel ) / 255.0,
                                                  qRed( topPixel ) / 255.0 );
            qreal const resultGreen = blendChannel( qGreen( bottomPixel ) / 255.0,
                                                    qGreen( topPixel ) / 255.0 );
            qreal const resultBlue = blendChannel( qBlue( bottomPixel ) / 255.0,
                                                   qBlue( topPixel ) / 255.0 );
            bottom->setPixel( x, y, qRgb( resultRed * 255.0,
                                          resultGreen * 255.0,
                                          resultBlue * 255.0 ));
        }
    }
}

// Neutral blendings

qreal AllanonBlending::blendChannel( qreal const bottomColorIntensity,
                                     qreal const topColorIntensity ) const
{
    return ( bottomColorIntensity + topColorIntensity ) / 2.0;
}

qreal ArcusTangentBlending::blendChannel( qreal const bottomColorIntensity,
                                          qreal const topColorIntensity ) const
{
    return 2.0 * atan( topColorIntensity / bottomColorIntensity ) / M_PI;
}

qreal GeometricMeanBlending::blendChannel( qreal const bottomColorIntensity,
                                           qreal const topColorIntensity ) const
{
    return sqrt( bottomColorIntensity * topColorIntensity );
}

qreal LinearLightBlending::blendChannel( qreal const bottomColorIntensity,
                                         qreal const topColorIntensity ) const
{
    return qMin( qreal( 1.0 ),
                 qMax( qreal( 0.0 ), qreal( bottomColorIntensity + 2.0 * topColorIntensity - 1.0 )));
}

qreal OverlayBlending::blendChannel( qreal const bottomColorIntensity,
                                     qreal const topColorIntensity ) const
{
    if ( bottomColorIntensity < 0.5 )
        return 2.0 * bottomColorIntensity * topColorIntensity;
    else
        return 1.0 - 2.0 * ( 1.0 - bottomColorIntensity ) * ( 1.0 - topColorIntensity );
}

qreal ParallelBlending::blendChannel( qreal const bottomColorIntensity,
                                      qreal const topColorIntensity ) const
{
    Q_UNUSED(bottomColorIntensity);
    Q_UNUSED(topColorIntensity);
    // FIXME:    return qMin( qMax( 2.0 / ( 1.0 / bottomColorIntensity + 1.0 / topColorIntensity )), 0.0, 1.0 );
    return 0.0;
}

qreal TextureBlending::blendChannel( qreal const bottomColorIntensity,
                                     qreal const topColorIntensity ) const
{
    Q_UNUSED(bottomColorIntensity);
    Q_UNUSED(topColorIntensity);
    // FIXME: return qMax( qMin( topColorIntensity + bottomColorIntensity ) - 0.5 ), 1.0 ), 0.0 );
    return 0.0;
}

// Darkening blendings

qreal ColorBurnBlending::blendChannel( qreal const bottomColorIntensity,
                                       qreal const topColorIntensity ) const
{
    Q_UNUSED(bottomColorIntensity);
    Q_UNUSED(topColorIntensity);
    // FIXME: check if this formula makes sense
    return qMin( qreal( 1.0 ),
                 qMax( qreal( 0.0 ), qreal( 1.0 - ( 1.0 - bottomColorIntensity ) / topColorIntensity )));
}

qreal DarkBlending::blendChannel( qreal const bottomColorIntensity,
                                  qreal const topColorIntensity ) const
{
    return ( bottomColorIntensity + 1.0 - topColorIntensity ) * topColorIntensity;
}

qreal DarkenBlending::blendChannel( qreal const bottomColorIntensity,
                                    qreal const topColorIntensity ) const
{
    // FIXME: is this really ok? not vice versa?
    return bottomColorIntensity > topColorIntensity ? topColorIntensity : bottomColorIntensity;
}

qreal DivideBlending::blendChannel( qreal const bottomColorIntensity,
                                    qreal const topColorIntensity ) const
{
    return log( 1.0  + bottomColorIntensity / ( 1.0  - topColorIntensity ) / 8.0) / log(2.0);
}

qreal GammaDarkBlending::blendChannel( qreal const bottomColorIntensity,
                                       qreal const topColorIntensity ) const
{
    return pow( bottomColorIntensity, 1.0 / topColorIntensity );
}

qreal LinearBurnBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return qMax( 0.0, bottomColorIntensity + topColorIntensity - 1.0 );
}

qreal MultiplyBlending::blendChannel( qreal const bottomColorIntensity,
                                      qreal const topColorIntensity ) const
{
    return bottomColorIntensity * topColorIntensity;
}

qreal SubtractiveBlending::blendChannel( qreal const bottomColorIntensity,
                                         qreal const topColorIntensity ) const
{
    return qMax( bottomColorIntensity - topColorIntensity, qreal(0.0) );
}

// Lightening blendings

qreal AdditiveBlending::blendChannel( qreal const bottomColorIntensity,
                                      qreal const topColorIntensity ) const
{
    return qMin( topColorIntensity + bottomColorIntensity, qreal(1.0) );
}

qreal ColorDodgeBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return qMin( qreal( 1.0 ),
                 qMax( qreal( 0.0 ), qreal( bottomColorIntensity / ( 1.0 - topColorIntensity ))));
}

qreal GammaLightBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return pow( bottomColorIntensity, topColorIntensity );
}

qreal HardLightBlending::blendChannel( qreal const bottomColorIntensity,
                                       qreal const topColorIntensity ) const
{
    return topColorIntensity < 0.5
        ? 2.0 * bottomColorIntensity * topColorIntensity
        : 1.0 - 2.0 * ( 1.0 - bottomColorIntensity ) * ( 1.0 - topColorIntensity );
}

qreal LightBlending::blendChannel( qreal const bottomColorIntensity,
                                   qreal const topColorIntensity ) const
{
    return bottomColorIntensity * ( 1.0 - topColorIntensity ) + pow( topColorIntensity, 2 );
}

qreal LightenBlending::blendChannel( qreal const bottomColorIntensity,
                                     qreal const topColorIntensity ) const
{
    // is this ok?
    return bottomColorIntensity < topColorIntensity ? topColorIntensity : bottomColorIntensity;
}

qreal PinLightBlending::blendChannel( qreal const bottomColorIntensity,
                                      qreal const topColorIntensity ) const
{
    return qMax( qreal(0.0), qMax( qreal(2.0 + topColorIntensity - 1.0),
                             qMin( bottomColorIntensity, qreal(2.0 * topColorIntensity ))));
}

qreal ScreenBlending::blendChannel( qreal const bottomColorIntensity,
                                    qreal const topColorIntensity ) const
{
    return 1.0 - ( 1.0 - bottomColorIntensity ) * ( 1.0 - topColorIntensity );
}

qreal SoftLightBlending::blendChannel( qreal const bottomColorIntensity,
                                       qreal const topColorIntensity ) const
{
    return pow( bottomColorIntensity, pow( 2.0, ( 2.0 * ( 0.5 - topColorIntensity ))));
}

qreal VividLightBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return topColorIntensity < 0.5
        ? qMin( qreal( 1.0 ),
                qMax( qreal( 0.0 ), qreal( 1.0 - ( 1.0 - bottomColorIntensity ) / ( 2.0 * topColorIntensity ))))
        : qMin( qreal( 1.0 ),
                qMax( qreal( 0.0 ), qreal( bottomColorIntensity / ( 2.0 * ( 1.0 - topColorIntensity )))));
}

// Inverter blendings

qreal AdditiveSubtractiveBlending::blendChannel( qreal const bottomColorIntensity,
                                                 qreal const topColorIntensity ) const
{
    Q_UNUSED(bottomColorIntensity);
    Q_UNUSED(topColorIntensity);
    // FIXME:
    //    return qMin( 1.0, qMax( 0.0, abs( bottomColorIntensity * bottomColorIntensity
    //                                      - topColorIntensity * topColorIntensity )));
    return 0.0;
}

qreal BleachBlending::blendChannel( qreal const bottomColorIntensity,
                                    qreal const topColorIntensity ) const
{
    // FIXME: "why this is the same formula as Screen Blending? Please correct.)"
    return 1.0 - ( 1.0 - bottomColorIntensity ) * ( 1.0 - topColorIntensity );
}

qreal DifferenceBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return qMax( qMin( qreal( 1.0 ), qreal( bottomColorIntensity - topColorIntensity + 0.5 )),
                 qreal( 0.0 ));
}

qreal EquivalenceBlending::blendChannel( qreal const bottomColorIntensity,
                                         qreal const topColorIntensity ) const
{
    return 1.0 - abs( bottomColorIntensity - topColorIntensity );
}

qreal HalfDifferenceBlending::blendChannel( qreal const bottomColorIntensity,
                                            qreal const topColorIntensity ) const
{
    return bottomColorIntensity + topColorIntensity
        - 2.0 * ( bottomColorIntensity * topColorIntensity );
}

// Special purpose blendings

void CloudsBlending::blend( QImage * const bottom, TextureTile const * const top ) const
{
    QImage const * const topImage = top->image();
    Q_ASSERT( topImage );
    Q_ASSERT( bottom->size() == topImage->size() );
    int const width = bottom->width();
    int const height = bottom->height();
    for ( int y = 0; y < height; ++y ) {
        for ( int x = 0; x < width; ++x ) {
            qreal const c = qRed( topImage->pixel( x, y )) / 255.0;
            QRgb const bottomPixel = bottom->pixel( x, y );
            int const bottomRed = qRed( bottomPixel );
            int const bottomGreen = qGreen( bottomPixel );
            int const bottomBlue = qBlue( bottomPixel );
            bottom->setPixel( x, y, qRgb(( int )( bottomRed + ( 255 - bottomRed ) * c ),
                                         ( int )( bottomGreen + ( 255 - bottomGreen ) * c ),
                                         ( int )( bottomBlue + ( 255 - bottomBlue ) * c )));
        }
    }
}

}
<commit_msg>Cast to qreal<commit_after>// Copyright 2010 Jens-Michael Hoffmann <jmho@c-xx.com>
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library. If not, see <http://www.gnu.org/licenses/>.

#include "BlendingAlgorithms.h"

#include "TextureTile.h"

#include <cmath>

#include <QtGui/QImage>
#include <QtGui/QPainter>

namespace Marble
{

void OverpaintBlending::blend( QImage * const bottom, TextureTile const * const top ) const
{
    Q_ASSERT( bottom );
    Q_ASSERT( top );
    Q_ASSERT( top->image() );
    Q_ASSERT( bottom->size() == top->image()->size() );
    Q_ASSERT( bottom->format() == QImage::Format_ARGB32_Premultiplied );

    QPainter painter( bottom );

    painter.drawImage( 0, 0, *top->image() );
}

// pre-conditions:
// - bottom and top image have the same size
// - bottom image format is ARGB32_Premultiplied
void IndependentChannelBlending::blend( QImage * const bottom,
                                        TextureTile const * const top ) const
{
    QImage const * const topImage = top->image();
    Q_ASSERT( topImage );
    Q_ASSERT( bottom->size() == topImage->size() );
    Q_ASSERT( bottom->format() == QImage::Format_ARGB32_Premultiplied );

    int const width = bottom->width();
    int const height = bottom->height();
    QImage const topImagePremult = topImage->convertToFormat( QImage::Format_ARGB32_Premultiplied );
    for ( int y = 0; y < height; ++y ) {
        for ( int x = 0; x < width; ++x ) {
            QRgb const bottomPixel = bottom->pixel( x, y );
            QRgb const topPixel = topImagePremult.pixel( x, y );
            qreal const resultRed = blendChannel( qRed( bottomPixel ) / 255.0,
                                                  qRed( topPixel ) / 255.0 );
            qreal const resultGreen = blendChannel( qGreen( bottomPixel ) / 255.0,
                                                    qGreen( topPixel ) / 255.0 );
            qreal const resultBlue = blendChannel( qBlue( bottomPixel ) / 255.0,
                                                   qBlue( topPixel ) / 255.0 );
            bottom->setPixel( x, y, qRgb( resultRed * 255.0,
                                          resultGreen * 255.0,
                                          resultBlue * 255.0 ));
        }
    }
}

// Neutral blendings

qreal AllanonBlending::blendChannel( qreal const bottomColorIntensity,
                                     qreal const topColorIntensity ) const
{
    return ( bottomColorIntensity + topColorIntensity ) / 2.0;
}

qreal ArcusTangentBlending::blendChannel( qreal const bottomColorIntensity,
                                          qreal const topColorIntensity ) const
{
    return 2.0 * atan( topColorIntensity / bottomColorIntensity ) / M_PI;
}

qreal GeometricMeanBlending::blendChannel( qreal const bottomColorIntensity,
                                           qreal const topColorIntensity ) const
{
    return sqrt( bottomColorIntensity * topColorIntensity );
}

qreal LinearLightBlending::blendChannel( qreal const bottomColorIntensity,
                                         qreal const topColorIntensity ) const
{
    return qMin( qreal( 1.0 ),
                 qMax( qreal( 0.0 ), qreal( bottomColorIntensity + 2.0 * topColorIntensity - 1.0 )));
}

qreal OverlayBlending::blendChannel( qreal const bottomColorIntensity,
                                     qreal const topColorIntensity ) const
{
    if ( bottomColorIntensity < 0.5 )
        return 2.0 * bottomColorIntensity * topColorIntensity;
    else
        return 1.0 - 2.0 * ( 1.0 - bottomColorIntensity ) * ( 1.0 - topColorIntensity );
}

qreal ParallelBlending::blendChannel( qreal const bottomColorIntensity,
                                      qreal const topColorIntensity ) const
{
    Q_UNUSED(bottomColorIntensity);
    Q_UNUSED(topColorIntensity);
    // FIXME:    return qMin( qMax( 2.0 / ( 1.0 / bottomColorIntensity + 1.0 / topColorIntensity )), 0.0, 1.0 );
    return 0.0;
}

qreal TextureBlending::blendChannel( qreal const bottomColorIntensity,
                                     qreal const topColorIntensity ) const
{
    Q_UNUSED(bottomColorIntensity);
    Q_UNUSED(topColorIntensity);
    // FIXME: return qMax( qMin( topColorIntensity + bottomColorIntensity ) - 0.5 ), 1.0 ), 0.0 );
    return 0.0;
}

// Darkening blendings

qreal ColorBurnBlending::blendChannel( qreal const bottomColorIntensity,
                                       qreal const topColorIntensity ) const
{
    Q_UNUSED(bottomColorIntensity);
    Q_UNUSED(topColorIntensity);
    // FIXME: check if this formula makes sense
    return qMin( qreal( 1.0 ),
                 qMax( qreal( 0.0 ), qreal( 1.0 - ( 1.0 - bottomColorIntensity ) / topColorIntensity )));
}

qreal DarkBlending::blendChannel( qreal const bottomColorIntensity,
                                  qreal const topColorIntensity ) const
{
    return ( bottomColorIntensity + 1.0 - topColorIntensity ) * topColorIntensity;
}

qreal DarkenBlending::blendChannel( qreal const bottomColorIntensity,
                                    qreal const topColorIntensity ) const
{
    // FIXME: is this really ok? not vice versa?
    return bottomColorIntensity > topColorIntensity ? topColorIntensity : bottomColorIntensity;
}

qreal DivideBlending::blendChannel( qreal const bottomColorIntensity,
                                    qreal const topColorIntensity ) const
{
    return log( 1.0  + bottomColorIntensity / ( 1.0  - topColorIntensity ) / 8.0) / log(2.0);
}

qreal GammaDarkBlending::blendChannel( qreal const bottomColorIntensity,
                                       qreal const topColorIntensity ) const
{
    return pow( bottomColorIntensity, 1.0 / topColorIntensity );
}

qreal LinearBurnBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return qMax( qreal(0.0), bottomColorIntensity + topColorIntensity - 1.0 );
}

qreal MultiplyBlending::blendChannel( qreal const bottomColorIntensity,
                                      qreal const topColorIntensity ) const
{
    return bottomColorIntensity * topColorIntensity;
}

qreal SubtractiveBlending::blendChannel( qreal const bottomColorIntensity,
                                         qreal const topColorIntensity ) const
{
    return qMax( bottomColorIntensity - topColorIntensity, qreal(0.0) );
}

// Lightening blendings

qreal AdditiveBlending::blendChannel( qreal const bottomColorIntensity,
                                      qreal const topColorIntensity ) const
{
    return qMin( topColorIntensity + bottomColorIntensity, qreal(1.0) );
}

qreal ColorDodgeBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return qMin( qreal( 1.0 ),
                 qMax( qreal( 0.0 ), qreal( bottomColorIntensity / ( 1.0 - topColorIntensity ))));
}

qreal GammaLightBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return pow( bottomColorIntensity, topColorIntensity );
}

qreal HardLightBlending::blendChannel( qreal const bottomColorIntensity,
                                       qreal const topColorIntensity ) const
{
    return topColorIntensity < 0.5
        ? 2.0 * bottomColorIntensity * topColorIntensity
        : 1.0 - 2.0 * ( 1.0 - bottomColorIntensity ) * ( 1.0 - topColorIntensity );
}

qreal LightBlending::blendChannel( qreal const bottomColorIntensity,
                                   qreal const topColorIntensity ) const
{
    return bottomColorIntensity * ( 1.0 - topColorIntensity ) + pow( topColorIntensity, 2 );
}

qreal LightenBlending::blendChannel( qreal const bottomColorIntensity,
                                     qreal const topColorIntensity ) const
{
    // is this ok?
    return bottomColorIntensity < topColorIntensity ? topColorIntensity : bottomColorIntensity;
}

qreal PinLightBlending::blendChannel( qreal const bottomColorIntensity,
                                      qreal const topColorIntensity ) const
{
    return qMax( qreal(0.0), qMax( qreal(2.0 + topColorIntensity - 1.0),
                             qMin( bottomColorIntensity, qreal(2.0 * topColorIntensity ))));
}

qreal ScreenBlending::blendChannel( qreal const bottomColorIntensity,
                                    qreal const topColorIntensity ) const
{
    return 1.0 - ( 1.0 - bottomColorIntensity ) * ( 1.0 - topColorIntensity );
}

qreal SoftLightBlending::blendChannel( qreal const bottomColorIntensity,
                                       qreal const topColorIntensity ) const
{
    return pow( bottomColorIntensity, pow( 2.0, ( 2.0 * ( 0.5 - topColorIntensity ))));
}

qreal VividLightBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return topColorIntensity < 0.5
        ? qMin( qreal( 1.0 ),
                qMax( qreal( 0.0 ), qreal( 1.0 - ( 1.0 - bottomColorIntensity ) / ( 2.0 * topColorIntensity ))))
        : qMin( qreal( 1.0 ),
                qMax( qreal( 0.0 ), qreal( bottomColorIntensity / ( 2.0 * ( 1.0 - topColorIntensity )))));
}

// Inverter blendings

qreal AdditiveSubtractiveBlending::blendChannel( qreal const bottomColorIntensity,
                                                 qreal const topColorIntensity ) const
{
    Q_UNUSED(bottomColorIntensity);
    Q_UNUSED(topColorIntensity);
    // FIXME:
    //    return qMin( 1.0, qMax( 0.0, abs( bottomColorIntensity * bottomColorIntensity
    //                                      - topColorIntensity * topColorIntensity )));
    return 0.0;
}

qreal BleachBlending::blendChannel( qreal const bottomColorIntensity,
                                    qreal const topColorIntensity ) const
{
    // FIXME: "why this is the same formula as Screen Blending? Please correct.)"
    return 1.0 - ( 1.0 - bottomColorIntensity ) * ( 1.0 - topColorIntensity );
}

qreal DifferenceBlending::blendChannel( qreal const bottomColorIntensity,
                                        qreal const topColorIntensity ) const
{
    return qMax( qMin( qreal( 1.0 ), qreal( bottomColorIntensity - topColorIntensity + 0.5 )),
                 qreal( 0.0 ));
}

qreal EquivalenceBlending::blendChannel( qreal const bottomColorIntensity,
                                         qreal const topColorIntensity ) const
{
    return 1.0 - abs( bottomColorIntensity - topColorIntensity );
}

qreal HalfDifferenceBlending::blendChannel( qreal const bottomColorIntensity,
                                            qreal const topColorIntensity ) const
{
    return bottomColorIntensity + topColorIntensity
        - 2.0 * ( bottomColorIntensity * topColorIntensity );
}

// Special purpose blendings

void CloudsBlending::blend( QImage * const bottom, TextureTile const * const top ) const
{
    QImage const * const topImage = top->image();
    Q_ASSERT( topImage );
    Q_ASSERT( bottom->size() == topImage->size() );
    int const width = bottom->width();
    int const height = bottom->height();
    for ( int y = 0; y < height; ++y ) {
        for ( int x = 0; x < width; ++x ) {
            qreal const c = qRed( topImage->pixel( x, y )) / 255.0;
            QRgb const bottomPixel = bottom->pixel( x, y );
            int const bottomRed = qRed( bottomPixel );
            int const bottomGreen = qGreen( bottomPixel );
            int const bottomBlue = qBlue( bottomPixel );
            bottom->setPixel( x, y, qRgb(( int )( bottomRed + ( 255 - bottomRed ) * c ),
                                         ( int )( bottomGreen + ( 255 - bottomGreen ) * c ),
                                         ( int )( bottomBlue + ( 255 - bottomBlue ) * c )));
        }
    }
}

}
<|endoftext|>


<commit_before>/*
 RecognitionKernel.cpp

 Copyright (c) 2011 AIST  All Rights Reserved.
 Eclipse Public License v1.0 (http://www.eclipse.org/legal/epl-v10.html)

 $Date::                            $
*/
#include <vector>

#include <stdio.h>
#include "recogImage.h"
#include "stereo.h"
#include "circle.h"
#include "vertex.h"
#include "extractEdge.h"
#include "extractFeature_old.h"
#include "extractFeature.hpp"
#include "visionErrorCode.h"

#include <sys/time.h>

static unsigned long
GetTickCount()
{
  struct timeval t;
  double msec;
  gettimeofday(&t, NULL);
  msec = (double) (t.tv_sec * 1.0E3 + t.tv_usec * 1.0E-3);
  return (unsigned long) msec;
}

static void
freeEdgeMemory(uchar* edgL, uchar* edgR, uchar* edgV)
{
  free(edgL);
  free(edgR);
  free(edgV);
}

static void
freeFeatures2D(Features2D_old* L, Features2D_old* R, Features2D_old* V)
{
  destructFeatures(L);
  destructFeatures(R);
  destructFeatures(V);
}

//
//! 三次元物体認識の実行
//
//! 画像データから 三次元特徴を抽出し、モデルとマッチングを行い、
//! 結果を返す。
//
//! 引数:
//!   RecogImage** image  :       カメラ画像
//!   CalibParam&  calib  :       カメラキャリブレーションデータ
//!   Features3D&  model  :       対象物体モデル
//!   Parameters&  param  :       認識パラメータ
//
Match3Dresults
RecognitionKernel(RecogImage** image,
                  CalibParam&  calib,
                  Features3D&  model,
                  Parameters&  param)
{
  Match3Dresults Match = {0};
  int imageNum = calib.numOfCameras;
  int colsize = image[0]->colsize;
  int rowsize = image[0]->rowsize;

  param.colsize = colsize;
  param.rowsize = rowsize;
  param.imgsize = colsize * rowsize;

  // 画像の歪みを補正する
  undistortImage(image[0], &calib.CameraL, image[0], &calib.CameraL);
  undistortImage(image[1], &calib.CameraR, image[1], &calib.CameraR);
  if (imageNum > 2)
    {
      undistortImage(image[2], &calib.CameraV, image[2], &calib.CameraV);
    }

  unsigned char* edgeL = (unsigned char*) calloc(colsize * rowsize, sizeof(unsigned char));
  unsigned char* edgeR = (unsigned char*) calloc(colsize * rowsize, sizeof(unsigned char));
  if (edgeL == NULL || edgeR == NULL)
    {
      freeEdgeMemory(edgeL, edgeR, NULL);
      Match.error = VISION_MALLOC_ERROR;
      return Match;
    }

  unsigned char* edgeV = NULL;
  if (imageNum > 2)
    {
      edgeV = (unsigned char*) calloc(colsize * rowsize, sizeof(unsigned char));
      if (edgeV == NULL)
        {
          freeEdgeMemory(edgeL, edgeR, NULL);
          Match.error = VISION_MALLOC_ERROR;
          return Match;
        }
    }

  model.calib = &calib;
  model.edge[0] = edgeL;
  model.edge[1] = edgeR;
  if (imageNum > 2)
    {
      model.edge[2] = edgeV;
    }

  // 処理時間計測
  unsigned long stime, etime, dtime1, dtime2, dtime3;

  stime = GetTickCount();

  Features2D_old* features0 = NULL;
  Features2D_old* features1 = NULL;
  Features2D_old* features2 = NULL;

  ovgr::Features2D f0, f1, f2;
  ovgr::CorrespondingPair cp_bi, cp_LR, cp_RV, cp_VL;
  std::vector<const ovgr::CorrespondingPair*> cps(1);
  ovgr::CorrespondingSet cs;
  ovgr::CorrespondenceThresholds cpThs;

  const uchar* edges[3] = {0};
  const CameraParam* camParam[3] = {0};
  std::vector<const ovgr::Features2D*> features(2);

  StereoPairing pairing = param.pairing;

  // 画像が 2 枚の時は、強制的に DBL_LR にする。
  if (imageNum == 2)
    {
      pairing = DBL_LR;
    }

  // 特徴データ識別 ID 番号
  param.feature2D.id = 0;

  // 二次元特徴の抽出
  switch (pairing)
    {
    case DBL_LR:
      features0 = ImageToFeature2D_old(image[0]->pixel, edgeL, param, model);
      f0 = ovgr::create_new_features_from_old_one(features0, image[0]->pixel, &param);
      param.feature2D.id = 1;
      features1 = ImageToFeature2D_old(image[1]->pixel, edgeR, param, model);
      f1 = ovgr::create_new_features_from_old_one(features1, image[1]->pixel, &param);
      camParam[0] = &calib.CameraL;
      camParam[1] = &calib.CameraR;
      edges[0] = edgeL;
      edges[1] = edgeR;
      break;

    case DBL_LV:
      features0 = ImageToFeature2D_old(image[2]->pixel, edgeV, param, model);
      f0 = ovgr::create_new_features_from_old_one(features0, image[2]->pixel, &param);
      param.feature2D.id = 1;
      features1 = ImageToFeature2D_old(image[0]->pixel, edgeL, param, model);
      f1 = ovgr::create_new_features_from_old_one(features1, image[0]->pixel, &param);
      camParam[0] = &calib.CameraV;
      camParam[1] = &calib.CameraL;
      edges[0] = edgeV;
      edges[1] = edgeL;
      break;

    case DBL_RV:
      features0 = ImageToFeature2D_old(image[1]->pixel, edgeR, param, model);
      f0 = ovgr::create_new_features_from_old_one(features0, image[1]->pixel, &param);
      param.feature2D.id = 1;
      features1 = ImageToFeature2D_old(image[2]->pixel, edgeV, param, model);
      f1 = ovgr::create_new_features_from_old_one(features1, image[2]->pixel, &param);
      camParam[0] = &calib.CameraR;
      camParam[1] = &calib.CameraV;
      edges[0] = edgeR;
      edges[1] = edgeV;
      break;

    case TBL_OR:
    case TBL_AND:
      features0 = ImageToFeature2D_old(image[0]->pixel, edgeL, param, model);
      f0 = ovgr::create_new_features_from_old_one(features0, image[0]->pixel, &param);
      param.feature2D.id = 1;
      features1 = ImageToFeature2D_old(image[1]->pixel, edgeR, param, model);
      f1 = ovgr::create_new_features_from_old_one(features1, image[1]->pixel, &param);
      param.feature2D.id = 2;
      features2 = ImageToFeature2D_old(image[2]->pixel, edgeV, param, model);
      f2 = ovgr::create_new_features_from_old_one(features2, image[2]->pixel, &param);
      camParam[0] = &calib.CameraL;
      camParam[1] = &calib.CameraR;
      camParam[2] = &calib.CameraV;
      edges[0] = edgeL;
      edges[1] = edgeR;
      edges[2] = edgeV;
      break;

    default:
      freeEdgeMemory(edgeL, edgeR, edgeV);
      Match.error = VISION_PARAM_ERROR;
      return Match;
    }

#ifdef RECOGNITION_TEST
    printf( "RecognitionKernel:RECOGNITION_TEST:StereoPair=%d\n", pairing);
    fflush(stdout);
#endif

  // 2D 処理時間計測
  etime = GetTickCount();
  dtime1 = etime - stime;
  stime = etime;

  if (features0 == NULL || features1 == NULL)
    {
      freeFeatures2D(features0, features1, features2);
      freeEdgeMemory(edgeL, edgeR, edgeV);
      Match.error = VISION_MALLOC_ERROR;
      return Match;
    }
  else if (pairing == TBL_OR || pairing == TBL_AND)
    {
      if (features2 == NULL)
        {
          freeFeatures2D(features0, features1, features2);
          freeEdgeMemory(edgeL, edgeR, edgeV);
          Match.error = VISION_MALLOC_ERROR;
          return Match;
        }
    }

  // 各ペアで、二次元特徴のすべての組み合わせでステレオ対応を試みる。

  if (pairing != TBL_OR && pairing != TBL_AND)
    {
      cp_bi = make_corresponding_pairs(f0, *camParam[0], f1, *camParam[1], cpThs);
      cps[0] = &cp_bi;
      features[0] = &f0;
      features[1] = &f1;
    }
  else
    {
      cp_LR = make_corresponding_pairs(f0, calib.CameraL, f1, calib.CameraR, cpThs);
      cp_RV = make_corresponding_pairs(f1, calib.CameraR, f2, calib.CameraV, cpThs);
      cp_VL = make_corresponding_pairs(f2, calib.CameraV, f0, calib.CameraL, cpThs);
      cps.resize(3);
      cps[0] = &cp_LR;
      cps[1] = &cp_RV;
      cps[2] = &cp_VL;
      features.resize(3);
      features[0] = &f0;
      features[1] = &f1;
      features[2] = &f2;
    }

  if (pairing != TBL_AND)
    {
      // ２眼と３眼ＯＲ
      cs = ovgr::filter_corresponding_set(cps, ovgr::CorresOr);
    }
  else
    {
      // ３眼ＡＮＤ
      cs = ovgr::filter_corresponding_set(cps, ovgr::CorresAnd);
    }

  printf("# vertex: %d, ellipse: %d\n", cs.vertex.size(), cs.ellipse.size());

  Features3D scene = { 0 };

  // 二次元頂点のステレオデータから三次元頂点情報の復元
  if (model.numOfVertices > 0 && !(param.feature2D.no_search_features & NO_SEARCH_VERTEX))
    {
      reconstruct_hyperbola_to_vertex3D(features, cs, camParam, edges, &scene, param);
    }

  // 二次元楕円のステレオデータから三次元真円情報の復元
  if (model.numOfCircles > 0 && !(param.feature2D.no_search_features & NO_SEARCH_ELLIPSE))
    {
      reconstruct_ellipse2D_to_circle3D(features, cs, camParam, edges, &scene, param);
    }

  // 3D 処理時間計測
  etime = GetTickCount();
  dtime2 = etime - stime;

  // 二次元特徴メモリ解放
  freeFeatures2D(features0, features1, features2);

  // 三次元特徴とモデルデータのマッチングを行う。
  stime = GetTickCount();

  // シーンとモデルデータを照合して認識を実行する。
  Match = matchFeatures3D(scene, model, param);

  etime = GetTickCount();
  dtime3 = etime - stime;

  // 認識時間計測
  printf("認識時間： %lu msec. (2D: %lu + 3D: %lu + 認識: %lu)\n",
	 dtime1 + dtime2 + dtime3, dtime1, dtime2, dtime3);
  fflush(stdout);

  freeEdgeMemory(edgeL, edgeR, edgeV);
  freeFeatures3D(&scene);

  return Match;
}
<commit_msg>added OpenMP directives.<commit_after>/*
 RecognitionKernel.cpp

 Copyright (c) 2011 AIST  All Rights Reserved.
 Eclipse Public License v1.0 (http://www.eclipse.org/legal/epl-v10.html)

 $Date::                            $
*/
#include <vector>

#include <stdio.h>
#include "recogImage.h"
#include "stereo.h"
#include "circle.h"
#include "vertex.h"
#include "extractEdge.h"
#include "extractFeature_old.h"
#include "extractFeature.hpp"
#include "visionErrorCode.h"

#include <sys/time.h>

static unsigned long
GetTickCount()
{
  struct timeval t;
  double msec;
  gettimeofday(&t, NULL);
  msec = (double) (t.tv_sec * 1.0E3 + t.tv_usec * 1.0E-3);
  return (unsigned long) msec;
}

static void
freeEdgeMemory(uchar* edgL, uchar* edgR, uchar* edgV)
{
  free(edgL);
  free(edgR);
  free(edgV);
}

static void
freeFeatures2D(Features2D_old* L, Features2D_old* R, Features2D_old* V)
{
  destructFeatures(L);
  destructFeatures(R);
  destructFeatures(V);
}

//
//! 三次元物体認識の実行
//
//! 画像データから 三次元特徴を抽出し、モデルとマッチングを行い、
//! 結果を返す。
//
//! 引数:
//!   RecogImage** image  :       カメラ画像
//!   CalibParam&  calib  :       カメラキャリブレーションデータ
//!   Features3D&  model  :       対象物体モデル
//!   Parameters&  param  :       認識パラメータ
//
Match3Dresults
RecognitionKernel(RecogImage** image,
                  CalibParam&  calib,
                  Features3D&  model,
                  Parameters&  param)
{
  Match3Dresults Match = {0};
  int imageNum = calib.numOfCameras;
  int colsize = image[0]->colsize;
  int rowsize = image[0]->rowsize;

  param.colsize = colsize;
  param.rowsize = rowsize;
  param.imgsize = colsize * rowsize;

  // 画像の歪みを補正する
  undistortImage(image[0], &calib.CameraL, image[0], &calib.CameraL);
  undistortImage(image[1], &calib.CameraR, image[1], &calib.CameraR);
  if (imageNum > 2)
    {
      undistortImage(image[2], &calib.CameraV, image[2], &calib.CameraV);
    }

  unsigned char* edgeL = (unsigned char*) calloc(colsize * rowsize, sizeof(unsigned char));
  unsigned char* edgeR = (unsigned char*) calloc(colsize * rowsize, sizeof(unsigned char));
  if (edgeL == NULL || edgeR == NULL)
    {
      freeEdgeMemory(edgeL, edgeR, NULL);
      Match.error = VISION_MALLOC_ERROR;
      return Match;
    }

  unsigned char* edgeV = NULL;
  if (imageNum > 2)
    {
      edgeV = (unsigned char*) calloc(colsize * rowsize, sizeof(unsigned char));
      if (edgeV == NULL)
        {
          freeEdgeMemory(edgeL, edgeR, NULL);
          Match.error = VISION_MALLOC_ERROR;
          return Match;
        }
    }

  model.calib = &calib;
  model.edge[0] = edgeL;
  model.edge[1] = edgeR;
  if (imageNum > 2)
    {
      model.edge[2] = edgeV;
    }

  // 処理時間計測
  unsigned long stime, etime, dtime1, dtime2, dtime3;

  stime = GetTickCount();

  Features2D_old* features0 = NULL;
  Features2D_old* features1 = NULL;
  Features2D_old* features2 = NULL;

  ovgr::Features2D f0, f1, f2;
  ovgr::CorrespondingPair cp_bi, cp_LR, cp_RV, cp_VL;
  std::vector<const ovgr::CorrespondingPair*> cps(1);
  ovgr::CorrespondingSet cs;
  ovgr::CorrespondenceThresholds cpThs;

  const uchar* edges[3] = {0};
  const CameraParam* camParam[3] = {0};
  std::vector<const ovgr::Features2D*> features(2);

  StereoPairing pairing = param.pairing;

  // 画像が 2 枚の時は、強制的に DBL_LR にする。
  if (imageNum == 2)
    {
      pairing = DBL_LR;
    }

  // 特徴データ識別 ID 番号
  param.feature2D.id = 0;

  // 二次元特徴の抽出
#ifdef _OPENMP
#pragma omp parallel
#endif
  switch (pairing)
    {
    case DBL_LR:
#ifdef _OPENMP
#pragma omp sections
#endif
      {
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features0 = ImageToFeature2D_old(image[0]->pixel, edgeL, param, model);
          f0 = ovgr::create_new_features_from_old_one(features0, image[0]->pixel, &param);
        }
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features1 = ImageToFeature2D_old(image[1]->pixel, edgeR, param, model);
          f1 = ovgr::create_new_features_from_old_one(features1, image[1]->pixel, &param);
        }
      }
      param.feature2D.id = 1;
      camParam[0] = &calib.CameraL;
      camParam[1] = &calib.CameraR;
      edges[0] = edgeL;
      edges[1] = edgeR;
      break;

    case DBL_LV:
#ifdef _OPENMP
#pragma omp sections
#endif
      {
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features0 = ImageToFeature2D_old(image[2]->pixel, edgeV, param, model);
          f0 = ovgr::create_new_features_from_old_one(features0, image[2]->pixel, &param);
        }
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features1 = ImageToFeature2D_old(image[0]->pixel, edgeL, param, model);
          f1 = ovgr::create_new_features_from_old_one(features1, image[0]->pixel, &param);
        }
      }
      param.feature2D.id = 1;
      camParam[0] = &calib.CameraV;
      camParam[1] = &calib.CameraL;
      edges[0] = edgeV;
      edges[1] = edgeL;
      break;

    case DBL_RV:
#ifdef _OPENMP
#pragma omp sections
#endif
      {
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features0 = ImageToFeature2D_old(image[1]->pixel, edgeR, param, model);
          f0 = ovgr::create_new_features_from_old_one(features0, image[1]->pixel, &param);
        }
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features1 = ImageToFeature2D_old(image[2]->pixel, edgeV, param, model);
          f1 = ovgr::create_new_features_from_old_one(features1, image[2]->pixel, &param);
        }
      }
      param.feature2D.id = 1;
      camParam[0] = &calib.CameraR;
      camParam[1] = &calib.CameraV;
      edges[0] = edgeR;
      edges[1] = edgeV;
      break;

    case TBL_OR:
    case TBL_AND:
#ifdef _OPENMP
#pragma omp sections
#endif
      {
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features0 = ImageToFeature2D_old(image[0]->pixel, edgeL, param, model);
          f0 = ovgr::create_new_features_from_old_one(features0, image[0]->pixel, &param);
        }
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features1 = ImageToFeature2D_old(image[1]->pixel, edgeR, param, model);
          f1 = ovgr::create_new_features_from_old_one(features1, image[1]->pixel, &param);
        }
#ifdef _OPENMP
#pragma omp section
#endif
        {
          features2 = ImageToFeature2D_old(image[2]->pixel, edgeV, param, model);
          f2 = ovgr::create_new_features_from_old_one(features2, image[2]->pixel, &param);
        }
      }
      //param.feature2D.id = 1;
      param.feature2D.id = 2;
      camParam[0] = &calib.CameraL;
      camParam[1] = &calib.CameraR;
      camParam[2] = &calib.CameraV;
      edges[0] = edgeL;
      edges[1] = edgeR;
      edges[2] = edgeV;
      break;

    default:
      freeEdgeMemory(edgeL, edgeR, edgeV);
      Match.error = VISION_PARAM_ERROR;
    }

  if (Match.error == VISION_PARAM_ERROR)
    {
      return Match;
    }

#ifdef RECOGNITION_TEST
    printf( "RecognitionKernel:RECOGNITION_TEST:StereoPair=%d\n", pairing);
    fflush(stdout);
#endif

  // 2D 処理時間計測
  etime = GetTickCount();
  dtime1 = etime - stime;
  stime = etime;

  if (features0 == NULL || features1 == NULL)
    {
      freeFeatures2D(features0, features1, features2);
      freeEdgeMemory(edgeL, edgeR, edgeV);
      Match.error = VISION_MALLOC_ERROR;
      return Match;
    }
  else if (pairing == TBL_OR || pairing == TBL_AND)
    {
      if (features2 == NULL)
        {
          freeFeatures2D(features0, features1, features2);
          freeEdgeMemory(edgeL, edgeR, edgeV);
          Match.error = VISION_MALLOC_ERROR;
          return Match;
        }
    }

  // 各ペアで、二次元特徴のすべての組み合わせでステレオ対応を試みる。

  if (pairing != TBL_OR && pairing != TBL_AND)
    {
      cp_bi = make_corresponding_pairs(f0, *camParam[0], f1, *camParam[1], cpThs);
      cps[0] = &cp_bi;
      features[0] = &f0;
      features[1] = &f1;
    }
  else
    {
      cp_LR = make_corresponding_pairs(f0, calib.CameraL, f1, calib.CameraR, cpThs);
      cp_RV = make_corresponding_pairs(f1, calib.CameraR, f2, calib.CameraV, cpThs);
      cp_VL = make_corresponding_pairs(f2, calib.CameraV, f0, calib.CameraL, cpThs);
      cps.resize(3);
      cps[0] = &cp_LR;
      cps[1] = &cp_RV;
      cps[2] = &cp_VL;
      features.resize(3);
      features[0] = &f0;
      features[1] = &f1;
      features[2] = &f2;
    }

  if (pairing != TBL_AND)
    {
      // ２眼と３眼ＯＲ
      cs = ovgr::filter_corresponding_set(cps, ovgr::CorresOr);
    }
  else
    {
      // ３眼ＡＮＤ
      cs = ovgr::filter_corresponding_set(cps, ovgr::CorresAnd);
    }

  printf("# vertex: %d, ellipse: %d\n", cs.vertex.size(), cs.ellipse.size());

  Features3D scene = { 0 };

  // 二次元頂点のステレオデータから三次元頂点情報の復元
  if (model.numOfVertices > 0 && !(param.feature2D.no_search_features & NO_SEARCH_VERTEX))
    {
      reconstruct_hyperbola_to_vertex3D(features, cs, camParam, edges, &scene, param);
    }

  // 二次元楕円のステレオデータから三次元真円情報の復元
  if (model.numOfCircles > 0 && !(param.feature2D.no_search_features & NO_SEARCH_ELLIPSE))
    {
      reconstruct_ellipse2D_to_circle3D(features, cs, camParam, edges, &scene, param);
    }

  // 3D 処理時間計測
  etime = GetTickCount();
  dtime2 = etime - stime;

  // 二次元特徴メモリ解放
  freeFeatures2D(features0, features1, features2);

  // 三次元特徴とモデルデータのマッチングを行う。
  stime = GetTickCount();

  // シーンとモデルデータを照合して認識を実行する。
  Match = matchFeatures3D(scene, model, param);

  etime = GetTickCount();
  dtime3 = etime - stime;

  // 認識時間計測
  printf("認識時間： %lu msec. (2D: %lu + 3D: %lu + 認識: %lu)\n",
	 dtime1 + dtime2 + dtime3, dtime1, dtime2, dtime3);
  fflush(stdout);

  freeEdgeMemory(edgeL, edgeR, edgeV);
  freeFeatures3D(&scene);

  return Match;
}
<|endoftext|>


<commit_before>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2011 Alesis Novik
 * Copyright (C) 2011 Berlin Institute of Technology and Max-Planck-Society
 */
#include "lib/config.h"

#ifdef HAVE_LAPACK

#include "distributions/Gaussian.h"
#include "lib/Mathematics.h"
#include "base/Parameter.h"

using namespace shogun;

CGaussian::CGaussian() : CDistribution(), m_constant(0),
m_cov(NULL), m_cov_rows(0), m_cov_cols(0), m_cov_inverse(NULL),
m_cov_inverse_rows(0), m_cov_inverse_cols(0), m_mean(NULL),
m_mean_length(0)
{
}

CGaussian::CGaussian(float64_t* mean, int32_t mean_length,
					float64_t* cov, int32_t cov_rows, int32_t cov_cols) : CDistribution(),
					m_cov_inverse(NULL)
{
	ASSERT(mean_length == cov_rows);
	ASSERT(cov_rows == cov_cols);
	m_mean = new float64_t[mean_length];
	memcpy(m_mean, mean, sizeof(float64_t)*mean_length);
	m_cov = new float64_t[cov_rows*cov_cols];
	memcpy(m_cov, cov, sizeof(float64_t)*cov_rows*cov_cols);
	m_mean_length = mean_length;
	m_cov_rows = cov_rows;
	m_cov_cols = cov_cols;
	init();
	register_params();
}

void CGaussian::init()
{
	delete[] m_cov_inverse;

	m_cov_inverse_rows = m_cov_cols;
	m_cov_inverse_cols = m_cov_rows;

	m_cov_inverse = new float64_t[m_cov_rows*m_cov_cols];
	memcpy(m_cov_inverse, m_cov, sizeof(float64_t)*m_cov_rows*m_cov_cols);
	int32_t result = clapack_dpotrf(CblasRowMajor, CblasLower, m_cov_rows, m_cov_inverse, m_cov_rows);
	m_constant = 1;

	for (int i = 0; i < m_cov_rows; i++)
		m_constant *= m_cov_inverse[i*m_cov_rows+i];
	m_constant = 1/m_constant;
	m_constant *= pow(2*M_PI, (float64_t) -m_cov_rows/2);

	result = clapack_dpotri(CblasRowMajor, CblasLower, m_cov_rows, m_cov_inverse, m_cov_rows);
}

CGaussian::~CGaussian()
{
	delete[] m_cov_inverse;
	delete[] m_cov;
	delete[] m_mean;
}

bool CGaussian::train(CFeatures* data)
{
	// init features with data if necessary and assure type is correct
	if (data)
	{
		if (!data->has_property(FP_DOT))
				SG_ERROR("Specified features are not of type CDotFeatures\n");
		set_features(data);
	}
	CDotFeatures* dotdata = (CDotFeatures *) data;

	delete[] m_mean;
	delete[] m_cov;

	dotdata->get_mean(&m_mean, &m_mean_length);
	dotdata->get_cov(&m_cov, &m_cov_rows, &m_cov_cols);

	init();

	return true;
}

int32_t CGaussian::get_num_model_parameters()
{
	return m_cov_rows*m_cov_cols+m_mean_length;
}

float64_t CGaussian::get_log_model_parameter(int32_t num_param)
{
	if (num_param<m_mean_length)
		return CMath::log(m_mean[num_param]);
	else
		return CMath::log(m_cov[num_param-m_mean_length]);
}

float64_t CGaussian::get_log_derivative(int32_t num_param, int32_t num_example)
{
	return 0;
}

float64_t CGaussian::get_likelihood_example(int32_t num_example)
{
	ASSERT(features->has_property(FP_DOT));
	float64_t* point;
	int32_t point_len;
	((CDotFeatures *)features)->get_feature_vector(&point, &point_len, num_example);
	float64_t answer = compute_PDF(point, point_len);
	delete[] point;
	return answer;
}

float64_t CGaussian::compute_PDF(float64_t* point, int32_t point_len)
{
	ASSERT(m_mean && m_cov);
	ASSERT(point_len == m_mean_length);
	float64_t* difference = new float64_t[m_mean_length];
	memcpy(difference, point, sizeof(float64_t)*m_mean_length);
	float64_t* result = new float64_t[m_mean_length];

	for (int i = 0; i < m_mean_length; i++)
		difference[i] -= m_mean[i];

	cblas_dsymv(CblasRowMajor, CblasLower, m_mean_length, -1.0/2.0, m_cov_inverse, m_mean_length,
				difference, 1, 0, result, 1);

	float64_t answer = m_constant * exp(cblas_ddot(m_mean_length, difference, 1, result, 1));

	delete[] difference;
	delete[] result;

	return answer;
}

void CGaussian::register_params()
{
	m_parameters->add_matrix(&m_cov, &m_cov_rows, &m_cov_cols, "m_cov", "Covariance.");
	m_parameters->add_matrix(&m_cov_inverse, &m_cov_inverse_rows, &m_cov_inverse_cols, "m_cov_inverse", "Covariance inverse.");
	m_parameters->add_vector(&m_mean, &m_mean_length, "m_mean", "Mean.");
	m_parameters->add(&m_constant, "m_constant", "Constant part.");
}
#endif
<commit_msg>Improved readability of Gaussian.cpp<commit_after>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2011 Alesis Novik
 * Copyright (C) 2011 Berlin Institute of Technology and Max-Planck-Society
 */
#include "lib/config.h"

#ifdef HAVE_LAPACK

#include "distributions/Gaussian.h"
#include "lib/Mathematics.h"
#include "base/Parameter.h"

using namespace shogun;

CGaussian::CGaussian() : CDistribution(), m_constant(0),
m_cov(NULL), m_cov_rows(0), m_cov_cols(0), m_cov_inverse(NULL),
m_cov_inverse_rows(0), m_cov_inverse_cols(0), m_mean(NULL),
m_mean_length(0)
{
}

CGaussian::CGaussian(float64_t* mean, int32_t mean_length,
					float64_t* cov, int32_t cov_rows, int32_t cov_cols) : CDistribution(),
					m_cov_inverse(NULL)
{
	ASSERT(mean_length == cov_rows);
	ASSERT(cov_rows == cov_cols);
	m_mean = new float64_t[mean_length];
	memcpy(m_mean, mean, sizeof(float64_t)*mean_length);
	m_cov = new float64_t[cov_rows*cov_cols];
	memcpy(m_cov, cov, sizeof(float64_t)*cov_rows*cov_cols);
	m_mean_length = mean_length;
	m_cov_rows = cov_rows;
	m_cov_cols = cov_cols;
	init();
	register_params();
}

void CGaussian::init()
{
	delete[] m_cov_inverse;

	m_cov_inverse_rows = m_cov_cols;
	m_cov_inverse_cols = m_cov_rows;

	m_cov_inverse = new float64_t[m_cov_rows*m_cov_cols];
	memcpy(m_cov_inverse, m_cov, sizeof(float64_t)*m_cov_rows*m_cov_cols);
	int32_t result = clapack_dpotrf(CblasRowMajor, CblasLower, m_cov_rows, m_cov_inverse, m_cov_rows);
	m_constant = 1;

	for (int i = 0; i < m_cov_rows; i++)
		m_constant *= m_cov_inverse[i*m_cov_rows+i];

	m_constant = 1/m_constant;
	m_constant *= pow(2*M_PI, (float64_t) -m_cov_rows/2);

	result = clapack_dpotri(CblasRowMajor, CblasLower, m_cov_rows, m_cov_inverse, m_cov_rows);
}

CGaussian::~CGaussian()
{
	delete[] m_cov_inverse;
	delete[] m_cov;
	delete[] m_mean;
}

bool CGaussian::train(CFeatures* data)
{
	// init features with data if necessary and assure type is correct
	if (data)
	{
		if (!data->has_property(FP_DOT))
				SG_ERROR("Specified features are not of type CDotFeatures\n");
		set_features(data);
	}
	CDotFeatures* dotdata = (CDotFeatures *) data;

	delete[] m_mean;
	delete[] m_cov;

	dotdata->get_mean(&m_mean, &m_mean_length);
	dotdata->get_cov(&m_cov, &m_cov_rows, &m_cov_cols);

	init();

	return true;
}

int32_t CGaussian::get_num_model_parameters()
{
	return m_cov_rows*m_cov_cols+m_mean_length;
}

float64_t CGaussian::get_log_model_parameter(int32_t num_param)
{
	if (num_param<m_mean_length)
		return CMath::log(m_mean[num_param]);
	else
		return CMath::log(m_cov[num_param-m_mean_length]);
}

float64_t CGaussian::get_log_derivative(int32_t num_param, int32_t num_example)
{
	return 0;
}

float64_t CGaussian::get_likelihood_example(int32_t num_example)
{
	ASSERT(features->has_property(FP_DOT));
	float64_t* point;
	int32_t point_len;
	((CDotFeatures *)features)->get_feature_vector(&point, &point_len, num_example);
	float64_t answer = compute_PDF(point, point_len);
	delete[] point;
	return answer;
}

float64_t CGaussian::compute_PDF(float64_t* point, int32_t point_len)
{
	ASSERT(m_mean && m_cov);
	ASSERT(point_len == m_mean_length);
	float64_t* difference = new float64_t[m_mean_length];
	memcpy(difference, point, sizeof(float64_t)*m_mean_length);
	float64_t* result = new float64_t[m_mean_length];

	for (int i = 0; i < m_mean_length; i++)
		difference[i] -= m_mean[i];

	cblas_dsymv(CblasRowMajor, CblasLower, m_mean_length, -1.0/2.0, m_cov_inverse, m_mean_length,
				difference, 1, 0, result, 1);

	float64_t answer = m_constant * exp(cblas_ddot(m_mean_length, difference, 1, result, 1));

	delete[] difference;
	delete[] result;

	return answer;
}

void CGaussian::register_params()
{
	m_parameters->add_matrix(&m_cov, &m_cov_rows, &m_cov_cols, "m_cov", "Covariance.");
	m_parameters->add_matrix(&m_cov_inverse, &m_cov_inverse_rows, &m_cov_inverse_cols, "m_cov_inverse", "Covariance inverse.");
	m_parameters->add_vector(&m_mean, &m_mean_length, "m_mean", "Mean.");
	m_parameters->add(&m_constant, "m_constant", "Constant part.");
}
#endif
<|endoftext|>


<commit_before>#pragma once
//=====================================================================//
/*!	@file
	@brief	ethernet_base クラス @n
			Copyright 2017 Kunihito Hiramatsu
	@author	平松邦仁 (hira@rvf-rc45.net)
*/
//=====================================================================//
#include "ethernet.hpp"

namespace net {

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief  ethernet_base
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	class ethernet_base {

		ethernet&	ethernet_;

		uint16_t	port_;
		uint32_t	cepid_;

		int16_t set_tcp_crep_(ID repid, UH portno) {
			if (repid == 0 || repid > __tcprepn) {
				return -1;
			}
			tcp_crep[repid - 1].myaddr.portno = portno;
			return 0;
		}

		int16_t set_tcp_ccep_(ID cepid, UB ch, int rbufsz) {
			if (cepid == 0 || cepid > __tcpcepn){
				return -1;
			}
			tcp_ccep[cepid - 1].cepatr = ch;
			tcp_ccep[cepid - 1].rbufsz = rbufsz;
			return 0;
		}

	public:
		//-----------------------------------------------------------------//
		/*!
			@brief  コンストラクター
		*/
		//-----------------------------------------------------------------//
		ethernet_base(ethernet& e) : ethernet_(e), port_(0), cepid_(0) { }

		//-----------------------------------------------------------------//
		/*!
			@brief  ethernet を参照
			@return ethernet
		*/
		//-----------------------------------------------------------------//
		ethernet& at_ethernet() { return ethernet_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  ポート番号を取得
			@return ポート番号
		*/
		//-----------------------------------------------------------------//
		uint16_t get_port() const { return port_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  CEP ID を取得
			@return CEP ID
		*/
		//-----------------------------------------------------------------//
		uint32_t get_cepid() const { return cepid_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  開始
			@param[in]	port	ポート番号
		*/
		//-----------------------------------------------------------------//
		void begin(uint16_t port)
		{
			port_ = port;

#ifdef ETHER_DEBUG
			utils::format("ethernet_server: begin: %d") % static_cast<int>(port);
#endif
			if(port_ > 0) {
				cepid_ = ethernet_.new_connection();
				if(cepid_ == 0) {
					utils::format("New connection fail\n");
					return;
				}

				/* initialize cep status */
				ethernet_.at_cep(cepid_).status = T4_CLOSED;
				/* Initialize TCP reception point */
#ifdef ETHER_DEBUG
				utils::format("set_tcp_crep_(): %d") % cepid_;
#endif
				if(set_tcp_crep_(cepid_, port_) != E_OK) {
					utils::format("set_tcp_crep() fail: %d\n") % cepid_;
					return;
				}

				/* Initialize TCP communication end point */
#ifdef ETHER_DEBUG
				utils::format("set_tcp_ccep(): %d\n") % cepid_;
#endif
				if(set_tcp_ccep_(cepid_, 0, ethernet::TCP_MSS) != E_OK) {
					utils::format("set_tcp_ccep_() fail: %d\n") % cepid_;
					return;
				}
			} else {
				utils::format("port == 0\n");
			}
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  終了
		*/
		//-----------------------------------------------------------------//
		void end()
		{
			ethernet_.end_connection(cepid_);
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  接続確認
			@return 接続中なら「true」
		*/
		//-----------------------------------------------------------------//
		bool connected()
		{
			int ercd = tcp_read_stat(cepid_);
			bool res;
			if(ercd == T4_TCPS_ESTABLISHED || ercd == T4_TCPS_CLOSE_WAIT) {
				res = true;
			} else {
				res = false;
			}
			return res;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  書き込み
			@param[in]	byte	データ
			@return 書き込み成功なら「１」
		*/
		//-----------------------------------------------------------------//
		int32_t write(uint8_t data)
		{
			int32_t ercd = tcp_send_data(cepid_, &data, 1, TMO_FEVR);
			return ercd;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  １バイト書き込み
			@return 書き込み成功なら「１」
		*/
		//-----------------------------------------------------------------//
		int32_t write(const void* buffer, uint32_t size)
		{
			int32_t ercd;
			uint32_t current_send_size = 0;

			const uint8_t* p = static_cast<const uint8_t*>(buffer);
			if (size <= 0x7fff) {
				ercd = tcp_send_data(cepid_, p, size, TMO_FEVR);
			} else {
				while (size > current_send_size) {
					if((size - current_send_size) > 0x7fff) {
						ercd = tcp_send_data(cepid_, (p + current_send_size), 0x7fff, TMO_FEVR);
					} else {
						ercd = tcp_send_data(cepid_, (p + current_send_size),
									(size - current_send_size), TMO_FEVR);
					}
					if(ercd < 0) {
						break; // error
					}
					current_send_size += static_cast<uint32_t>(ercd);
				}
			}
			return current_send_size;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  １バイト読み込み
			@return 読み込みデータ
		*/
		//-----------------------------------------------------------------//
		int read()
		{
#if defined(ETHER_DEBUG)
			utils::format("EthernetClinet::read()");
			utils::format("%u : ") % static_cast<uint32_t>(millis());
#endif
			ethernet::CEP& cep = ethernet_.at_cep(cepid_);
			int32_t ercd = tcp_recv_data(cepid_, cep.recv_buf, 1, TMO_FEVR);
#if defined(ETHER_DEBUG)
			utils::format("%u\n") % static_cast<uint32_t>(millis());
#endif
			int recv_data;
			if(ercd <= 0) {
				recv_data = -1;
			} else {
				recv_data = static_cast<int>(cep.recv_buf[0]);
			}
			return recv_data;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  読み込み
			@param[out]	buf		読み込み先
			@param[in]	size	読み込みサイズ
			@return 読み込みサイズ
		*/
		//-----------------------------------------------------------------//
		int read(void* buf, size_t size)
		{
			int32_t ercd = tcp_recv_data(cepid_, buf, size, TMO_FEVR);
			if(ercd <= 0) {
				ercd = -1;
			}
			return ercd;
		}

		int print(const char* t) {
			if(t == nullptr) return 0;
			size_t l = strlen(t);
			return write(t, l);
		}

		int print(int value) {
			char tmp[16];
			utils::format("%d", tmp, sizeof(tmp)) % value;
			return print(tmp);
		}

		/* get the length of the string * s return. '\ 0' is not included in length. */
		int println(const char* t = nullptr)
		{
			if(t == nullptr) {
				return write("\r\n", 2);
			}

			size_t l = strlen(t);
			char tmp[l + 2];
			strcpy(tmp, t);
			tmp[l + 0] = '\r';
			tmp[l + 1] = '\n';
			return write(tmp, l + 2);
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  有効なデータがあるか
			@return 有効なデータ数
		*/
		//-----------------------------------------------------------------//
		int available()
		{
			int res = 0;
			if(connected()) {
				res = head_tcb[cepid_ - 1].rdsize;
				int ercd = tcp_read_stat(cepid_);

				if(res == 0 && ercd == T4_TCPS_CLOSE_WAIT) {
					tcp_sht_cep(cepid_, TMO_FEVR);
				}
			}
			return res;
		}
	};
}
<commit_msg>cleanup timeout handling<commit_after>#pragma once
//=====================================================================//
/*!	@file
	@brief	ethernet_base クラス @n
			Copyright 2017 Kunihito Hiramatsu
	@author	平松邦仁 (hira@rvf-rc45.net)
*/
//=====================================================================//
#include "ethernet.hpp"

namespace net {

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief  ethernet_base
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	class ethernet_base {

		ethernet&	ethernet_;

		uint16_t	port_;
		uint32_t	cepid_;

		int16_t set_tcp_crep_(ID repid, UH portno) {
			if (repid == 0 || repid > __tcprepn) {
				return -1;
			}
			tcp_crep[repid - 1].myaddr.portno = portno;
			return 0;
		}

		int16_t set_tcp_ccep_(ID cepid, UB ch, int rbufsz) {
			if (cepid == 0 || cepid > __tcpcepn){
				return -1;
			}
			tcp_ccep[cepid - 1].cepatr = ch;
			tcp_ccep[cepid - 1].rbufsz = rbufsz;
			return 0;
		}

	public:
		//-----------------------------------------------------------------//
		/*!
			@brief  コンストラクター
		*/
		//-----------------------------------------------------------------//
		ethernet_base(ethernet& e) : ethernet_(e), port_(0), cepid_(0) { }

		//-----------------------------------------------------------------//
		/*!
			@brief  ethernet を参照
			@return ethernet
		*/
		//-----------------------------------------------------------------//
		ethernet& at_ethernet() { return ethernet_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  ポート番号を取得
			@return ポート番号
		*/
		//-----------------------------------------------------------------//
		uint16_t get_port() const { return port_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  CEP ID を取得
			@return CEP ID
		*/
		//-----------------------------------------------------------------//
		uint32_t get_cepid() const { return cepid_; }

		//-----------------------------------------------------------------//
		/*!
			@brief  開始
			@param[in]	port	ポート番号
		*/
		//-----------------------------------------------------------------//
		void begin(uint16_t port)
		{
			port_ = port;

#ifdef ETHER_DEBUG
			utils::format("ethernet_server: begin: %d") % static_cast<int>(port);
#endif
			if(port_ > 0) {
				cepid_ = ethernet_.new_connection();
				if(cepid_ == 0) {
					utils::format("New connection fail\n");
					return;
				}

				/* initialize cep status */
				ethernet_.at_cep(cepid_).status = T4_CLOSED;
				/* Initialize TCP reception point */
#ifdef ETHER_DEBUG
				utils::format("set_tcp_crep_(): %d") % cepid_;
#endif
				if(set_tcp_crep_(cepid_, port_) != E_OK) {
					utils::format("set_tcp_crep() fail: %d\n") % cepid_;
					return;
				}

				/* Initialize TCP communication end point */
#ifdef ETHER_DEBUG
				utils::format("set_tcp_ccep(): %d\n") % cepid_;
#endif
				if(set_tcp_ccep_(cepid_, 0, ethernet::TCP_MSS) != E_OK) {
					utils::format("set_tcp_ccep_() fail: %d\n") % cepid_;
					return;
				}
			} else {
				utils::format("port == 0\n");
			}
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  終了
		*/
		//-----------------------------------------------------------------//
		void end()
		{
			ethernet_.end_connection(cepid_);
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  接続確認
			@return 接続中なら「true」
		*/
		//-----------------------------------------------------------------//
		bool connected()
		{
			bool ret = false;
			int ercd = tcp_read_stat(cepid_);
			if(ercd == T4_TCPS_ESTABLISHED || ercd == T4_TCPS_CLOSE_WAIT) {
				ret = true;
			}
			return ret;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  書き込み
			@param[in]	byte	データ
			@return 書き込み成功なら「１」
		*/
		//-----------------------------------------------------------------//
		int32_t write(uint8_t data)
		{
			int32_t ercd = tcp_send_data(cepid_, &data, 1, TMO_FEVR);
			return ercd;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  １バイト書き込み
			@return 書き込み成功なら「１」
		*/
		//-----------------------------------------------------------------//
		int32_t write(const void* buffer, uint32_t size)
		{
			int32_t ercd;
			uint32_t current_send_size = 0;

			const uint8_t* p = static_cast<const uint8_t*>(buffer);
			if (size <= 0x7fff) {
				ercd = tcp_send_data(cepid_, p, size, TMO_FEVR);
			} else {
				while (size > current_send_size) {
					if((size - current_send_size) > 0x7fff) {
						ercd = tcp_send_data(cepid_, (p + current_send_size), 0x7fff, TMO_FEVR);
					} else {
						ercd = tcp_send_data(cepid_, (p + current_send_size),
									(size - current_send_size), TMO_FEVR);
					}
					if(ercd < 0) {
						break; // error
					}
					current_send_size += static_cast<uint32_t>(ercd);
				}
			}
			return current_send_size;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  １バイト読み込み
			@return 読み込みデータ
		*/
		//-----------------------------------------------------------------//
		int read()
		{
#if defined(ETHER_DEBUG)
			utils::format("EthernetClinet::read()");
			utils::format("%u : ") % static_cast<uint32_t>(millis());
#endif
			ethernet::CEP& cep = ethernet_.at_cep(cepid_);
			int32_t ercd = tcp_recv_data(cepid_, cep.recv_buf, 1, TMO_FEVR);
#if defined(ETHER_DEBUG)
			utils::format("%u\n") % static_cast<uint32_t>(millis());
#endif
			int recv_data;
			if(ercd <= 0) {
				recv_data = -1;
			} else {
				recv_data = static_cast<int>(cep.recv_buf[0]);
			}
			return recv_data;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  読み込み
			@param[out]	buf		読み込み先
			@param[in]	size	読み込みサイズ
			@return 読み込みサイズ
		*/
		//-----------------------------------------------------------------//
		int read(void* buf, size_t size)
		{
			int32_t ercd = tcp_recv_data(cepid_, buf, size, TMO_FEVR);
			if(ercd <= 0) {
				ercd = -1;
			}
			return ercd;
		}

		int print(const char* t) {
			if(t == nullptr) return 0;
			size_t l = strlen(t);
			return write(t, l);
		}

		int print(int value) {
			char tmp[16];
			utils::format("%d", tmp, sizeof(tmp)) % value;
			return print(tmp);
		}

		/* get the length of the string * s return. '\ 0' is not included in length. */
		int println(const char* t = nullptr)
		{
			if(t == nullptr) {
				return write("\r\n", 2);
			}

			size_t l = strlen(t);
			char tmp[l + 2];
			strcpy(tmp, t);
			tmp[l + 0] = '\r';
			tmp[l + 1] = '\n';
			return write(tmp, l + 2);
		}

		//-----------------------------------------------------------------//
		/*!
			@brief  有効なデータがあるか
			@return 有効なデータ数
		*/
		//-----------------------------------------------------------------//
		int available()
		{
			int ret = 0;
			if(connected()) {
				ret = head_tcb[cepid_ - 1].rdsize;
				int ercd = tcp_read_stat(cepid_);

				if(ret == 0 && ercd == T4_TCPS_CLOSE_WAIT) {
					tcp_sht_cep(cepid_, TMO_FEVR);
				}
			}
			return ret;
		}
	};
}
<|endoftext|>


<commit_before>//-----------------------------------------------------------------------------
// Copyright (c) 2014 Michael G. Brehm
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//-----------------------------------------------------------------------------

#include "stdafx.h"						// Include project pre-compiled headers
#include "ElfImage.h"					// Include ELFImage declarations

#pragma warning(push, 4)				// Enable maximum compiler warnings

//-----------------------------------------------------------------------------
// Explicit Instantiations

#ifdef _M_X64
template ElfImageT<Elf64_Ehdr, Elf64_Phdr, Elf64_Shdr>;
#else
template ElfImageT<Elf32_Ehdr, Elf32_Phdr, Elf32_Shdr>;
#endif

//-----------------------------------------------------------------------------
// ElfImageT Constructor (private)
//
// Arguments:
//
//	mapping		- Memory-mapped ELF image file to load
//	length		- Optional length to use from mapping

template <class ehdr_t, class phdr_t, class shdr_t>
ElfImageT<ehdr_t, phdr_t, shdr_t>::ElfImageT(std::shared_ptr<MappedFile>& mapping, size_t length)
{
	// Create a read-only view of the memory-mapped ELF image
	std::unique_ptr<MappedFileView> view(MappedFileView::Create(mapping, FILE_MAP_READ, 0, length));
	intptr_t viewptr = intptr_t(view->Pointer);

	// Validate the ELF header that should be at the start of the view
	ValidateHeader(view->Pointer, view->Length);
	ehdr_t*	elfheader = reinterpret_cast<ehdr_t*>(view->Pointer);

	// Create a collection of all the ELF program headers
	std::vector<phdr_t>	progheaders;
	for(int index = 0; index < elfheader->e_phnum; index++) {

		// Get the offset into the file for the next program header and check it
		intptr_t offset = elfheader->e_phoff + (index * elfheader->e_phentsize);
		if(view->Length < (offset + sizeof(phdr_t))) throw Exception(E_ELF_TRUNCATED);

		// Insert the program header into the collection
		progheaders.push_back(*reinterpret_cast<phdr_t*>(viewptr + offset));
	}

	// Determine the memory requirements of the loaded image
	intptr_t minpaddr = 0, maxpaddr = 0;
	for_each(progheaders.begin(), progheaders.end(), [&](phdr_t& phdr) {

		if(phdr.p_type == PT_LOAD) {

			// Calculate the minimum and maximum physical addresses of the segment
			// and adjust the overall minimum and maximums accordingly
			intptr_t minsegaddr(phdr.p_paddr);
			intptr_t maxsegaddr(phdr.p_paddr + phdr.p_memsz);

			minpaddr = (minpaddr == 0) ? minsegaddr : min(minsegaddr, minpaddr);
			maxpaddr = (maxpaddr == 0) ? maxsegaddr : max(maxsegaddr, maxpaddr);
		}
	});

	// Attempt to reserve the virtual memory for the image.  First try to use the physical address specified
	// by the image to avoid relocations, but go ahead and put it anywhere if that doesn't work
	try { m_region.reset(MemoryRegion::Reserve(reinterpret_cast<void*>(minpaddr), maxpaddr - minpaddr)); }
	catch(Exception&) { m_region.reset(MemoryRegion::Reserve(maxpaddr - minpaddr, MEM_TOP_DOWN)); }

	// Determine the delta between the allocated region and the original base physical address
	intptr_t regionbase = intptr_t(m_region->Pointer);
	intptr_t paddrdelta = minpaddr - regionbase;

	// Load the PT_LOAD segments into virtual memory
	for_each(progheaders.begin(), progheaders.end(), [&](phdr_t& phdr) {

		if((phdr.p_type == PT_LOAD) && (phdr.p_memsz)) {

			// Get the base address of the loadable segment and commit the virtual memory
			intptr_t segbase = phdr.p_paddr - paddrdelta;
			m_region->Commit(reinterpret_cast<void*>(segbase), phdr.p_memsz, PAGE_READWRITE);

			// Not all segments contain data that needs to be copied from the source image
			if(phdr.p_filesz) {

				// Ensure that there is enough source data to copy and copy it into the segment region
				if(view->Length < (phdr.p_offset + phdr.p_filesz)) throw Exception(E_ELF_TRUNCATED);
				memcpy(reinterpret_cast<void*>(segbase), reinterpret_cast<void*>(viewptr + phdr.p_offset), phdr.p_filesz);
			}

			// Memory that was not loaded from the ELF image must be initialized to zero
			memset(reinterpret_cast<void*>(segbase + phdr.p_filesz), 0, phdr.p_memsz - phdr.p_filesz);

			// Attempt to apply the proper virtual memory protection flags to the segment
			try { m_region->Protect(reinterpret_cast<void*>(segbase), phdr.p_memsz, FlagsToProtection(phdr.p_flags)); }
			catch(Exception& ex) { throw Exception(ex, E_ELFSEGMENTPROTECTION); }
		}
	});

	// Calculate the address of the image entry point, if one has been specified in the header
	m_entry = (elfheader->e_entry) ? reinterpret_cast<EntryPoint>(elfheader->e_entry - paddrdelta) : nullptr;
}

//---------------------------------------------------------------------------
// ElfImageT::AlignDown (private, static)
//
// Aligns an offset down to the specified alignment
//
// Arguments:
//
//	address		- Address to be aligned
//	alignment	- Alignment

template <class ehdr_t, class phdr_t, class shdr_t>
uintptr_t ElfImageT<ehdr_t, phdr_t, shdr_t>::AlignDown(uintptr_t address, size_t alignment)
{
	if(alignment < 1) throw Exception(E_ARGUMENTOUTOFRANGE, _T("alignment"));

	if(address < alignment) return 0;
	else return AlignUp(address - (alignment - 1), alignment);
}

//---------------------------------------------------------------------------
// ElfImageT::AlignUp (private, static)
//
// Aligns an offset up to the specified alignment
//
// Arguments:
//
//	address		- Address to be aligned
//	alignment	- Alignment

template <class ehdr_t, class phdr_t, class shdr_t>
uintptr_t ElfImageT<ehdr_t, phdr_t, shdr_t>::AlignUp(uintptr_t address, size_t alignment)
{
	if(alignment < 1) throw Exception(E_ARGUMENTOUTOFRANGE, _T("alignment"));

	if(address == 0) return 0;
	else return address + ((alignment - (address % alignment)) % alignment);
}

//-----------------------------------------------------------------------------
// ElfImageT::FlagsToProtection (private, static)
//
// Converts an ELF program header p_flags into VirtualAlloc() protection flags
//
// Arguments:
//
//	flags		- ELF program header p_flags value

template <class ehdr_t, class phdr_t, class shdr_t>
DWORD ElfImageT<ehdr_t, phdr_t, shdr_t>::FlagsToProtection(uint32_t flags)
{
	switch(flags) {

		case PF_X:					return PAGE_EXECUTE;
		case PF_W :					return PAGE_READWRITE;
		case PF_R :					return PAGE_READONLY;
		case PF_X | PF_W :			return PAGE_EXECUTE_READWRITE;
		case PF_X | PF_R :			return PAGE_EXECUTE_READ;
		case PF_W | PF_R :			return PAGE_READWRITE;
		case PF_X | PF_W | PF_R :	return PAGE_EXECUTE_READWRITE;
	}

	return PAGE_NOACCESS;
}

//-----------------------------------------------------------------------------
// ElfImageT::Load (static)
//
// Parses and loads the specified ELF image into virtual memory
//
// Arguments:
//
//	mapping			- Memory-mapped image file
//	length			- Optional length of mapping to use

template <class ehdr_t, class phdr_t, class shdr_t>
ElfImageT<ehdr_t, phdr_t, shdr_t>* ElfImageT<ehdr_t, phdr_t, shdr_t>::Load(std::shared_ptr<MappedFile>& mapping, size_t length)
{
	// All the validation code has been moved into the constructor
	return new ElfImageT<ehdr_t, phdr_t, shdr_t>(mapping, length);
}

//-----------------------------------------------------------------------------
// ElfImageT::TryValidateHeader (static)
//
// Validates that the provided pointer points to a 64-bit ELF binary header --
// returns a boolean flag rather than throwing an exception
//
// Arguments:
//
//	base		- Base address of the ELF binary to test
//	length		- Length of the buffer pointed to by base

template <class ehdr_t, class phdr_t, class shdr_t>
bool ElfImageT<ehdr_t, phdr_t, shdr_t>::TryValidateHeader(const void* base, size_t length)
{
	// Invoke the version that throws exceptions and just eat them
	try { ValidateHeader(base, length); return true; }
	catch(Exception&) { return false; }
}

//-----------------------------------------------------------------------------
// ElfImageT::ValidateHeader (static)
//
// Validates that the provided pointer points to a 64-bit ELF binary header
//
// Arguments:
//
//	base		- Base address of the ELF binary to test
//	length		- Length of the buffer pointed to by base

template <class ehdr_t, class phdr_t, class shdr_t>
void ElfImageT<ehdr_t, phdr_t, shdr_t>::ValidateHeader(const void* base, size_t length)
{
	const ehdr_t*				header;					// ELF header structure

	// Check the length and cast out a pointer to the header structure
	if(length < sizeof(ehdr_t)) throw Exception(E_TRUNCATEDELFHEADER);
	header = reinterpret_cast<const ehdr_t*>(base);

	// Check the ELF header magic number
	if(memcmp(&header->e_ident[EI_MAG0], ELFMAG, SELFMAG) != 0) throw Exception(E_INVALIDELFMAGIC);

	// Verify the ELF class matches the build configuration (32-bit vs. 64-bit)
	int elfclass = (sizeof(ehdr_t) == sizeof(Elf32_Ehdr)) ? ELFCLASS32 : ELFCLASS64;
	if(header->e_ident[EI_CLASS] != elfclass) throw Exception(E_UNEXPECTEDELFCLASS, header->e_ident[EI_CLASS]);

	// Verify the endianness and version of the ELF binary
	if(header->e_ident[EI_DATA] != ELFDATA2LSB) throw Exception(E_UNEXPECTEDELFENCODING);
	if(header->e_ident[EI_VERSION] != EV_CURRENT) throw Exception(E_UNKNOWNELFVERSION);

	// TODO: Verify x86 / x86_64 machine - check size of ehdr_t to know which
	// TODO: Verify object file type

	// Verify that the length of the header is the same size as the Elfxx_Ehdr struct
	// and that the header entries are at least as big as the known structures
	if(header->e_ehsize != sizeof(ehdr_t)) throw Exception(E_UNKNOWNELFVERSION);
	if((header->e_phentsize) && (header->e_phentsize < sizeof(phdr_t))) throw Exception(E_INVALIDELFPROGRAMTABLE);
	if((header->e_shentsize) && (header->e_shentsize < sizeof(shdr_t))) throw Exception(E_INVALIDELFSECTIONTABLE);
}

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

#pragma warning(pop)
<commit_msg>Allow for binaries that have a load address of zero to support position-independent executables<commit_after>//-----------------------------------------------------------------------------
// Copyright (c) 2014 Michael G. Brehm
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//-----------------------------------------------------------------------------

#include "stdafx.h"						// Include project pre-compiled headers
#include "ElfImage.h"					// Include ELFImage declarations

#pragma warning(push, 4)				// Enable maximum compiler warnings

//-----------------------------------------------------------------------------
// Explicit Instantiations

#ifdef _M_X64
template ElfImageT<Elf64_Ehdr, Elf64_Phdr, Elf64_Shdr>;
#else
template ElfImageT<Elf32_Ehdr, Elf32_Phdr, Elf32_Shdr>;
#endif

//-----------------------------------------------------------------------------
// ElfImageT Constructor (private)
//
// Arguments:
//
//	mapping		- Memory-mapped ELF image file to load
//	length		- Optional length to use from mapping

template <class ehdr_t, class phdr_t, class shdr_t>
ElfImageT<ehdr_t, phdr_t, shdr_t>::ElfImageT(std::shared_ptr<MappedFile>& mapping, size_t length)
{
	// Create a read-only view of the memory-mapped ELF image
	std::unique_ptr<MappedFileView> view(MappedFileView::Create(mapping, FILE_MAP_READ, 0, length));
	intptr_t viewptr = intptr_t(view->Pointer);

	// Validate the ELF header that should be at the start of the view
	ValidateHeader(view->Pointer, view->Length);
	ehdr_t*	elfheader = reinterpret_cast<ehdr_t*>(view->Pointer);

	// Create a collection of all the ELF program headers
	std::vector<phdr_t>	progheaders;
	for(int index = 0; index < elfheader->e_phnum; index++) {

		// Get the offset into the file for the next program header and check it
		intptr_t offset = elfheader->e_phoff + (index * elfheader->e_phentsize);
		if(view->Length < (offset + sizeof(phdr_t))) throw Exception(E_ELF_TRUNCATED);

		// Insert the program header into the collection
		progheaders.push_back(*reinterpret_cast<phdr_t*>(viewptr + offset));
	}

	// Determine the memory requirements of the loaded image
	intptr_t minpaddr = 0, maxpaddr = 0;
	for_each(progheaders.begin(), progheaders.end(), [&](phdr_t& phdr) {

		if(phdr.p_type == PT_LOAD) {

			// Calculate the minimum and maximum physical addresses of the segment
			// and adjust the overall minimum and maximums accordingly
			minpaddr = min(intptr_t(phdr.p_paddr), minpaddr);
			maxpaddr = max(intptr_t(phdr.p_paddr + phdr.p_memsz), maxpaddr);
		}
	});

	// Attempt to reserve the virtual memory for the image.  First try to use the physical address specified
	// by the image to avoid relocations, but go ahead and put it anywhere if that doesn't work
	try { m_region.reset(MemoryRegion::Reserve(reinterpret_cast<void*>(minpaddr), maxpaddr - minpaddr)); }
	catch(Exception&) { m_region.reset(MemoryRegion::Reserve(maxpaddr - minpaddr, MEM_TOP_DOWN)); }

	// Determine the delta between the allocated region and the original base physical address
	intptr_t regionbase = intptr_t(m_region->Pointer);
	intptr_t paddrdelta = minpaddr - regionbase;

	// Load the PT_LOAD segments into virtual memory
	for_each(progheaders.begin(), progheaders.end(), [&](phdr_t& phdr) {

		if((phdr.p_type == PT_LOAD) && (phdr.p_memsz)) {

			// Get the base address of the loadable segment and commit the virtual memory
			intptr_t segbase = phdr.p_paddr - paddrdelta;
			m_region->Commit(reinterpret_cast<void*>(segbase), phdr.p_memsz, PAGE_READWRITE);

			// Not all segments contain data that needs to be copied from the source image
			if(phdr.p_filesz) {

				// Ensure that there is enough source data to copy and copy it into the segment region
				if(view->Length < (phdr.p_offset + phdr.p_filesz)) throw Exception(E_ELF_TRUNCATED);
				memcpy(reinterpret_cast<void*>(segbase), reinterpret_cast<void*>(viewptr + phdr.p_offset), phdr.p_filesz);
			}

			// Memory that was not loaded from the ELF image must be initialized to zero
			memset(reinterpret_cast<void*>(segbase + phdr.p_filesz), 0, phdr.p_memsz - phdr.p_filesz);

			// Attempt to apply the proper virtual memory protection flags to the segment
			try { m_region->Protect(reinterpret_cast<void*>(segbase), phdr.p_memsz, FlagsToProtection(phdr.p_flags)); }
			catch(Exception& ex) { throw Exception(ex, E_ELFSEGMENTPROTECTION); }
		}
	});

	// Calculate the address of the image entry point, if one has been specified in the header
	m_entry = (elfheader->e_entry) ? reinterpret_cast<EntryPoint>(elfheader->e_entry - paddrdelta) : nullptr;
}

//---------------------------------------------------------------------------
// ElfImageT::AlignDown (private, static)
//
// Aligns an offset down to the specified alignment
//
// Arguments:
//
//	address		- Address to be aligned
//	alignment	- Alignment

template <class ehdr_t, class phdr_t, class shdr_t>
uintptr_t ElfImageT<ehdr_t, phdr_t, shdr_t>::AlignDown(uintptr_t address, size_t alignment)
{
	if(alignment < 1) throw Exception(E_ARGUMENTOUTOFRANGE, _T("alignment"));

	if(address < alignment) return 0;
	else return AlignUp(address - (alignment - 1), alignment);
}

//---------------------------------------------------------------------------
// ElfImageT::AlignUp (private, static)
//
// Aligns an offset up to the specified alignment
//
// Arguments:
//
//	address		- Address to be aligned
//	alignment	- Alignment

template <class ehdr_t, class phdr_t, class shdr_t>
uintptr_t ElfImageT<ehdr_t, phdr_t, shdr_t>::AlignUp(uintptr_t address, size_t alignment)
{
	if(alignment < 1) throw Exception(E_ARGUMENTOUTOFRANGE, _T("alignment"));

	if(address == 0) return 0;
	else return address + ((alignment - (address % alignment)) % alignment);
}

//-----------------------------------------------------------------------------
// ElfImageT::FlagsToProtection (private, static)
//
// Converts an ELF program header p_flags into VirtualAlloc() protection flags
//
// Arguments:
//
//	flags		- ELF program header p_flags value

template <class ehdr_t, class phdr_t, class shdr_t>
DWORD ElfImageT<ehdr_t, phdr_t, shdr_t>::FlagsToProtection(uint32_t flags)
{
	switch(flags) {

		case PF_X:					return PAGE_EXECUTE;
		case PF_W :					return PAGE_READWRITE;
		case PF_R :					return PAGE_READONLY;
		case PF_X | PF_W :			return PAGE_EXECUTE_READWRITE;
		case PF_X | PF_R :			return PAGE_EXECUTE_READ;
		case PF_W | PF_R :			return PAGE_READWRITE;
		case PF_X | PF_W | PF_R :	return PAGE_EXECUTE_READWRITE;
	}

	return PAGE_NOACCESS;
}

//-----------------------------------------------------------------------------
// ElfImageT::Load (static)
//
// Parses and loads the specified ELF image into virtual memory
//
// Arguments:
//
//	mapping			- Memory-mapped image file
//	length			- Optional length of mapping to use

template <class ehdr_t, class phdr_t, class shdr_t>
ElfImageT<ehdr_t, phdr_t, shdr_t>* ElfImageT<ehdr_t, phdr_t, shdr_t>::Load(std::shared_ptr<MappedFile>& mapping, size_t length)
{
	// All the validation code has been moved into the constructor
	return new ElfImageT<ehdr_t, phdr_t, shdr_t>(mapping, length);
}

//-----------------------------------------------------------------------------
// ElfImageT::TryValidateHeader (static)
//
// Validates that the provided pointer points to a 64-bit ELF binary header --
// returns a boolean flag rather than throwing an exception
//
// Arguments:
//
//	base		- Base address of the ELF binary to test
//	length		- Length of the buffer pointed to by base

template <class ehdr_t, class phdr_t, class shdr_t>
bool ElfImageT<ehdr_t, phdr_t, shdr_t>::TryValidateHeader(const void* base, size_t length)
{
	// Invoke the version that throws exceptions and just eat them
	try { ValidateHeader(base, length); return true; }
	catch(Exception&) { return false; }
}

//-----------------------------------------------------------------------------
// ElfImageT::ValidateHeader (static)
//
// Validates that the provided pointer points to a 64-bit ELF binary header
//
// Arguments:
//
//	base		- Base address of the ELF binary to test
//	length		- Length of the buffer pointed to by base

template <class ehdr_t, class phdr_t, class shdr_t>
void ElfImageT<ehdr_t, phdr_t, shdr_t>::ValidateHeader(const void* base, size_t length)
{
	const ehdr_t*				header;					// ELF header structure

	// Check the length and cast out a pointer to the header structure
	if(length < sizeof(ehdr_t)) throw Exception(E_TRUNCATEDELFHEADER);
	header = reinterpret_cast<const ehdr_t*>(base);

	// Check the ELF header magic number
	if(memcmp(&header->e_ident[EI_MAG0], ELFMAG, SELFMAG) != 0) throw Exception(E_INVALIDELFMAGIC);

	// Verify the ELF class matches the build configuration (32-bit vs. 64-bit)
	int elfclass = (sizeof(ehdr_t) == sizeof(Elf32_Ehdr)) ? ELFCLASS32 : ELFCLASS64;
	if(header->e_ident[EI_CLASS] != elfclass) throw Exception(E_UNEXPECTEDELFCLASS, header->e_ident[EI_CLASS]);

	// Verify the endianness and version of the ELF binary
	if(header->e_ident[EI_DATA] != ELFDATA2LSB) throw Exception(E_UNEXPECTEDELFENCODING);
	if(header->e_ident[EI_VERSION] != EV_CURRENT) throw Exception(E_UNKNOWNELFVERSION);

	// TODO: Verify x86 / x86_64 machine - check size of ehdr_t to know which
	// TODO: Verify object file type

	// Verify that the length of the header is the same size as the Elfxx_Ehdr struct
	// and that the header entries are at least as big as the known structures
	if(header->e_ehsize != sizeof(ehdr_t)) throw Exception(E_UNKNOWNELFVERSION);
	if((header->e_phentsize) && (header->e_phentsize < sizeof(phdr_t))) throw Exception(E_INVALIDELFPROGRAMTABLE);
	if((header->e_shentsize) && (header->e_shentsize < sizeof(shdr_t))) throw Exception(E_INVALIDELFSECTIONTABLE);
}

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

#pragma warning(pop)
<|endoftext|>