Sound.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms of the
 * BSD-3-Clause license. See the accompanying LICENSE file for details.
 */
 
#include <yarp/sig/Sound.h>
#include <yarp/sig/Image.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/PortablePair.h>
#include <yarp/os/LogComponent.h>
#include <yarp/os/LogStream.h>
#include <yarp/os/Time.h>
#include <yarp/os/Value.h>
#include <functional>
 
#include <cstring>
#include <cstdio>
 
using namespace yarp::sig;
using namespace yarp::os;
 
namespace {
YARP_LOG_COMPONENT(SOUND, "yarp.sig.Sound")
}
 
#define HELPER(x) (*((FlexImage*)(x)))
 
Sound::Sound(size_t bytesPerSample)
{
    init(bytesPerSample);
    m_frequency = 0;
}
 
Sound::Sound(const Sound& alt) : yarp::os::Portable()
{
    init(alt.getBytesPerSample());
    FlexImage& img1 = HELPER(implementation);
    FlexImage& img2 = HELPER(alt.implementation);
    img1.copy(img2);
    m_frequency = alt.m_frequency;
    synchronize();
}
 
Sound& Sound::operator += (const Sound& alt)
{
    if (alt.m_channels!= m_channels)
    {
        yCError(SOUND, "unable to concatenate sounds with different number of channels!");
        return *this;
    }
    if (alt.m_frequency!= m_frequency)
    {
        yCError(SOUND, "unable to concatenate sounds with different sample rate!");
        return *this;
    }
 
    Sound orig= *this;
    this->resize(this->m_samples+alt.m_samples,m_channels);
 
    unsigned char* p1    = orig.getRawData();
    unsigned char* p2    = alt.getRawData();
    unsigned char* pout  = this->getRawData();
 
    for (size_t ch=0; ch<m_channels; ch++)
    {
        size_t out1 = ch* this->getBytesPerSample() * this->m_samples;
        size_t out2 = ch* this->getBytesPerSample() * this->m_samples + this->getBytesPerSample() * orig.m_samples;
 
        size_t ori1 = ch * orig.getBytesPerSample() * orig.m_samples;
        size_t s1   = orig.getBytesPerSample() * orig.m_samples;
 
        size_t alt1 = ch * orig.getBytesPerSample() * alt.m_samples;
        unsigned int s2 = alt.getBytesPerSample() * alt.m_samples;
 
        memcpy((void *) &pout[out1], (void *) (p1+ori1), s1);
        memcpy((void *) &pout[out2], (void *) (p2+alt1), s2);
    }
 
    this->synchronize();
    return *this;
}
 
const Sound& Sound::operator = (const Sound& alt)
{
    yCAssert(SOUND, getBytesPerSample()==alt.getBytesPerSample());
    FlexImage& img1 = HELPER(implementation);
    FlexImage& img2 = HELPER(alt.implementation);
    img1.copy(img2);
    m_frequency = alt.m_frequency;
    synchronize();
    return *this;
}
 
void Sound::synchronize()
{
    FlexImage& img = HELPER(implementation);
    m_samples = img.width();
    m_channels = img.height();
}
 
Sound Sound::subSound(size_t first_sample, size_t last_sample)
{
    if (last_sample  > this->m_samples)
        last_sample = m_samples;
    if (first_sample > this->m_samples)
        first_sample = m_samples;
    if (last_sample < first_sample)
        last_sample = first_sample;
 
    Sound s;
 
    s.resize(last_sample-first_sample, this->m_channels);
    s.setFrequency(this->m_frequency);
 
    /*
    //faster implementation but currently not working
    unsigned char* p1    = this->getRawData();
    unsigned char* p2    = s.getRawData();
    int j=0;
    for (int i=first_sample; i<last_sample*2; i++)
    {
        p2[j++]=p1[i];
    }
    */
 
    //safe implementation
    size_t j=0;
    for (size_t i=first_sample; i<last_sample; i++)
    {
        for (size_t c=0; c< this->m_channels; c++)
            s.set(this->get(i,c),j,c);
        j++;
    }
 
    s.synchronize();
 
    return s;
}
 
