<commit_before>////////////////////////////////////////////////////////////////////////////////
///
/// Linear interpolation algorithm.
///
/// Author        : Copyright (c) Olli Parviainen
/// Author e-mail : oparviai 'at' iki.fi
/// SoundTouch WWW: http://www.surina.net/soundtouch
///
////////////////////////////////////////////////////////////////////////////////
//
// $Id$
//
////////////////////////////////////////////////////////////////////////////////
//
// License :
//
//  SoundTouch audio processing library
//  Copyright (c) Olli Parviainen
//
//  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, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
////////////////////////////////////////////////////////////////////////////////

#include <assert.h>
#include <stdlib.h>
#include "InterpolateLinear.h"

using namespace soundtouch;

//////////////////////////////////////////////////////////////////////////////
//
// InterpolateLinearInteger - integer arithmetic implementation
//

/// fixed-point interpolation routine precision
#define SCALE    65536

// Constructor
InterpolateLinearInteger::InterpolateLinearInteger() : TransposerBase()
{
    // Notice: use local function calling syntax for sake of clarity,
    // to indicate the fact that C++ constructor can't call virtual functions.
    resetRegisters();
    setRate(1.0f);
}

void InterpolateLinearInteger::resetRegisters()
{
    iFract = 0;
}

// Transposes the sample rate of the given samples using linear interpolation.
// 'Mono' version of the routine. Returns the number of samples returned in
// the "dest" buffer
int InterpolateLinearInteger::transposeMono(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    // not yet implemented
    assert(FALSE);
    return 0;
}

// Transposes the sample rate of the given samples using linear interpolation.
// 'Stereo' version of the routine. Returns the number of samples returned in
// the "dest" buffer
int InterpolateLinearInteger::transposeStereo(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    // not yet implemented
    return 0;
}

int InterpolateLinearInteger::transposeMulti(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        LONG_SAMPLETYPE temp, vol1;

        assert(iFract < SCALE);
        vol1 = (SCALE - iFract);
        for (int c = 0; c < numChannels; c ++)
        {
            temp = iFract * src[c] + vol1 * src[c + numChannels];
            *dest = (SAMPLETYPE)(temp / SCALE);
            dest ++;
        }
        i++;

        iFract += iRate;

        int iWhole = iFract / SCALE;
        iFract -= iWhole * SCALE;
        srcCount += iWhole;
        src += iWhole * numChannels;
    }
    srcSamples = srcCount;

    return i;
}

// Sets new target iRate. Normal iRate = 1.0, smaller values represent slower
// iRate, larger faster iRates.
void InterpolateLinearInteger::setRate(float newRate)
{
    iRate = (int)(newRate * SCALE + 0.5f);
    TransposerBase::setRate(newRate);
}

//////////////////////////////////////////////////////////////////////////////
//
// InterpolateLinearFloat - floating point arithmetic implementation
//
//////////////////////////////////////////////////////////////////////////////

// Constructor
InterpolateLinearFloat::InterpolateLinearFloat() : TransposerBase()
{
    // Notice: use local function calling syntax for sake of clarity,
    // to indicate the fact that C++ constructor can't call virtual functions.
    resetRegisters();
    setRate(1.0f);
}

void InterpolateLinearFloat::resetRegisters()
{
    fract = 0;
}

// Transposes the sample rate of the given samples using linear interpolation.
// 'Mono' version of the routine. Returns the number of samples returned in
// the "dest" buffer
int InterpolateLinearFloat::transposeMono(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        double out;
        assert(fract < 1.0);

        out = (1.0 - fract) * src[0] + fract * src[1];
        dest[i] = (SAMPLETYPE)out;
        i ++;

        // update position fraction
        fract += rate;
        // update whole positions
        int whole = (int)fract;
        fract -= whole;
        src += whole;
        srcCount += whole;
    }
    srcSamples = srcCount;
    return i;
}

// Transposes the sample rate of the given samples using linear interpolation.
// 'Mono' version of the routine. Returns the number of samples returned in
// the "dest" buffer
int InterpolateLinearFloat::transposeStereo(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        double out0, out1;
        assert(fract < 1.0);

        out0 = (1.0 - fract) * src[0] + fract * src[2];
        out1 = (1.0 - fract) * src[1] + fract * src[3];
        dest[2*i]   = (SAMPLETYPE)out0;
        dest[2*i+1] = (SAMPLETYPE)out1;
        i ++;

        // update position fraction
        fract += rate;
        // update whole positions
        int whole = (int)fract;
        fract -= whole;
        src += 2*whole;
        srcCount += whole;
    }
    srcSamples = srcCount;
    return i;
}

int InterpolateLinearFloat::transposeMulti(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        float temp, vol1;

        vol1 = (1.0f- fract);
        for (int c = 0; c < numChannels; c ++)
        {
            temp = vol1 * src[c] + fract * src[c + numChannels];
            *dest = (SAMPLETYPE)temp;
            dest ++;
        }
        i++;

        fract += rate;

        int iWhole = (int)fract;
        fract -= iWhole;
        srcCount += iWhole;
        src += iWhole * numChannels;
    }
    srcSamples = srcCount;

    return i;
}
<commit_msg>Implemented integer version of linear interpolator<commit_after>////////////////////////////////////////////////////////////////////////////////
///
/// Linear interpolation algorithm.
///
/// Author        : Copyright (c) Olli Parviainen
/// Author e-mail : oparviai 'at' iki.fi
/// SoundTouch WWW: http://www.surina.net/soundtouch
///
////////////////////////////////////////////////////////////////////////////////
//
// $Id$
//
////////////////////////////////////////////////////////////////////////////////
//
// License :
//
//  SoundTouch audio processing library
//  Copyright (c) Olli Parviainen
//
//  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, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
////////////////////////////////////////////////////////////////////////////////

#include <assert.h>
#include <stdlib.h>
#include "InterpolateLinear.h"

using namespace soundtouch;

//////////////////////////////////////////////////////////////////////////////
//
// InterpolateLinearInteger - integer arithmetic implementation
//

/// fixed-point interpolation routine precision
#define SCALE    65536

// Constructor
InterpolateLinearInteger::InterpolateLinearInteger() : TransposerBase()
{
    // Notice: use local function calling syntax for sake of clarity,
    // to indicate the fact that C++ constructor can't call virtual functions.
    resetRegisters();
    setRate(1.0f);
}

void InterpolateLinearInteger::resetRegisters()
{
    iFract = 0;
}

// Transposes the sample rate of the given samples using linear interpolation.
// 'Mono' version of the routine. Returns the number of samples returned in
// the "dest" buffer
int InterpolateLinearInteger::transposeMono(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        LONG_SAMPLETYPE temp;

        assert(iFract < SCALE);

        temp = (SCALE - iFract) * src[0] + iFract * src[1];
        dest[i] = (SAMPLETYPE)(temp / SCALE);
        i++;

        iFract += iRate;

        int iWhole = iFract / SCALE;
        iFract -= iWhole * SCALE;
        srcCount += iWhole;
        src += iWhole;
    }
    srcSamples = srcCount;

    return i;
}

// Transposes the sample rate of the given samples using linear interpolation.
// 'Stereo' version of the routine. Returns the number of samples returned in
// the "dest" buffer
int InterpolateLinearInteger::transposeStereo(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        LONG_SAMPLETYPE temp0;
        LONG_SAMPLETYPE temp1;

        assert(iFract < SCALE);

        temp0 = (SCALE - iFract) * src[0] + iFract * src[2];
        temp1 = (SCALE - iFract) * src[1] + iFract * src[3];
        dest[0] = (SAMPLETYPE)(temp0 / SCALE);
        dest[1] = (SAMPLETYPE)(temp1 / SCALE);
        dest += 2;
        i++;

        iFract += iRate;

        int iWhole = iFract / SCALE;
        iFract -= iWhole * SCALE;
        srcCount += iWhole;
        src += 2*iWhole;
    }
    srcSamples = srcCount;

    return i;
}

int InterpolateLinearInteger::transposeMulti(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        LONG_SAMPLETYPE temp, vol1;

        assert(iFract < SCALE);
        vol1 = (SCALE - iFract);
        for (int c = 0; c < numChannels; c ++)
        {
            temp = vol1 * src[c] + iFract * src[c + numChannels];
            dest[0] = (SAMPLETYPE)(temp / SCALE);
            dest ++;
        }
        i++;

        iFract += iRate;

        int iWhole = iFract / SCALE;
        iFract -= iWhole * SCALE;
        srcCount += iWhole;
        src += iWhole * numChannels;
    }
    srcSamples = srcCount;

    return i;
}

// Sets new target iRate. Normal iRate = 1.0, smaller values represent slower
// iRate, larger faster iRates.
void InterpolateLinearInteger::setRate(float newRate)
{
    iRate = (int)(newRate * SCALE + 0.5f);
    TransposerBase::setRate(newRate);
}

//////////////////////////////////////////////////////////////////////////////
//
// InterpolateLinearFloat - floating point arithmetic implementation
//
//////////////////////////////////////////////////////////////////////////////

// Constructor
InterpolateLinearFloat::InterpolateLinearFloat() : TransposerBase()
{
    // Notice: use local function calling syntax for sake of clarity,
    // to indicate the fact that C++ constructor can't call virtual functions.
    resetRegisters();
    setRate(1.0f);
}

void InterpolateLinearFloat::resetRegisters()
{
    fract = 0;
}

// Transposes the sample rate of the given samples using linear interpolation.
// 'Mono' version of the routine. Returns the number of samples returned in
// the "dest" buffer
int InterpolateLinearFloat::transposeMono(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        double out;
        assert(fract < 1.0);

        out = (1.0 - fract) * src[0] + fract * src[1];
        dest[i] = (SAMPLETYPE)out;
        i ++;

        // update position fraction
        fract += rate;
        // update whole positions
        int whole = (int)fract;
        fract -= whole;
        src += whole;
        srcCount += whole;
    }
    srcSamples = srcCount;
    return i;
}

// Transposes the sample rate of the given samples using linear interpolation.
// 'Mono' version of the routine. Returns the number of samples returned in
// the "dest" buffer
int InterpolateLinearFloat::transposeStereo(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        double out0, out1;
        assert(fract < 1.0);

        out0 = (1.0 - fract) * src[0] + fract * src[2];
        out1 = (1.0 - fract) * src[1] + fract * src[3];
        dest[2*i]   = (SAMPLETYPE)out0;
        dest[2*i+1] = (SAMPLETYPE)out1;
        i ++;

        // update position fraction
        fract += rate;
        // update whole positions
        int whole = (int)fract;
        fract -= whole;
        src += 2*whole;
        srcCount += whole;
    }
    srcSamples = srcCount;
    return i;
}

int InterpolateLinearFloat::transposeMulti(SAMPLETYPE *dest, const SAMPLETYPE *src, int &srcSamples)
{
    int i;
    int srcSampleEnd = srcSamples - 1;
    int srcCount = 0;

    i = 0;
    while (srcCount < srcSampleEnd)
    {
        float temp, vol1;

        vol1 = (1.0f- fract);
        for (int c = 0; c < numChannels; c ++)
        {
            temp = vol1 * src[c] + fract * src[c + numChannels];
            *dest = (SAMPLETYPE)temp;
            dest ++;
        }
        i++;

        fract += rate;

        int iWhole = (int)fract;
        fract -= iWhole;
        srcCount += iWhole;
        src += iWhole * numChannels;
    }
    srcSamples = srcCount;

    return i;
}
<|endoftext|>


<commit_before>/*
 * avs2wav.cpp
 *  extract audio data as RIFF WAV format, from avs file
 *  Copyright (C) 2010 janus_wel<janus.wel.3@gmail.com>
 *  see LICENSE for redistributing, modifying, and so on.
 * */

#include "../../include/avsutil.hpp"
#include "avs2wav.h"
#include <iostream>
#include <fstream>
#include <iomanip>
#include <memory>
#include <string>
#include <stdexcept>
#include <vector>
#include "../../helper/tconv.hpp"

#ifdef _MSC_VER
#include <io.h>
#include <fcntl.h>
#else
#include <unistd.h>
#endif

using namespace std;
using namespace avsutil;

// global object
// progresss is abbr of "progress stream"
// this stream is used in progress_cl()
// streambuf is stdout fow now
ostream progresss(cout.rdbuf());

// converter routing through std::string;
util::string::converter conv;

// forward declarations
// return true if redirected to file or connected to pipe
bool is_connected(void);
// set stdout to binary mode
void set_stdout_binary(void);
// callback function for IAudio::write()
void progress_cl(const unsigned __int64, const unsigned __int64);
// output audio informations
ostream& operator <<(ostream&, const AudioInfo&);

int main(const unsigned int argc, const char* argv[]) {
    // constants
    const unsigned int header_width = 24;
    const unsigned int buf_samples_def = 4096;
    const unsigned int buf_size_def = 4096;
    const unsigned int buf_samples_min = 1;
    const unsigned int buf_size_min = 2;

    // analyze options
    string inputfile;
    string outputfile;
    unsigned int buf_samples = buf_samples_def;
    unsigned int buf_size = buf_size_def;
    for (unsigned int i = 1; i < argc; ++i) {
        const string arg(argv[i]);
        if (arg == "-h" || arg == "--help") {
            usage(cout);
            return OK;
        }
        else if (arg == "-v" || arg == "--version") {
            version_license(cout);
            return OK;
        }
        else if (arg == "-b" || arg == "--buffers") {
            buf_size = conv.strto<unsigned int>(argv[++i]);
            if (buf_size < buf_size_min) {
                cerr << "Size of buffers for output is required at least: "
                     << buf_size_min << endl
                     << "Check the argument with \"-b\" option." << endl;
                return BAD_ARG;
            }
        }
        else if (arg == "-s" || arg == "--samples") {
            buf_samples = conv.strto<unsigned int>(argv[++i]);
            if (buf_samples < buf_samples_min) {
                cerr << "A number of samples processed at one time is required at least: "
                     << buf_samples_min << endl
                     << "Check the argument with \"-s\" option." << endl;
                return BAD_ARG;
            }
        }
        else if (arg == "-o" || arg == "--output") {
            outputfile = argv[++i];
        }
        else if (arg[0] == '-') {
            cerr << "Unknown option: \"" << arg << '"' << endl;
            usage(cerr);
            return BAD_ARG;
        }
        else {
            inputfile = arg;
        }
    }

    if (inputfile.empty()) {
        cerr << "Specify <inputfile>." << endl;
        usage(cerr);
        return BAD_ARG;
    }

    try {
        auto_ptr<IAvs> avs(CreateAvsObj(inputfile.c_str()));
        if (!avs->is_fine()) {
            cerr << avs->errmsg() << endl;
            return BAD_AVS;
        }

        auto_ptr<IAudio> audio(avs->audio());
        AudioInfo ai = audio->info();

        if (!ai.exists) {
            cerr << "no audio in the file: " << inputfile << endl;
            return BAD_AVS;
        }

        // preparations
        // output and information stream (to stdout for now)
        ostream outputs(cout.rdbuf());
        ostream infos(cout.rdbuf());
        // settings for stream
        progresss << right << fixed << setprecision(2);
        infos << left;
        // this is true if redirected to file or connected to pipe
        bool is_stdout = !is_connected();
        // settings for infos, outputs and progresss
        if (is_stdout) {
            // output to file
            if (outputfile.empty()) outputfile.assign(inputfile).append(".wav");
            // create filebuf and set it output stream
            filebuf* fbuf = new filebuf;
            fbuf->open(outputfile.c_str(), ios::out | ios::binary | ios::trunc);
            outputs.rdbuf(fbuf);
        }
        else {
            // redirected or connected pipe
            // output to stdout
            outputfile.assign("stdout");
            progresss.rdbuf(cerr.rdbuf());
            infos.rdbuf(cerr.rdbuf());
            set_stdout_binary();
        }

        infos
            << setw(header_width) << "source:"                << inputfile << endl
            << setw(header_width) << "destination:"           << outputfile << endl
            << setw(header_width) << "buffers for output:"    << buf_size << " bytes" << endl
            << setw(header_width) << "processed at one time:" << buf_samples << " samples" << endl
            << ai;

        std::vector<char> internalbuf(buf_size);
        outputs.rdbuf()->pubsetbuf(&internalbuf[0], buf_size);
        audio->buf_samples(buf_samples);
        audio->progress_callback(progress_cl);
        outputs << audio.get();

        infos
            << endl
            << "done." << endl
            << endl;
    }
    catch (exception& ex) {
        cerr << endl << ex.what() << endl;
        return UNKNOWN;
    }

    return OK;
}

// definitions of functions
bool inline is_connected(void) {
#ifdef _MSC_VER
    return !_isatty(_fileno(stdout));
#else
    return !isatty(fileno(stdout));
#endif
}

void inline set_stdout_binary(void) {
#ifdef _MSC_VER
    _setmode(_fileno(stdout), _O_BINARY);
#endif
}

void progress_cl(const unsigned __int64 processed, const unsigned __int64 max) {
    float percentage = (static_cast<float>(processed) / static_cast<float>(max)) * 100;
    progresss
        << "\rprocessing... "
        << setw(10) << processed << "/" << max << " samples"
        << " (" << setw(6) << percentage << "%)";
}

ostream& operator <<(ostream& out, const AudioInfo& ai) {
    // constants
    static const unsigned int header_width = 18;

    // preparations
    string channels;
    switch (ai.channels) {
        case 1: channels = "mono";   break;
        case 2: channels = "stereo"; break;
        case 6: channels = "5.1ch";  break;
        default:
            channels.assign(conv.strfrom(ai.channels)).append("ch");
            break;
    }
    float sampling_rate = static_cast<float>(ai.sampling_rate) / 1000;

    // instantiate new ostream object and set streambuf of "out" to it
    // in order to save the formattings of "out"
    ostream o(out.rdbuf());
    o << left << setfill('.')
        << "\n"
        << setw(header_width) << "bit depth "     << ' ' << ai.bit_depth << "bit\n"
        << setw(header_width) << "channels "      << ' ' << channels << "\n"
        << setw(header_width) << "sampling rate " << ' ' << sampling_rate << "kHz\n"
        << endl;

    return out;
}
<commit_msg>Allow the filenames that has non-ascii characters<commit_after>/*
 * avs2wav.cpp
 *  extract audio data as RIFF WAV format, from avs file
 *  Copyright (C) 2010 janus_wel<janus.wel.3@gmail.com>
 *  see LICENSE for redistributing, modifying, and so on.
 * */

#include "../../include/avsutil.hpp"
#include "avs2wav.h"
#include <iostream>
#include <fstream>
#include <iomanip>
#include <memory>
#include <string>
#include <stdexcept>
#include <vector>
#include <locale>
#include "../../helper/tconv.hpp"

#ifdef _MSC_VER
#include <io.h>
#include <fcntl.h>
#else
#include <unistd.h>
#endif

using namespace std;
using namespace avsutil;

// global object
// progresss is abbr of "progress stream"
// this stream is used in progress_cl()
// streambuf is stdout fow now
ostream progresss(cout.rdbuf());

// converter routing through std::string;
util::string::converter conv;

// forward declarations
// return true if redirected to file or connected to pipe
bool is_connected(void);
// set stdout to binary mode
void set_stdout_binary(void);
// callback function for IAudio::write()
void progress_cl(const unsigned __int64, const unsigned __int64);
// output audio informations
ostream& operator <<(ostream&, const AudioInfo&);