void Sound::init(size_t bytesPerSample)
{
    implementation = new FlexImage();
    yCAssert(SOUND, implementation!=nullptr);
 
    yCAssert(SOUND, bytesPerSample==2); // that's all that's implemented right now
    HELPER(implementation).setPixelCode(VOCAB_PIXEL_MONO16);
    HELPER(implementation).setQuantum(2);
 
    m_samples = 0;
    m_channels = 0;
    this->m_bytesPerSample = bytesPerSample;
}
 
Sound::~Sound()
{
    if (implementation!=nullptr)
    {
        delete &HELPER(implementation);
        implementation = nullptr;
    }
}
 
void Sound::resize(size_t samples, size_t m_channels)
{
    FlexImage& img = HELPER(implementation);
    img.resize(samples,m_channels);
    synchronize();
}
 
Sound::audio_sample Sound::get(size_t location, size_t channel) const
{
    FlexImage& img = HELPER(implementation);
    unsigned char *addr = img.getPixelAddress(location,channel);
    if (m_bytesPerSample ==2)
    {
        return *(reinterpret_cast<NetUint16*>(addr));
    }
    else
    {
        yCError(SOUND, "sound only implemented for 16 bit samples");
    }
    return 0;
}
 
void Sound::clear()
{
    size_t size = this->getRawDataSize();
    unsigned char* p  = this->getRawData();
    memset(p,0,size);
}
 
bool Sound::clearChannel(size_t chan)
{
    if (chan > this->m_channels) return false;
    for (size_t i = 0; i < this->m_samples; i++)
    {
        set(0, i, chan);
    }
    return true;
}
 
void Sound::set(audio_sample value, size_t location, size_t channel)
{
    FlexImage& img = HELPER(implementation);
    unsigned char *addr = img.getPixelAddress(location,channel);
    if (m_bytesPerSample ==2)
    {
        *(reinterpret_cast<NetUint16*>(addr)) = value;
        return;
    }
    else
    {
        yCError(SOUND, "sound only implemented for 16 bit samples");
    }
}
 
int Sound::getFrequency() const
{
    return m_frequency;
}
 
void Sound::setFrequency(int freq)
{
    this->m_frequency = freq;
}
 
bool Sound::read(ConnectionReader& connection)
{
    // lousy format - fix soon!
    FlexImage& img = HELPER(implementation);
    Bottle bot;
    bool ok = PortablePair<FlexImage,Bottle>::readPair(connection,img,bot);
    m_frequency = bot.get(0).asInt32();
    synchronize();
    return ok;
}
 
 
bool Sound::write(ConnectionWriter& connection) const
{
    // lousy format - fix soon!
    FlexImage& img = HELPER(implementation);
    Bottle bot;
    bot.addInt32(m_frequency);
    return PortablePair<FlexImage,Bottle>::writePair(connection,img,bot);
}
 
unsigned char *Sound::getRawData() const
{
    FlexImage& img = HELPER(implementation);
    return img.getRawImage();
}
 
size_t Sound::getRawDataSize() const
{
    FlexImage& img = HELPER(implementation);
    return img.getRawImageSize();
}
 
void Sound::setSafe(audio_sample value, size_t sample, size_t channel)
{
    if (isSample(sample, channel))
    {
        set(value, sample, channel);
    }
    else
    {
        yCError(SOUND) << "Sample out of bound:" << sample << "," << channel;
    }
}
 
Sound Sound::extractChannelAsSound(size_t channel_id) const
{
    Sound news(this->m_bytesPerSample);
    news.setFrequency(this->m_frequency);
    news.resize(this->m_samples, 1);
 
    unsigned char* p_src = this->getRawData();
    unsigned char* p_dst = news.getRawData();
    size_t j = 0;
    //pointer to the first element of the row of the matrix we need to copy
    size_t first_sample = 0 + (this->m_samples * this->m_bytesPerSample)*channel_id;
    //pointer to the last element of the row of the matrix we need to copy
    size_t last_sample = first_sample + (this->m_samples * this->m_bytesPerSample);
    for (auto i = first_sample; i < last_sample; i++)
    {
        p_dst[j++] = p_src[i];
    }
    return news;
}
 