int main(const unsigned int argc, const char* argv[]) {
    locale::global(locale(""));

    // constants
    const unsigned int header_width = 24;
    const unsigned int buf_samples_def = 4096;
    const unsigned int buf_size_def = 4096;
    const unsigned int buf_samples_min = 1;
    const unsigned int buf_size_min = 2;

    // analyze options
    string inputfile;
    string outputfile;
    unsigned int buf_samples = buf_samples_def;
    unsigned int buf_size = buf_size_def;
    for (unsigned int i = 1; i < argc; ++i) {
        const string arg(argv[i]);
        if (arg == "-h" || arg == "--help") {
            usage(cout);
            return OK;
        }
        else if (arg == "-v" || arg == "--version") {
            version_license(cout);
            return OK;
        }
        else if (arg == "-b" || arg == "--buffers") {
            buf_size = conv.strto<unsigned int>(argv[++i]);
            if (buf_size < buf_size_min) {
                cerr << "Size of buffers for output is required at least: "
                     << buf_size_min << endl
                     << "Check the argument with \"-b\" option." << endl;
                return BAD_ARG;
            }
        }
        else if (arg == "-s" || arg == "--samples") {
            buf_samples = conv.strto<unsigned int>(argv[++i]);
            if (buf_samples < buf_samples_min) {
                cerr << "A number of samples processed at one time is required at least: "
                     << buf_samples_min << endl
                     << "Check the argument with \"-s\" option." << endl;
                return BAD_ARG;
            }
        }
        else if (arg == "-o" || arg == "--output") {
            outputfile = argv[++i];
        }
        else if (arg[0] == '-') {
            cerr << "Unknown option: \"" << arg << '"' << endl;
            usage(cerr);
            return BAD_ARG;
        }
        else {
            inputfile = arg;
        }
    }

    if (inputfile.empty()) {
        cerr << "Specify <inputfile>." << endl;
        usage(cerr);
        return BAD_ARG;
    }

    try {
        auto_ptr<IAvs> avs(CreateAvsObj(inputfile.c_str()));
        if (!avs->is_fine()) {
            cerr << avs->errmsg() << endl;
            return BAD_AVS;
        }

        auto_ptr<IAudio> audio(avs->audio());
        AudioInfo ai = audio->info();

        if (!ai.exists) {
            cerr << "no audio in the file: " << inputfile << endl;
            return BAD_AVS;
        }

        // preparations
        // output and information stream (to stdout for now)
        ostream outputs(cout.rdbuf());
        ostream infos(cout.rdbuf());
        // settings for stream
        progresss << right << fixed << setprecision(2);
        infos << left;
        // this is true if redirected to file or connected to pipe
        bool is_stdout = !is_connected();
        // settings for infos, outputs and progresss
        if (is_stdout) {
            // output to file
            if (outputfile.empty()) outputfile.assign(inputfile).append(".wav");
            // create filebuf and set it output stream
            filebuf* fbuf = new filebuf;
            fbuf->open(outputfile.c_str(), ios::out | ios::binary | ios::trunc);
            outputs.rdbuf(fbuf);
        }
        else {
            // redirected or connected pipe
            // output to stdout
            outputfile.assign("stdout");
            progresss.rdbuf(cerr.rdbuf());
            infos.rdbuf(cerr.rdbuf());
            set_stdout_binary();
        }

        infos
            << setw(header_width) << "source:"                << inputfile << endl
            << setw(header_width) << "destination:"           << outputfile << endl
            << setw(header_width) << "buffers for output:"    << buf_size << " bytes" << endl
            << setw(header_width) << "processed at one time:" << buf_samples << " samples" << endl
            << ai;

        std::vector<char> internalbuf(buf_size);
        outputs.rdbuf()->pubsetbuf(&internalbuf[0], buf_size);
        audio->buf_samples(buf_samples);
        audio->progress_callback(progress_cl);
        outputs << audio.get();

        infos
            << endl
            << "done." << endl
            << endl;
    }
    catch (exception& ex) {
        cerr << endl << ex.what() << endl;
        return UNKNOWN;
    }

    return OK;
}

// definitions of functions
bool inline is_connected(void) {
#ifdef _MSC_VER
    return !_isatty(_fileno(stdout));
#else
    return !isatty(fileno(stdout));
#endif
}

void inline set_stdout_binary(void) {
#ifdef _MSC_VER
    _setmode(_fileno(stdout), _O_BINARY);
#endif
}

void progress_cl(const unsigned __int64 processed, const unsigned __int64 max) {
    float percentage = (static_cast<float>(processed) / static_cast<float>(max)) * 100;
    progresss
        << "\rprocessing... "
        << setw(10) << processed << "/" << max << " samples"
        << " (" << setw(6) << percentage << "%)";
}

ostream& operator <<(ostream& out, const AudioInfo& ai) {
    // constants
    static const unsigned int header_width = 18;

    // preparations
    string channels;
    switch (ai.channels) {
        case 1: channels = "mono";   break;
        case 2: channels = "stereo"; break;
        case 6: channels = "5.1ch";  break;
        default:
            channels.assign(conv.strfrom(ai.channels)).append("ch");
            break;
    }
    float sampling_rate = static_cast<float>(ai.sampling_rate) / 1000;

    // instantiate new ostream object and set streambuf of "out" to it
    // in order to save the formattings of "out"
    ostream o(out.rdbuf());
    o << left << setfill('.')
        << "\n"
        << setw(header_width) << "bit depth "     << ' ' << ai.bit_depth << "bit\n"
        << setw(header_width) << "channels "      << ' ' << channels << "\n"
        << setw(header_width) << "sampling rate " << ' ' << sampling_rate << "kHz\n"
        << endl;

    return out;
}
<|endoftext|>


<commit_before>///
/// @file   help.cpp
/// @brief  help() and version() functions of the primesieve
///         console application.
///
/// Copyright (C) 2016 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <primesieve.hpp>

#include <iostream>
#include <cstdlib>
#include <string>

using namespace std;

namespace {

const string helpMenu(
  "Usage: primesieve [START] STOP [OPTION]...\n"
  "Generate the primes and/or prime k-tuplets in the interval [START, STOP]\n"
  "(< 2^64) using the segmented sieve of Eratosthenes.\n"
  "\n"
  "Options:\n"
  "\n"
  "  -c[N+], --count[=N+]    Count primes and prime k-tuplets, N <= 6,\n"
  "                          e.g. -c1 primes, -c2 twins, -c3 triplets, ...\n"
  "  -d<N>,  --dist=<N>      Sieve the interval [START, START + N]\n"
  "  -h,     --help          Print this help menu\n"
  "  -n,     --nthprime      Calculate the nth prime,\n"
  "                          e.g. 1 100 -n finds the 1st prime > 100\n"
  "          --no-status     Turn off the progressing status\n"
  "  -p[N],  --print[=N]     Print primes or prime k-tuplets, N <= 6,\n"
  "                          e.g. -p1 primes, -p2 twins, -p3 triplets, ...\n"
  "  -q,     --quiet         Quiet mode, prints less output\n"
  "  -s<N>,  --size=<N>      Set the sieve size in kilobytes, N <= 2048\n"
  "          --test          Run various sieving tests and exit\n"
  "  -t<N>,  --threads=<N>   Set the number of threads, N <= CPU cores\n"
  "          --time          Print the time elapsed in seconds\n"
  "  -v,     --version       Print version and license information\n"
  "\n"
  "Examples:\n"
  "\n"
  "  primesieve 1000         Count the primes below 1000\n"
  "  primesieve 1000 -c2     Count the twin primes below 1000\n"
  "  primesieve 1e6 --print  Print the primes below 10^6\n"
  "  primesieve 100 200 -p   Print the primes inside [100, 200]\n"
);

} // namespace

void help()
{
  cout << helpMenu << endl;
  exit(1);
}

void version()
{
  cout << "primesieve " << primesieve::primesieve_version();
  cout << ", <http://primesieve.org>" << endl;
  cout << "Copyright (C) 2010-2016 Kim Walisch" << endl;
  cout << endl;
  cout << "BSD 2-Clause License <http://opensource.org/licenses/BSD-2-Clause>" << endl;

  exit(1);
}
<commit_msg>Refactor<commit_after>///
/// @file   help.cpp
/// @brief  help() and version() functions of the primesieve
///         console application.
///
/// Copyright (C) 2016 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <primesieve.hpp>

#include <iostream>
#include <cstdlib>
#include <string>

using namespace std;

namespace {

const string helpMenu(
  "Usage: primesieve [START] STOP [OPTION]...\n"
  "Generate the primes and/or prime k-tuplets in the interval [START, STOP]\n"
  "(< 2^64) using the segmented sieve of Eratosthenes.\n"
  "\n"
  "Options:\n"
  "\n"
  "  -c[N+], --count[=N+]    Count primes and prime k-tuplets, N <= 6,\n"
  "                          e.g. -c1 primes, -c2 twins, -c3 triplets, ...\n"
  "  -d<N>,  --dist=<N>      Sieve the interval [START, START + N]\n"
  "  -h,     --help          Print this help menu\n"
  "  -n,     --nthprime      Calculate the nth prime,\n"
  "                          e.g. 1 100 -n finds the 1st prime > 100\n"
  "          --no-status     Turn off the progressing status\n"
  "  -p[N],  --print[=N]     Print primes or prime k-tuplets, N <= 6,\n"
  "                          e.g. -p1 primes, -p2 twins, -p3 triplets, ...\n"
  "  -q,     --quiet         Quiet mode, prints less output\n"
  "  -s<N>,  --size=<N>      Set the sieve size in kilobytes, N <= 2048\n"
  "          --test          Run various sieving tests and exit\n"
  "  -t<N>,  --threads=<N>   Set the number of threads, N <= CPU cores\n"
  "          --time          Print the time elapsed in seconds\n"
  "  -v,     --version       Print version and license information\n"
  "\n"
  "Examples:\n"
  "\n"
  "  primesieve 1000         Count the primes below 1000\n"
  "  primesieve 1000 -c2     Count the twin primes below 1000\n"
  "  primesieve 1e6 --print  Print the primes below 10^6\n"
  "  primesieve 100 200 -p   Print the primes inside [100, 200]\n"
);

} // namespace

void help()
{
  cout << helpMenu << endl;
  exit(1);
}

void version()
{
  cout << "primesieve " << primesieve::primesieve_version();
  cout << ", <http://primesieve.org>" << endl;
  cout << "Copyright (C) 2010-2016 Kim Walisch" << endl;
  cout << endl;

  cout << "BSD 2-Clause License <http://opensource.org/licenses/BSD-2-Clause>" << endl;
  cout << endl;

  exit(1);
}
<|endoftext|>


<commit_before>//------------------------------------------------------------------------------
//
//    Copyright (C) Streamlet. All rights reserved.
//
//    File Name:   Main.cpp
//    Author:      Streamlet
//    Create Time: 2010-08-14
//    Description:
//
//    Version history:
//
//
//
//------------------------------------------------------------------------------

int main()
{
    return 0;
}
<commit_msg>add test cases and unit test for xl::Set<commit_after>//------------------------------------------------------------------------------
//
//    Copyright (C) Streamlet. All rights reserved.
//
//    File Name:   Main.cpp
//    Author:      Streamlet
//    Create Time: 2010-08-14
//    Description:
//
//    Version history:
//
//
//
//------------------------------------------------------------------------------

#include <xl/Containers/xlSet.h>
#include "../UnitTestFrame.h"

using namespace xl;

TEST_CASE(ctor)
{
    Set<int> a;
    TEST_ASSERT(a.Size() == 0);
}

TEST_CASE(ctor_copy)
{
    Set<int> a;
    TEST_ASSERT(a.Size() == 0);

    Set<int> b(a);
    TEST_ASSERT(b.Size() == 0);

    a.Insert(1);
    a.Insert(2);
    Set<int> c(a);
    TEST_ASSERT(c.Size() == 2);
    TEST_ASSERT(*c.Begin() == 1);
    TEST_ASSERT(*c.ReverseBegin() == 2);
}

TEST_CASE(operator_set)
{
    Set<int> a;
    TEST_ASSERT(a.Size() == 0);

    Set<int> b;
    b = a;
    TEST_ASSERT(b.Size() == 0);

    a.Insert(1);
    a.Insert(2);
    Set<int> c;
    c = a;
    TEST_ASSERT(c.Size() == 2);
    TEST_ASSERT(*a.Begin() == 1);
    TEST_ASSERT(*++a.Begin() == 2);
}

TEST_CASE(operator_equal)
{
    Set<int> a;
    TEST_ASSERT(a.Size() == 0);

    Set<int> b;
    b = a;
    TEST_ASSERT(b == a);
    TEST_ASSERT(!(b != a));

    a.Insert(1);
    a.Insert(2);
    TEST_ASSERT(b != a);
    TEST_ASSERT(!(b == a));

    Set<int> c(a);
    TEST_ASSERT(c == a);
    TEST_ASSERT(!(c != a));

    b.Insert(1);
    b.Insert(2);
    TEST_ASSERT(b == a);
    TEST_ASSERT(!(b != a));
    TEST_ASSERT(b == c);
    TEST_ASSERT(!(b != c));
}

TEST_CASE(method_size)
{
    Set<int> a;
    TEST_ASSERT(a.Size() == 0);
    TEST_ASSERT(a.Empty() == true);

    a.Insert(10);
    TEST_ASSERT(a.Size() == 1);
    TEST_ASSERT(a.Empty() == false);

    a.Insert(20);
    a.Insert(30);
    a.Insert(40);
    a.Insert(50);
    TEST_ASSERT(a.Size() == 5);
    TEST_ASSERT(a.Empty() == false);
}

TEST_CASE(method_insert_delete)
{
    Set<int> a;

    a.Insert(2);
    a.Insert(6);
    a.Insert(4);
    a.Insert(3);
    a.Insert(5);
    a.Insert(1);

    Set<int>::Iterator it = a.Begin();

    TEST_ASSERT(*it == 1);

    ++it;
    TEST_ASSERT(*it == 2);

    ++it;
    TEST_ASSERT(*it == 3);

    ++it;
    TEST_ASSERT(*it == 4);

    ++it;
    TEST_ASSERT(*it == 5);

    ++it;
    TEST_ASSERT(*it == 6);

    ++it;
    TEST_ASSERT(it == a.End());
}

TEST_CASE(method_find)
{
    Set<int> a;

    a.Insert(2);
    a.Insert(6);
    a.Insert(4);
    a.Insert(3);
    a.Insert(5);
    a.Insert(1);

    Set<int>::Iterator it = a.Find(5);

    TEST_ASSERT(*it == 5);

    Set<int>::Iterator it2 = it;

    ++it2;
    TEST_ASSERT(*it2 == 6);

    ++it2;
    TEST_ASSERT(it2 == a.End());
}

// Performance Test

#include <stdio.h>
#include <time.h>