bool Sound::operator==(const Sound& alt) const
{
    if (this->m_channels != alt.getChannels()) return false;
    if (this->m_bytesPerSample != alt.getBytesPerSample()) return false;
    if (this->m_frequency != alt.getFrequency()) return false;
    if (this->m_samples != alt.getSamples()) return false;
 
    for (size_t ch = 0; ch < this->m_channels; ch++)
    {
        for (size_t s = 0; s < this->m_samples; s++)
        {
            if (this->getSafe(s, ch) != alt.getSafe(s, ch))
            {
                return false;
            }
        }
    }
 
    return true;
}
 
bool Sound::replaceChannel(size_t id, Sound schannel)
{
    if (schannel.getChannels() != 1) return false;
    if (this->m_samples != schannel.getSamples()) return false;
    for (size_t s = 0; s < this->m_samples; s++)
    {
        this->setSafe(schannel.getSafe(s, 0), s, id);
    }
    return true;
}
 
std::vector<std::reference_wrapper<Sound::audio_sample>> Sound::getChannel(size_t channel_id)
{
    FlexImage& img = HELPER(implementation);
 
    std::vector<std::reference_wrapper<audio_sample>> vec;
    vec.reserve(this->m_samples);
    for (size_t t = 0; t < this->m_samples; t++)
    {
        unsigned char *addr = img.getPixelAddress(t, channel_id);
        audio_sample*  addr2 = reinterpret_cast<audio_sample*>(addr);
        vec.push_back(std::ref(*addr2));
    }
    return vec;
}
 
std::vector<std::reference_wrapper<Sound::audio_sample>> Sound::getInterleavedAudioRawData() const
{
    FlexImage& img = HELPER(implementation);
 
    std::vector<std::reference_wrapper<audio_sample>> vec;
    vec.reserve(this->m_samples*this->m_channels);
    for (size_t t = 0; t < this->m_samples; t++)
    {
        for (size_t c = 0; c < this->m_channels; c++)
        {
            unsigned char *addr  = img.getPixelAddress(t, c);
            audio_sample*  addr2 = reinterpret_cast<audio_sample*>(addr);
            vec.push_back(std::ref(*addr2));
        }
    }
    return vec;
}
 
std::vector<std::reference_wrapper<Sound::audio_sample>> Sound::getNonInterleavedAudioRawData() const
{
    FlexImage& img = HELPER(implementation);
 
    std::vector<std::reference_wrapper<audio_sample>> vec;
    vec.reserve(this->m_samples*this->m_channels);
    for (size_t c = 0; c < this->m_channels; c++)
    {
        for (size_t t = 0; t < this->m_samples; t++)
        {
            unsigned char *addr = img.getPixelAddress(t, c);
            audio_sample*  addr2 = reinterpret_cast<audio_sample*>(addr);
            vec.push_back(std::ref(*addr2));
        }
    }
    return vec;
}
 
std::string Sound::toString() const
{
    std::string s;
    for (size_t c = 0; c < this->m_channels; c++)
    {
        for (size_t t = 0; t < this->m_samples; t++)
        {
            s += " ";
            s += std::to_string(this->get(t, c));
        }
        s += '\n';
    }
    return s;
}
 
bool Sound::isSample(size_t sample, size_t channel) const
{
    return (sample<this->m_samples && channel<this->m_channels);
}
 
size_t Sound::getBytesPerSample() const
{
    return this->m_bytesPerSample;
}
 
size_t Sound::getSamples() const
{
    return this->m_samples;
}
 
size_t Sound::getChannels() const
{
    return this->m_channels;
}
 
double Sound::getDuration() const
{
    return (double)(this->m_samples)*(double)(1 / this->m_frequency);
}