#define SECTION_BEGIN(name) \
    printf("Testing \"" #name "\"...\n")
#define SECTION_END()   \
    printf("\n")
#define PERFORMANCE_TEST_BEGIN(name)    \
    printf("Test begins.\n");   \
    clock_t begin_##name = clock()
#define PERFORMANCE_TEST_END(name)  \
    clock_t end_##name = clock();   \
    printf("Test ends. Time elasped: %lfs.\n", (double)(end_##name - begin_##name) / CLK_TCK)

#include <vector>
using namespace std;

#include <set>

int main()
{
    SECTION_BEGIN(StdSetInsert100000);
    set<int> s1;
    PERFORMANCE_TEST_BEGIN(StdSetInsert100000);
    for (int i = 0; i < 100000; ++i)
    {
        s1.insert(i);
    }
    PERFORMANCE_TEST_END(StdSetInsert100000);
    SECTION_END();

    SECTION_BEGIN(xlSetInsert100000);
    Set<int> a1;
    PERFORMANCE_TEST_BEGIN(xlSetInsert100000);
    for (int i = 0; i < 100000; ++i)
    {
        a1.Insert(i);
    }
    PERFORMANCE_TEST_END(xlSetInsert100000);
    SECTION_END();

    SECTION_BEGIN(StdSetInsert1000000);
    set<int> s2;
    PERFORMANCE_TEST_BEGIN(StdSetInsert1000000);
    for (int i = 0; i < 1000000; ++i)
    {
        s2.insert(i);
    }
    PERFORMANCE_TEST_END(StdSetInsert1000000);
    SECTION_END();

    SECTION_BEGIN(xlSetInsert1000000);
    Set<int> a2;
    PERFORMANCE_TEST_BEGIN(xlSetInsert1000000);
    for (int i = 0; i < 1000000; ++i)
    {
        a2.Insert(i);
    }
    PERFORMANCE_TEST_END(xlSetInsert1000000);
    SECTION_END();

    SECTION_BEGIN(StdSetInsert10000000);
    set<int> s3;
    PERFORMANCE_TEST_BEGIN(StdSetInsert10000000);
    for (int i = 0; i < 10000000; ++i)
    {
        s3.insert(i);
    }
    PERFORMANCE_TEST_END(StdSetInsert10000000);
    SECTION_END();

    SECTION_BEGIN(xlSetInsert10000000);
    Set<int> a3;
    PERFORMANCE_TEST_BEGIN(xlSetInsert10000000);
    for (int i = 0; i < 10000000; ++i)
    {
        a3.Insert(i);
    }
    PERFORMANCE_TEST_END(xlSetInsert10000000);
    SECTION_END();

    return 0;
}
<|endoftext|>


<commit_before>/** \file Ex7.cpp
 *  \brief This example shows how to set and solve the weak form
 *   of the Boussinesq appoximation of the Navier-Stokes Equation
 *
 *  \f{eqnarray*}
 *  && \mathbf{V} \cdot \nabla T - \nabla \cdot\alpha \nabla T = 0 \\
 *  && \mathbf{V} \cdot \nabla \mathbf{V} - \nabla \cdot \nu (\nabla \mathbf{V} +(\nabla \mathbf{V})^T)
 *  +\nabla P = \beta T \mathbf{j} \\
 *  && \nabla \cdot \mathbf{V} = 0
 *  \f}
 *  in a unit box domain (in 2D and 3D) with given temperature 0 and 1 on
 *  the left and right walls, respectively, and insulated walls elsewhere.
 *  \author Eugenio Aulisa
 */

#include "FemusInit.hpp"
#include "MultiLevelProblem.hpp"
#include "NumericVector.hpp"
#include "VTKWriter.hpp"
#include "GMVWriter.hpp"
#include "NonLinearImplicitSystem.hpp"
#include "adept.h"

using namespace femus;

bool SetBoundaryCondition(const std::vector < double >& x, const char SolName[], double& value, const int facename, const double time) {
  bool dirichlet = true; //dirichlet
  value = 0.;
  return dirichlet;
}

bool SetRefinementFlag(const std::vector < double >& x, const int& elemgroupnumber, const int& level) {

  bool refine = 0;

  if (elemgroupnumber == 6 && level < 4) refine = 1;

  if (elemgroupnumber == 7 && level < 5) refine = 1;

  if (elemgroupnumber == 8 && level < 6) refine = 1;

//   if (elemgroupnumber==6 && level<1) refine=1;
//   if (elemgroupnumber==7 && level<2) refine=1;
//   if (elemgroupnumber==8 && level<3) refine=1;

  return refine;

}

void AssemblePoisson_AD(MultiLevelProblem& ml_prob);    //, unsigned level, const unsigned &levelMax, const bool &assembleMatrix );

int main(int argc, char** args) {

  // init Petsc-MPI communicator
  FemusInit mpinit(argc, args, MPI_COMM_WORLD);

  // define multilevel mesh
  MultiLevelMesh mlMsh;
  // read coarse level mesh and generate finers level meshes
  double scalingFactor = 1.;
  //mlMsh.ReadCoarseMesh("./input/cube_hex.neu","seventh",scalingFactor);
  //mlMsh.ReadCoarseMesh("./input/square_quad.neu", "seventh", scalingFactor);
  mlMsh.ReadCoarseMesh("./input/quadAMR.neu", "seventh", scalingFactor);
  /* "seventh" is the order of accuracy that is used in the gauss integration scheme
     probably in the furure it is not going to be an argument of this function   */
  unsigned dim = mlMsh.GetDimension();

//   unsigned numberOfUniformLevels = 3;
//   unsigned numberOfSelectiveLevels = 0;
//   mlMsh.RefineMesh(numberOfUniformLevels , numberOfUniformLevels + numberOfSelectiveLevels, NULL);

  unsigned numberOfUniformLevels = 4;
  unsigned numberOfSelectiveLevels = 3;
  mlMsh.RefineMesh(numberOfUniformLevels + numberOfSelectiveLevels, numberOfUniformLevels , SetRefinementFlag);
  // Q01 Why does number Of UniformLevels add number Of Selective Levels

  // erase all the coarse mesh levels
  //mlMsh.EraseCoarseLevels(numberOfUniformLevels - 3);

  // print mesh info
  mlMsh.PrintInfo();

  MultiLevelSolution mlSol(&mlMsh);

  // add variables to mlSol

  mlSol.AddSolution("V", LAGRANGE, SECOND);

  mlSol.AddSolution("U", LAGRANGE, SECOND);

  mlSol.Initialize("All");

  // attach the boundary condition function and generate boundary data
  mlSol.AttachSetBoundaryConditionFunction(SetBoundaryCondition);

  mlSol.GenerateBdc("All");

  // define the multilevel problem attach the mlSol object to it
  MultiLevelProblem mlProb(&mlSol);

  // add system Poisson in mlProb as a Linear Implicit System
  NonLinearImplicitSystem& system = mlProb.add_system < NonLinearImplicitSystem > ("Poisson");

  // add solution "u" to system
  system.AddSolutionToSystemPDE("U");

  //system.SetMgSmoother(GMRES_SMOOTHER);
  system.SetMgSmoother(ASM_SMOOTHER); // Additive Swartz Method
  // attach the assembling function to system
  system.SetAssembleFunction(AssemblePoisson_AD);

  system.SetMaxNumberOfNonLinearIterations(10);
  system.SetMaxNumberOfLinearIterations(3);
  system.SetLinearConvergenceTolerance(1.e-12);
  system.SetNonLinearConvergenceTolerance(1.e-8);
  system.SetMgType(F_CYCLE);

  system.SetNumberPreSmoothingStep(0);  // What's Pre (post) smoothing step? Is related to smoothstep?
  system.SetNumberPostSmoothingStep(2);
  // initilaize and solve the system
  system.init();

  system.SetSolverFineGrids(GMRES);
  system.SetPreconditionerFineGrids(ILU_PRECOND);
  system.SetTolerances(1.e-3, 1.e-20, 1.e+50, 5);

  system.SetNumberOfSchurVariables(1);
  system.SetElementBlockNumber(4); // Q2 What's Block number?
  //system.SetDirichletBCsHandling(ELIMINATION);
  //system.solve();
  system.MGsolve();

  // print solutions
  std::vector < std::string > variablesToBePrinted;
  variablesToBePrinted.push_back("All");

  VTKWriter vtkIO(&mlSol);
  vtkIO.write(DEFAULT_OUTPUTDIR, "biquadratic", variablesToBePrinted);

  GMVWriter gmvIO(&mlSol);
  variablesToBePrinted.push_back("all");
  gmvIO.SetDebugOutput(true);
  gmvIO.write(DEFAULT_OUTPUTDIR, "biquadratic", variablesToBePrinted);

  return 0;
}

void AssemblePoisson_AD(MultiLevelProblem& ml_prob) {
  //  ml_prob is the global object from/to where get/set all the data
  //  level is the level of the PDE system to be assembled
  //  levelMax is the Maximum level of the MultiLevelProblem
  //  assembleMatrix is a flag that tells if only the residual or also the matrix should be assembled

  // call the adept stack object
  adept::Stack& s = FemusInit::_adeptStack;

  //  extract pointers to the several objects that we are going to use
  NonLinearImplicitSystem* mlPdeSys   = &ml_prob.get_system<NonLinearImplicitSystem> ("Poisson");   // pointer to the linear implicit system named "Poisson"
  const unsigned level = mlPdeSys->GetLevelToAssemble();

  Mesh*           msh         = ml_prob._ml_msh->GetLevel(level);    // pointer to the mesh (level) object
  elem*           el          = msh->el;  // pointer to the elem object in msh (level)

  MultiLevelSolution*   mlSol = ml_prob._ml_sol;  // pointer to the multilevel solution object
  Solution*   sol             = ml_prob._ml_sol->GetSolutionLevel(level);    // pointer to the solution (level) object

  LinearEquationSolver* pdeSys  = mlPdeSys->_LinSolver[level];  // pointer to the equation (level) object
  SparseMatrix*   KK          = pdeSys->_KK;  // pointer to the global stifness matrix object in pdeSys (level)
  NumericVector*  RES         = pdeSys->_RES; // pointer to the global residual vector object in pdeSys (level)

  const unsigned  dim = msh->GetDimension(); // get the domain dimension of the problem
  unsigned dim2 = (3 * (dim - 1) + !(dim - 1));        // dim2 is the number of second order partial derivatives (1,3,6 depending on the dimension)
  unsigned    iproc = msh->processor_id(); // get the process_id (for parallel computation)

  // reserve memory for the local standar vectors
  const unsigned maxSize = static_cast< unsigned >(ceil(pow(3, dim)));          // conservative: based on line3, quad9, hex27

  //solution variable
  unsigned solUIndex;
  solUIndex = mlSol->GetIndex("U");    // get the position of "U" in the ml_sol object = 0
  unsigned solUType = mlSol->GetSolutionType(solUIndex);    // get the finite element type for "T"

  unsigned solUPdeIndex;
  solUPdeIndex = mlPdeSys->GetSolPdeIndex("U");    // get the position of "U" in the pdeSys object = 0

  std::cout << solUIndex << " " << solUPdeIndex << std::endl;

  vector < adept::adouble >  solU; // local solution
  vector< adept::adouble > aResU; // local redidual vector

  vector < vector < double > > crdX(dim);    // local coordinates
  unsigned crdXType = 2; // get the finite element type for "x", it is always 2 (LAGRANGE QUADRATIC)

  solU.reserve(maxSize);
  aResU.reserve(maxSize);

  for (unsigned  k = 0; k < dim; k++) {
    crdX[k].reserve(maxSize);
  }

  vector <double> phi;  // local test function
  vector <double> phi_x; // local test function first order partial derivatives
  vector <double> phi_xx; // local test function second order partial derivatives

  phi.reserve(maxSize);
  phi_x.reserve(maxSize * dim);
  phi_xx.reserve(maxSize * dim2);

  double weight; // gauss point weight

  vector< int > sysDof; // local to global pdeSys dofs
  sysDof.reserve(maxSize);

  vector< double > ResU; // local redidual vector
  ResU.reserve(maxSize);

  vector < double > Jac;
  Jac.reserve(maxSize * maxSize);

  KK->zero(); // Set to zero all the entries of the Global Matrix

  // element loop: each process loops only on the elements that owns
  for (int iel = msh->_elementOffset[iproc]; iel < msh->_elementOffset[iproc + 1]; iel++) {

    unsigned kel = iel; //msh->IS_Mts2Gmt_elem[iel]; // mapping between paralell dof and mesh dof
    short unsigned kelGeom = el->GetElementType(kel);      // element geometry type

    unsigned nDofsU = el->GetElementDofNumber(kel, solUType);      // number of solution element dofs
    unsigned nDofsX = el->GetElementDofNumber(kel, crdXType);      // number of solution element dofs

    // resize local arrays
    sysDof.resize(nDofsU);

    solU.resize(nDofsU);

    for (unsigned  k = 0; k < dim; k++) {
      crdX[k].resize(nDofsX);
    }

    aResU.assign(nDofsU, 0);

    // local storage of global mapping and solution
    for (unsigned i = 0; i < nDofsU; i++) {
      unsigned solUDof = msh->GetSolutionDof(i, iel, solUType);    // local to global mapping of the solution U
      solU[i] = (*sol->_Sol[solUIndex])(solUDof);      // value of the solution U in the dofs
      sysDof[i] = pdeSys->GetSystemDof(solUIndex, solUPdeIndex, i, iel);    // local to global mapping between solution U and system
    }

    // local storage of coordinates
    for (unsigned i = 0; i < nDofsX; i++) {
      unsigned coordXDof  = msh->GetSolutionDof(i, iel, crdXType);   // local to global mapping of the coordinate X[dim]

      for (unsigned k = 0; k < dim; k++) {
        crdX[k][i] = (*msh->_topology->_Sol[k])(coordXDof);      // value of the solution X[dim]
      }
    }

    s.new_recording();

    // *** Gauss point loop *** // I don't understand clearly What variable ig mean and the principle in the Gauss point
    for (unsigned ig = 0; ig < msh->_finiteElement[kelGeom][solUType]->GetGaussPointNumber(); ig++) {
      // *** get gauss point weight, test function and test function partial derivatives ***
      msh->_finiteElement[kelGeom][solUType]->Jacobian(crdX, ig, weight, phi, phi_x, phi_xx);

      adept::adouble solUig = 0; // solution U in the gauss point
      vector < adept::adouble > gradSolUig(dim, 0.); // gradient of solution U in the gauss point

      for (unsigned i = 0; i < nDofsU; i++) {
        solUig += phi[i] * solU[i];

        for (unsigned j = 0; j < dim; j++) {
          gradSolUig[j] += phi_x[i * dim + j] * solU[i];
        }
      }

      double nu = 1.;

      // *** phiU_i loop ***
      for (unsigned i = 0; i < nDofsU; i++) {
        adept::adouble LaplaceU = 0.;

        for (unsigned j = 0; j < dim; j++) {
          LaplaceU +=  nu * phi_x[i * dim + j] * gradSolUig[j];
        }

        aResU[i] += (phi[i] - LaplaceU) * weight;
      } // end phiU_i loop
    } // end gauss point loop

    // } // endif single element not refined or fine grid loop

    //--------------------------------------------------------------------------------------------------------
    // Add the local Matrix/Vector into the global Matrix/Vector

    //copy the value of the adept::adoube aRes in double Res and store them in RES
    ResU.resize(nDofsU);    //resize

    for (int i = 0; i < nDofsU; i++) {
      ResU[i] = -aResU[i].value();
    }

    RES->add_vector_blocked(ResU, sysDof);

    //Extarct and store the Jacobian

    Jac.resize(nDofsU * nDofsU);
    // define the dependent variables
    s.dependent(&aResU[0], nDofsU);

    // define the independent variables
    s.independent(&solU[0], nDofsU);

    // get the and store jacobian matrix (row-major)
    s.jacobian(&Jac[0] , true);
    KK->add_matrix_blocked(Jac, sysDof, sysDof);

    s.clear_independents();
    s.clear_dependents();

  } //end element loop for each process

  RES->close();

  KK->close();

  // ***************** END ASSEMBLY *******************
}

<commit_msg>add a question commit for gauss point loop<commit_after>/** \file Ex7.cpp
 *  \brief This example shows how to set and solve the weak form
 *   of the Boussinesq appoximation of the Navier-Stokes Equation
 *
 *  \f{eqnarray*}
 *  && \mathbf{V} \cdot \nabla T - \nabla \cdot\alpha \nabla T = 0 \\
 *  && \mathbf{V} \cdot \nabla \mathbf{V} - \nabla \cdot \nu (\nabla \mathbf{V} +(\nabla \mathbf{V})^T)
 *  +\nabla P = \beta T \mathbf{j} \\
 *  && \nabla \cdot \mathbf{V} = 0
 *  \f}
 *  in a unit box domain (in 2D and 3D) with given temperature 0 and 1 on
 *  the left and right walls, respectively, and insulated walls elsewhere.
 *  \author Eugenio Aulisa
 */

#include "FemusInit.hpp"
#include "MultiLevelProblem.hpp"
#include "NumericVector.hpp"
#include "VTKWriter.hpp"
#include "GMVWriter.hpp"
#include "NonLinearImplicitSystem.hpp"
#include "adept.h"

using namespace femus;

bool SetBoundaryCondition(const std::vector < double >& x, const char SolName[], double& value, const int facename, const double time) {
  bool dirichlet = true; //dirichlet
  value = 0.;
  return dirichlet;
}

bool SetRefinementFlag(const std::vector < double >& x, const int& elemgroupnumber, const int& level) {

  bool refine = 0;

  if (elemgroupnumber == 6 && level < 4) refine = 1;

  if (elemgroupnumber == 7 && level < 5) refine = 1;

  if (elemgroupnumber == 8 && level < 6) refine = 1;

//   if (elemgroupnumber==6 && level<1) refine=1;
//   if (elemgroupnumber==7 && level<2) refine=1;
//   if (elemgroupnumber==8 && level<3) refine=1;

  return refine;

}

void AssemblePoisson_AD(MultiLevelProblem& ml_prob);    //, unsigned level, const unsigned &levelMax, const bool &assembleMatrix );

int main(int argc, char** args) {

  // init Petsc-MPI communicator
  FemusInit mpinit(argc, args, MPI_COMM_WORLD);

  // define multilevel mesh
  MultiLevelMesh mlMsh;
  // read coarse level mesh and generate finers level meshes
  double scalingFactor = 1.;
  //mlMsh.ReadCoarseMesh("./input/cube_hex.neu","seventh",scalingFactor);
  //mlMsh.ReadCoarseMesh("./input/square_quad.neu", "seventh", scalingFactor);
  mlMsh.ReadCoarseMesh("./input/quadAMR.neu", "seventh", scalingFactor);
  /* "seventh" is the order of accuracy that is used in the gauss integration scheme
     probably in the furure it is not going to be an argument of this function   */
  unsigned dim = mlMsh.GetDimension();

//   unsigned numberOfUniformLevels = 3;
//   unsigned numberOfSelectiveLevels = 0;
//   mlMsh.RefineMesh(numberOfUniformLevels , numberOfUniformLevels + numberOfSelectiveLevels, NULL);

  unsigned numberOfUniformLevels = 4;
  unsigned numberOfSelectiveLevels = 3;
  mlMsh.RefineMesh(numberOfUniformLevels + numberOfSelectiveLevels, numberOfUniformLevels , SetRefinementFlag);
  // Q01 Why does number Of UniformLevels add number Of Selective Levels

  // erase all the coarse mesh levels
  //mlMsh.EraseCoarseLevels(numberOfUniformLevels - 3);

  // print mesh info
  mlMsh.PrintInfo();

  MultiLevelSolution mlSol(&mlMsh);

  // add variables to mlSol

  mlSol.AddSolution("V", LAGRANGE, SECOND);

  mlSol.AddSolution("U", LAGRANGE, SECOND);

  mlSol.Initialize("All");

  // attach the boundary condition function and generate boundary data
  mlSol.AttachSetBoundaryConditionFunction(SetBoundaryCondition);

  mlSol.GenerateBdc("All");

  // define the multilevel problem attach the mlSol object to it
  MultiLevelProblem mlProb(&mlSol);

  // add system Poisson in mlProb as a Linear Implicit System
  NonLinearImplicitSystem& system = mlProb.add_system < NonLinearImplicitSystem > ("Poisson");

  // add solution "u" to system
  system.AddSolutionToSystemPDE("U");

  //system.SetMgSmoother(GMRES_SMOOTHER);
  system.SetMgSmoother(ASM_SMOOTHER); // Additive Swartz Method
  // attach the assembling function to system
  system.SetAssembleFunction(AssemblePoisson_AD);

  system.SetMaxNumberOfNonLinearIterations(10);
  system.SetMaxNumberOfLinearIterations(3);
  system.SetLinearConvergenceTolerance(1.e-12);
  system.SetNonLinearConvergenceTolerance(1.e-8);
  system.SetMgType(F_CYCLE);

  system.SetNumberPreSmoothingStep(0);  // What's Pre (post) smoothing step? Is related to smoothstep?
  system.SetNumberPostSmoothingStep(2);
  // initilaize and solve the system
  system.init();

  system.SetSolverFineGrids(GMRES);
  system.SetPreconditionerFineGrids(ILU_PRECOND);
  system.SetTolerances(1.e-3, 1.e-20, 1.e+50, 5);

  system.SetNumberOfSchurVariables(1);
  system.SetElementBlockNumber(4); // Q2 What's Block number?
  //system.SetDirichletBCsHandling(ELIMINATION);
  //system.solve();
  system.MGsolve();

  // print solutions
  std::vector < std::string > variablesToBePrinted;
  variablesToBePrinted.push_back("All");

  VTKWriter vtkIO(&mlSol);
  vtkIO.write(DEFAULT_OUTPUTDIR, "biquadratic", variablesToBePrinted);

  GMVWriter gmvIO(&mlSol);
  variablesToBePrinted.push_back("all");
  gmvIO.SetDebugOutput(true);
  gmvIO.write(DEFAULT_OUTPUTDIR, "biquadratic", variablesToBePrinted);

  return 0;
}

void AssemblePoisson_AD(MultiLevelProblem& ml_prob) {
  //  ml_prob is the global object from/to where get/set all the data
  //  level is the level of the PDE system to be assembled
  //  levelMax is the Maximum level of the MultiLevelProblem
  //  assembleMatrix is a flag that tells if only the residual or also the matrix should be assembled

  // call the adept stack object
  adept::Stack& s = FemusInit::_adeptStack;

  //  extract pointers to the several objects that we are going to use
  NonLinearImplicitSystem* mlPdeSys   = &ml_prob.get_system<NonLinearImplicitSystem> ("Poisson");   // pointer to the linear implicit system named "Poisson"
  const unsigned level = mlPdeSys->GetLevelToAssemble();

  Mesh*           msh         = ml_prob._ml_msh->GetLevel(level);    // pointer to the mesh (level) object
  elem*           el          = msh->el;  // pointer to the elem object in msh (level)

  MultiLevelSolution*   mlSol = ml_prob._ml_sol;  // pointer to the multilevel solution object
  Solution*   sol             = ml_prob._ml_sol->GetSolutionLevel(level);    // pointer to the solution (level) object

  LinearEquationSolver* pdeSys  = mlPdeSys->_LinSolver[level];  // pointer to the equation (level) object
  SparseMatrix*   KK          = pdeSys->_KK;  // pointer to the global stifness matrix object in pdeSys (level)
  NumericVector*  RES         = pdeSys->_RES; // pointer to the global residual vector object in pdeSys (level)

  const unsigned  dim = msh->GetDimension(); // get the domain dimension of the problem
  unsigned dim2 = (3 * (dim - 1) + !(dim - 1));        // dim2 is the number of second order partial derivatives (1,3,6 depending on the dimension)
  unsigned    iproc = msh->processor_id(); // get the process_id (for parallel computation)

  // reserve memory for the local standar vectors
  const unsigned maxSize = static_cast< unsigned >(ceil(pow(3, dim)));          // conservative: based on line3, quad9, hex27

  //solution variable
  unsigned solUIndex;
  solUIndex = mlSol->GetIndex("U");    // get the position of "U" in the ml_sol object = 0
  unsigned solUType = mlSol->GetSolutionType(solUIndex);    // get the finite element type for "T"

  unsigned solUPdeIndex;
  solUPdeIndex = mlPdeSys->GetSolPdeIndex("U");    // get the position of "U" in the pdeSys object = 0

  std::cout << solUIndex << " " << solUPdeIndex << std::endl;

  vector < adept::adouble >  solU; // local solution
  vector< adept::adouble > aResU; // local redidual vector

  vector < vector < double > > crdX(dim);    // local coordinates
  unsigned crdXType = 2; // get the finite element type for "x", it is always 2 (LAGRANGE QUADRATIC)

  solU.reserve(maxSize);
  aResU.reserve(maxSize);

  for (unsigned  k = 0; k < dim; k++) {
    crdX[k].reserve(maxSize);
  }

  vector <double> phi;  // local test function
  vector <double> phi_x; // local test function first order partial derivatives
  vector <double> phi_xx; // local test function second order partial derivatives

  phi.reserve(maxSize);
  phi_x.reserve(maxSize * dim);
  phi_xx.reserve(maxSize * dim2);

  double weight; // gauss point weight

  vector< int > sysDof; // local to global pdeSys dofs
  sysDof.reserve(maxSize);

  vector< double > ResU; // local redidual vector
  ResU.reserve(maxSize);

  vector < double > Jac;
  Jac.reserve(maxSize * maxSize);

  KK->zero(); // Set to zero all the entries of the Global Matrix

  // element loop: each process loops only on the elements that owns
  for (int iel = msh->_elementOffset[iproc]; iel < msh->_elementOffset[iproc + 1]; iel++) {

    unsigned kel = iel; //msh->IS_Mts2Gmt_elem[iel]; // mapping between paralell dof and mesh dof
    short unsigned kelGeom = el->GetElementType(kel);      // element geometry type

    unsigned nDofsU = el->GetElementDofNumber(kel, solUType);      // number of solution element dofs
    unsigned nDofsX = el->GetElementDofNumber(kel, crdXType);      // number of solution element dofs

    // resize local arrays
    sysDof.resize(nDofsU);

    solU.resize(nDofsU);

    for (unsigned  k = 0; k < dim; k++) {
      crdX[k].resize(nDofsX);
    }

    aResU.assign(nDofsU, 0);

    // local storage of global mapping and solution
    for (unsigned i = 0; i < nDofsU; i++) {
      unsigned solUDof = msh->GetSolutionDof(i, iel, solUType);    // local to global mapping of the solution U
      solU[i] = (*sol->_Sol[solUIndex])(solUDof);      // value of the solution U in the dofs
      sysDof[i] = pdeSys->GetSystemDof(solUIndex, solUPdeIndex, i, iel);    // local to global mapping between solution U and system
    }

    // local storage of coordinates
    for (unsigned i = 0; i < nDofsX; i++) {
      unsigned coordXDof  = msh->GetSolutionDof(i, iel, crdXType);   // local to global mapping of the coordinate X[dim]

      for (unsigned k = 0; k < dim; k++) {
        crdX[k][i] = (*msh->_topology->_Sol[k])(coordXDof);      // value of the solution X[dim]
      }
    }

    s.new_recording();

    // *** Gauss point loop *** // Q I don't clearly understand this loop. Is it sol the qestionn U=\sum \nu_i *\phi_i.
    for (unsigned ig = 0; ig < msh->_finiteElement[kelGeom][solUType]->GetGaussPointNumber(); ig++) {
      // *** get gauss point weight, test function and test function partial derivatives ***
      msh->_finiteElement[kelGeom][solUType]->Jacobian(crdX, ig, weight, phi, phi_x, phi_xx);

      adept::adouble solUig = 0; // solution U in the gauss point
      vector < adept::adouble > gradSolUig(dim, 0.); // gradient of solution U in the gauss point

      for (unsigned i = 0; i < nDofsU; i++) {
        solUig += phi[i] * solU[i];

        for (unsigned j = 0; j < dim; j++) {
          gradSolUig[j] += phi_x[i * dim + j] * solU[i];
        }
      }

      double nu = 1.;

      // *** phiU_i loop ***
      for (unsigned i = 0; i < nDofsU; i++) {
        adept::adouble LaplaceU = 0.;

        for (unsigned j = 0; j < dim; j++) {
          LaplaceU +=  nu * phi_x[i * dim + j] * gradSolUig[j];
        }

        aResU[i] += (phi[i] - LaplaceU) * weight;
      } // end phiU_i loop
    } // end gauss point loop

    // } // endif single element not refined or fine grid loop

    //--------------------------------------------------------------------------------------------------------
    // Add the local Matrix/Vector into the global Matrix/Vector

    //copy the value of the adept::adoube aRes in double Res and store them in RES
    ResU.resize(nDofsU);    //resize

    for (int i = 0; i < nDofsU; i++) {
      ResU[i] = -aResU[i].value();
    }

    RES->add_vector_blocked(ResU, sysDof);

    //Extarct and store the Jacobian

    Jac.resize(nDofsU * nDofsU);
    // define the dependent variables
    s.dependent(&aResU[0], nDofsU);

    // define the independent variables
    s.independent(&solU[0], nDofsU);

    // get the and store jacobian matrix (row-major)
    s.jacobian(&Jac[0] , true);
    KK->add_matrix_blocked(Jac, sysDof, sysDof);

    s.clear_independents();
    s.clear_dependents();

  } //end element loop for each process

  RES->close();

  KK->close();

  // ***************** END ASSEMBLY *******************
}

<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 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 "modules/dreamview/backend/sim_control/sim_control.h"

#include <cmath>

#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/time/time.h"
#include "modules/common/util/file.h"

namespace apollo {
namespace dreamview {

using apollo::common::adapter::AdapterManager;
using apollo::common::math::HeadingToQuaternion;
using apollo::common::math::NormalizeAngle;
using apollo::common::time::Clock;
using apollo::common::TrajectoryPoint;
using apollo::common::util::GetProtoFromFile;
using apollo::hdmap::RoutingResult;
using apollo::localization::LocalizationEstimate;
using apollo::canbus::Chassis;

SimControl::SimControl()
    : prev_point_index_(0),
      next_point_index_(0),
      received_planning_(false),
      initial_start_(true),
      enabled_(FLAGS_enable_sim_control) {
  if (enabled_) {
    RoutingResult routing;
    if (!GetProtoFromFile(FLAGS_routing_result_file, &routing)) {
      AWARN << "Unable to read start point from file: "
            << FLAGS_routing_result_file;
      return;
    }
    SetStartPoint(routing);
  }
}

void SimControl::SetStartPoint(const RoutingResult& routing) {
  next_point_.set_v(0.0);
  next_point_.set_a(0.0);

  auto* p = next_point_.mutable_path_point();

  p->set_x(routing.routing_request().start().pose().x());
  p->set_y(routing.routing_request().start().pose().y());
  p->set_z(0.0);

  // TODO(siyangy): Calculate the real theta when the API is ready.
  p->set_theta(0.0);
  p->set_kappa(0.0);
  p->set_s(0.0);

  prev_point_index_ = next_point_index_ = 0;
  received_planning_ = false;

  if (enabled_) {
    Start();
  }
}

void SimControl::Start() {
  if (initial_start_) {
    // Setup planning and routing result data callback.
    AdapterManager::SetPlanningCallback(&SimControl::OnPlanning, this);
    AdapterManager::SetRoutingResultCallback(&SimControl::SetStartPoint, this);

    // Start timer to publish localiztion and chassis messages.
    sim_control_timer_ = AdapterManager::CreateTimer(
        ros::Duration(kSimControlInterval), &SimControl::TimerCallback, this);

    initial_start_ = false;
  } else {
    sim_control_timer_.start();
  }
}

void SimControl::Stop() {
  sim_control_timer_.stop();
}

void SimControl::OnPlanning(const apollo::planning::ADCTrajectory& trajectory) {
  // Reset current trajectory and the indices upon receiving a new trajectory.
  current_trajectory_ = trajectory;
  prev_point_index_ = 0;
  next_point_index_ = 0;
  received_planning_ = true;
}

void SimControl::Freeze() {
  next_point_.set_v(0.0);
  next_point_.set_a(0.0);
  prev_point_ = next_point_;
}

double SimControl::AbsoluteTimeOfNextPoint() {
  return current_trajectory_.header().timestamp_sec() +
         current_trajectory_.trajectory_point(next_point_index_)
             .relative_time();
}

bool SimControl::NextPointWithinRange() {
  return next_point_index_ < current_trajectory_.trajectory_point_size() - 1;
}

void SimControl::TimerCallback(const ros::TimerEvent& event) {
  // Result of the interpolation.
  double lambda = 0;
  auto current_time = apollo::common::time::ToSecond(Clock::Now());

  if (!received_planning_) {
    prev_point_ = next_point_;
  } else {
    if (current_trajectory_.estop().is_estop() || !NextPointWithinRange()) {
      // Freeze the car when there's an estop or the current trajectory has been
      // exhausted.
      Freeze();
    } else {
      // Determine the status of the car based on received planning message.
      double timestamp = current_trajectory_.header().timestamp_sec();

      while (NextPointWithinRange() &&
             current_time > AbsoluteTimeOfNextPoint()) {
        ++next_point_index_;
      }

      if (next_point_index_ == 0) {
        AERROR << "First trajectory point is a future point!";
        return;
      }

      if (current_time > AbsoluteTimeOfNextPoint()) {
        prev_point_index_ = next_point_index_;
      } else {
        prev_point_index_ = next_point_index_ - 1;
      }

      next_point_ = current_trajectory_.trajectory_point(next_point_index_);
      prev_point_ = current_trajectory_.trajectory_point(prev_point_index_);

      // Calculate the ratio based on the the position of current time in
      // between the previous point and the next point, where lamda =
      // (current_point - prev_point) / (next_point - prev_point).
      if (next_point_index_ != prev_point_index_) {
        lambda = (current_time - timestamp - prev_point_.relative_time()) /
                 (next_point_.relative_time() - prev_point_.relative_time());
      }
    }
  }

  PublishChassis(lambda);
  PublishLocalization(lambda);
}

void SimControl::PublishChassis(double lambda) {
  Chassis chassis;
  AdapterManager::FillChassisHeader("SimControl", chassis.mutable_header());

  chassis.set_engine_started(true);
  chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
  chassis.set_gear_location(Chassis::GEAR_DRIVE);
  // TODO(siyangy): set the real steering percentage.
  chassis.set_steering_percentage(0.0);

  double cur_speed = Interpolate(prev_point_.v(), next_point_.v(), lambda);
  chassis.set_speed_mps(cur_speed);

  AdapterManager::PublishChassis(chassis);
}

void SimControl::PublishLocalization(double lambda) {
  LocalizationEstimate localization;
  AdapterManager::FillLocalizationHeader("SimControl",
                                         localization.mutable_header());

  auto* pose = localization.mutable_pose();
  auto prev = prev_point_.path_point();
  auto next = next_point_.path_point();

  // Set position
  double cur_x = Interpolate(prev.x(), next.x(), lambda);
  pose->mutable_position()->set_x(cur_x);
  double cur_y = Interpolate(prev.y(), next.y(), lambda);
  pose->mutable_position()->set_y(cur_y);
  double cur_z = Interpolate(prev.z(), next.z(), lambda);
  pose->mutable_position()->set_z(cur_z);

  // Set orientation
  double cur_theta = NormalizeAngle(
      prev.theta() + lambda * NormalizeAngle(next.theta() - prev.theta()));

  Eigen::Quaternion<double> cur_orientation =
      HeadingToQuaternion<double>(cur_theta);
  pose->mutable_orientation()->set_qx(cur_orientation.x());
  pose->mutable_orientation()->set_qy(cur_orientation.y());
  pose->mutable_orientation()->set_qz(cur_orientation.z());
  pose->mutable_orientation()->set_qw(cur_orientation.w());

  // Set linear_velocity
  double cur_speed = Interpolate(prev_point_.v(), next_point_.v(), lambda);
  pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * cur_speed);
  pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * cur_speed);
  pose->mutable_linear_velocity()->set_z(0);

  // Set angular_velocity
  double cur_curvature = Interpolate(prev.kappa(), next.kappa(), lambda);
  pose->mutable_angular_velocity()->set_x(0);
  pose->mutable_angular_velocity()->set_y(0);
  pose->mutable_angular_velocity()->set_z(cur_speed * cur_curvature);

  // Set linear_acceleration
  double cur_acceleration_s =
      Interpolate(prev_point_.a(), next_point_.a(), lambda);
  auto* linear_acceleration = pose->mutable_linear_acceleration();
  linear_acceleration->set_x(std::cos(cur_theta) * cur_acceleration_s);
  linear_acceleration->set_y(std::sin(cur_theta) * cur_acceleration_s);
  linear_acceleration->set_z(0);

  AdapterManager::PublishLocalization(localization);
}

}  // namespace dreamview
}  // namespace apollo
<commit_msg>Fix heading in sim control (#289)<commit_after>/******************************************************************************
 * Copyright 2017 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 "modules/dreamview/backend/sim_control/sim_control.h"

#include <cmath>

#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/time/time.h"
#include "modules/common/util/file.h"

namespace apollo {
namespace dreamview {

using apollo::common::adapter::AdapterManager;
using apollo::common::math::HeadingToQuaternion;
using apollo::common::math::QuaternionToHeading;
using apollo::common::math::NormalizeAngle;
using apollo::common::time::Clock;
using apollo::common::TrajectoryPoint;
using apollo::common::util::GetProtoFromFile;
using apollo::hdmap::RoutingResult;
using apollo::localization::LocalizationEstimate;
using apollo::canbus::Chassis;

SimControl::SimControl()
    : prev_point_index_(0),
      next_point_index_(0),
      received_planning_(false),
      initial_start_(true),
      enabled_(FLAGS_enable_sim_control) {
  if (enabled_) {
    RoutingResult routing;
    if (!GetProtoFromFile(FLAGS_routing_result_file, &routing)) {
      AWARN << "Unable to read start point from file: "
            << FLAGS_routing_result_file;
      return;
    }
    SetStartPoint(routing);
  }
}

void SimControl::SetStartPoint(const RoutingResult& routing) {
  next_point_.set_v(0.0);
  next_point_.set_a(0.0);

  auto* p = next_point_.mutable_path_point();

  p->set_x(routing.routing_request().start().pose().x());
  p->set_y(routing.routing_request().start().pose().y());
  p->set_z(0.0);

  // TODO(siyangy): Calculate the real theta when the API is ready.
  p->set_theta(0.0);
  p->set_kappa(0.0);
  p->set_s(0.0);

  prev_point_index_ = next_point_index_ = 0;
  received_planning_ = false;

  if (enabled_) {
    Start();
  }
}

void SimControl::Start() {
  if (initial_start_) {
    // Setup planning and routing result data callback.
    AdapterManager::SetPlanningCallback(&SimControl::OnPlanning, this);
    AdapterManager::SetRoutingResultCallback(&SimControl::SetStartPoint, this);

    // Start timer to publish localiztion and chassis messages.
    sim_control_timer_ = AdapterManager::CreateTimer(
        ros::Duration(kSimControlInterval), &SimControl::TimerCallback, this);

    initial_start_ = false;
  } else {
    sim_control_timer_.start();
  }
}

void SimControl::Stop() {
  sim_control_timer_.stop();
}

void SimControl::OnPlanning(const apollo::planning::ADCTrajectory& trajectory) {
  // Reset current trajectory and the indices upon receiving a new trajectory.
  current_trajectory_ = trajectory;
  prev_point_index_ = 0;
  next_point_index_ = 0;
  received_planning_ = true;
}

void SimControl::Freeze() {
  next_point_.set_v(0.0);
  next_point_.set_a(0.0);
  prev_point_ = next_point_;
}

double SimControl::AbsoluteTimeOfNextPoint() {
  return current_trajectory_.header().timestamp_sec() +
         current_trajectory_.trajectory_point(next_point_index_)
             .relative_time();
}

bool SimControl::NextPointWithinRange() {
  return next_point_index_ < current_trajectory_.trajectory_point_size() - 1;
}

void SimControl::TimerCallback(const ros::TimerEvent& event) {
  // Result of the interpolation.
  double lambda = 0;
  auto current_time = apollo::common::time::ToSecond(Clock::Now());

  if (!received_planning_) {
    prev_point_ = next_point_;
  } else {
    if (current_trajectory_.estop().is_estop() || !NextPointWithinRange()) {
      // Freeze the car when there's an estop or the current trajectory has been
      // exhausted.
      Freeze();
    } else {
      // Determine the status of the car based on received planning message.
      double timestamp = current_trajectory_.header().timestamp_sec();

      while (NextPointWithinRange() &&
             current_time > AbsoluteTimeOfNextPoint()) {
        ++next_point_index_;
      }

      if (next_point_index_ == 0) {
        AERROR << "First trajectory point is a future point!";
        return;
      }

      if (current_time > AbsoluteTimeOfNextPoint()) {
        prev_point_index_ = next_point_index_;
      } else {
        prev_point_index_ = next_point_index_ - 1;
      }

      next_point_ = current_trajectory_.trajectory_point(next_point_index_);
      prev_point_ = current_trajectory_.trajectory_point(prev_point_index_);

      // Calculate the ratio based on the the position of current time in
      // between the previous point and the next point, where lamda =
      // (current_point - prev_point) / (next_point - prev_point).
      if (next_point_index_ != prev_point_index_) {
        lambda = (current_time - timestamp - prev_point_.relative_time()) /
                 (next_point_.relative_time() - prev_point_.relative_time());
      }
    }
  }

  PublishChassis(lambda);
  PublishLocalization(lambda);
}

void SimControl::PublishChassis(double lambda) {
  Chassis chassis;
  AdapterManager::FillChassisHeader("SimControl", chassis.mutable_header());

  chassis.set_engine_started(true);
  chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
  chassis.set_gear_location(Chassis::GEAR_DRIVE);
  // TODO(siyangy): set the real steering percentage.
  chassis.set_steering_percentage(0.0);

  double cur_speed = Interpolate(prev_point_.v(), next_point_.v(), lambda);
  chassis.set_speed_mps(cur_speed);

  AdapterManager::PublishChassis(chassis);
}

void SimControl::PublishLocalization(double lambda) {
  LocalizationEstimate localization;
  AdapterManager::FillLocalizationHeader("SimControl",
                                         localization.mutable_header());

  auto* pose = localization.mutable_pose();
  auto prev = prev_point_.path_point();
  auto next = next_point_.path_point();

  // Set position
  double cur_x = Interpolate(prev.x(), next.x(), lambda);
  pose->mutable_position()->set_x(cur_x);
  double cur_y = Interpolate(prev.y(), next.y(), lambda);
  pose->mutable_position()->set_y(cur_y);
  double cur_z = Interpolate(prev.z(), next.z(), lambda);
  pose->mutable_position()->set_z(cur_z);

  // Set orientation and heading
  double cur_theta = NormalizeAngle(
      prev.theta() + lambda * NormalizeAngle(next.theta() - prev.theta()));

  Eigen::Quaternion<double> cur_orientation =
      HeadingToQuaternion<double>(cur_theta);
  pose->mutable_orientation()->set_qw(cur_orientation.w());
  pose->mutable_orientation()->set_qx(cur_orientation.x());
  pose->mutable_orientation()->set_qy(cur_orientation.y());
  pose->mutable_orientation()->set_qz(cur_orientation.z());
  pose->set_heading(cur_theta);

  // Set linear_velocity
  double cur_speed = Interpolate(prev_point_.v(), next_point_.v(), lambda);
  pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * cur_speed);
  pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * cur_speed);
  pose->mutable_linear_velocity()->set_z(0);

  // Set angular_velocity
  double cur_curvature = Interpolate(prev.kappa(), next.kappa(), lambda);
  pose->mutable_angular_velocity()->set_x(0);
  pose->mutable_angular_velocity()->set_y(0);
  pose->mutable_angular_velocity()->set_z(cur_speed * cur_curvature);

  // Set linear_acceleration
  double cur_acceleration_s =
      Interpolate(prev_point_.a(), next_point_.a(), lambda);
  auto* linear_acceleration = pose->mutable_linear_acceleration();
  linear_acceleration->set_x(std::cos(cur_theta) * cur_acceleration_s);
  linear_acceleration->set_y(std::sin(cur_theta) * cur_acceleration_s);
  linear_acceleration->set_z(0);

  AdapterManager::PublishLocalization(localization);
}

}  // namespace dreamview
}  // namespace apollo
<|endoftext|>


<commit_before>#include "TensorMechanicsModule.h"
#include "Factory.h"
#include "ActionFactory.h"
#include "Parser.h"

// tensor_mechanics
#include "TensorMechanicsAction.h"
#include "StressDivergenceTensors.h"
#include "LinearElasticMaterial.h"
#include "FiniteStrainElasticMaterial.h"
#include "RankTwoAux.h"
#include "RealTensorValueAux.h"
#include "RankFourAux.h"
#include "TensorElasticEnergyAux.h"

void
Elk::TensorMechanics::registerObjects()
{
  // tensor_mechanics
  registerKernel(StressDivergenceTensors);

  registerMaterial(LinearElasticMaterial);
  registerMaterial(FiniteStrainElasticMaterial);

  registerAux(RankTwoAux);
  registerAux(RealTensorValueAux);
  registerAux(RankFourAux);
  registerAux(TensorElasticEnergyAux);
}

void
Elk::TensorMechanics::associateSyntax(Syntax & syntax)
{
  syntax.registerActionSyntax("TensorMechanicsAction", "TensorMechanics/*");

  registerAction(TensorMechanicsAction, "add_kernel");
}
<commit_msg>Registered FiniteStrainPlastic<commit_after>#include "TensorMechanicsModule.h"
#include "Factory.h"
#include "ActionFactory.h"
#include "Parser.h"

// tensor_mechanics
#include "TensorMechanicsAction.h"
#include "StressDivergenceTensors.h"
#include "LinearElasticMaterial.h"
#include "FiniteStrainElasticMaterial.h"
#include "FiniteStrainPlasticMaterial.h"
#include "RankTwoAux.h"
#include "RealTensorValueAux.h"
#include "RankFourAux.h"
#include "TensorElasticEnergyAux.h"

void
Elk::TensorMechanics::registerObjects()
{
  // tensor_mechanics
  registerKernel(StressDivergenceTensors);

  registerMaterial(LinearElasticMaterial);
  registerMaterial(FiniteStrainElasticMaterial);
  registerMaterial(FiniteStrainPlasticMaterial);

  registerAux(RankTwoAux);
  registerAux(RealTensorValueAux);
  registerAux(RankFourAux);
  registerAux(TensorElasticEnergyAux);
}

void
Elk::TensorMechanics::associateSyntax(Syntax & syntax)
{
  syntax.registerActionSyntax("TensorMechanicsAction", "TensorMechanics/*");

  registerAction(TensorMechanicsAction, "add_kernel");
}
<|endoftext|>