<commit_before>/*
Copyright 2011-2017 Frederic Langlet
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 <algorithm>
#include <vector>
#include <sstream>
#include "../IllegalArgumentException.hpp"
#include "EntropyUtils.hpp"

using namespace kanzi;

class FreqSortData {
public:
    uint _symbol;
    int* _errors;
    uint* _frequencies;

    FreqSortData(int errors[], uint frequencies[], int symbol)
    {
        _errors = errors;
        _frequencies = frequencies;
        _symbol = symbol;
    }
};

struct FreqDataComparator {
    bool operator()(FreqSortData* fd1, FreqSortData* fd2) const
    {
        int res = fd1->_errors[fd1->_symbol] - fd2->_errors[fd2->_symbol];

        // Increasing frequency
        if (res == 0) {
            res = fd1->_frequencies[fd1->_symbol] - fd2->_frequencies[fd2->_symbol];

            // Increasing symbol
            if (res == 0)
                return fd1->_symbol < fd2->_symbol;
        }

        return res < 0;
    }
};

// alphabet must be sorted in increasing order
// length = alphabet array length
int EntropyUtils::encodeAlphabet(OutputBitStream& obs, uint alphabet[], int length, int count)
{
    // Alphabet length must be a power of 2
    if ((length & (length - 1)) != 0)
        return -1;

    if ((count > length) || (count >= 256))
        return -1;

    // First, push alphabet encoding mode
    if ((length > 0) && (count == length)) {
        // Fuint64 alphabet
        obs.writeBit(FULL_ALPHABET);

        if (count == 256)
            obs.writeBit(ALPHABET_256); // shortcut
        else {
            int log = 1;

            while (1 << log <= count)
                log++;

            // Write alphabet size
            obs.writeBit(ALPHABET_NOT_256);
            obs.writeBits(log - 1, 5);
            obs.writeBits(count, log);
        }

        return count;
    }

    obs.writeBit(PARTIAL_ALPHABET);

    if ((length == 256) && (count >= 32) && (count <= 224)) {
        // Regular alphabet of symbols less than 256
        obs.writeBit(BIT_ENCODED_ALPHABET_256);
		uint64 masks[4] = { 0, 0, 0, 0 };

		for (int i = 0; i < count; i++)
			masks[alphabet[i] >> 6] |= ((uint64(1)) << (alphabet[i] & 63));

        for (int i = 0; i < 4; i++)
            obs.writeBits(masks[i], 64);

        return count;
    }

    obs.writeBit(DELTA_ENCODED_ALPHABET);
    int diffs[256];

    if (length - count < count) {
        // Encode all missing symbols
        count = length - count;
        int log = 1;

        while (1 << log <= count)
            log++;

        // Write length
        obs.writeBits(log - 1, 4);
        obs.writeBits(count, log);

        if (count == 0)
            return 0;

        obs.writeBit(ABSENT_SYMBOLS_MASK);
        log = 1;

        while (1 << log <= length)
            log++;

        // Write log(alphabet size)
        obs.writeBits(log - 1, 5);
        uint symbol = 0;
        int previous = 0;

        // Create deltas of missing symbols
        for (int n = 0, i = 0; n < count;) {
            if (symbol == alphabet[i]) {
                if (i < length - 1 - count)
                    i++;

                symbol++;
                continue;
            }

            diffs[n] = symbol - previous;
            symbol++;
            previous = symbol;
            n++;
        }
    }
    else {
        // Encode all present symbols
        int log = 1;

        while (1 << log <= count)
            log++;

        // Write length
        obs.writeBits(log - 1, 4);
        obs.writeBits(count, log);

        if (count == 0)
            return 0;

        obs.writeBit(PRESENT_SYMBOLS_MASK);
        int previous = 0;

        // Create deltas of present symbols
        for (int i = 0; i < count; i++) {
            diffs[i] = alphabet[i] - previous;
            previous = alphabet[i] + 1;
        }
    }

    const int ckSize = (count <= 64) ? 8 : 16;

    // Encode all deltas by chunks
    for (int i = 0; i < count; i += ckSize) {
        int max = 0;

        // Find log(max(deltas)) for this chunk
        for (int j = i; (j < count) && (j < i + ckSize); j++) {
            if (max < diffs[j])
                max = diffs[j];
        }

        int log = 1;

        while (1 << log <= max)
            log++;

        obs.writeBits(log - 1, 4);

        // Write deltas for this chunk
        for (int j = i; (j < count) && (j < i + ckSize); j++)
            encodeSize(obs, log, diffs[j]);
    }

    return count;
}

void EntropyUtils::encodeSize(OutputBitStream& obs, int log, int val)
{
    obs.writeBits(val, log);
}

uint64 EntropyUtils::decodeSize(InputBitStream& ibs, int log)
{
    return ibs.readBits(log);
}

int EntropyUtils::decodeAlphabet(InputBitStream& ibs, uint alphabet[]) THROW
{
    // Read encoding mode from bitstream
    const int alphabetType = ibs.readBit();

    if (alphabetType == FULL_ALPHABET) {
        int alphabetSize;

        if (ibs.readBit() == ALPHABET_256)
            alphabetSize = 256;
        else {
            int log = 1 + int(ibs.readBits(5));
            alphabetSize = int(ibs.readBits(log));
        }

        if (alphabetSize > 256) {
            stringstream ss;
            ss << "Invalid bitstream: incorrect alphabet size: " << alphabetSize;
            throw BitStreamException(ss.str(), BitStreamException::INVALID_STREAM);
        }

        // Full alphabet
        for (int i = 0; i < alphabetSize; i++)
            alphabet[i] = i;

        return alphabetSize;
    }

    int count = 0;
    const int mode = ibs.readBit();

    if (mode == BIT_ENCODED_ALPHABET_256) {
        // Decode presence flags
        for (int i = 0; i < 256; i += 64) {
            const uint64 val = ibs.readBits(64);

            for (int j = 0; j < 64; j++) {
                if ((val & ((uint64(1)) << j)) != 0) {
                    alphabet[count] = i + j;
                    count++;
                }
            }
        }

        return count;
    }

    // DELTA_ENCODED_ALPHABET
    int log = 1 + int(ibs.readBits(4));
    count = int(ibs.readBits(log));

    if (count == 0)
        return 0;

    const int ckSize = (count <= 64) ? 8 : 16;
    int n = 0;
    int symbol = 0;

    if (ibs.readBit() == ABSENT_SYMBOLS_MASK) {
        int alphabetSize = 1 << int(ibs.readBits(5));

        // Read missing symbols
        for (int i = 0; i < count; i += ckSize) {
            log = 1 + int(ibs.readBits(4));

            // Read deltas for this chunk
            for (int j = i; (j < count) && (j < i + ckSize); j++) {
                const int next = symbol + int(decodeSize(ibs, log));

                while ((symbol < next) && (n < alphabetSize)) {
                    alphabet[n] = symbol++;
                    n++;
                }

                symbol++;
            }
        }

        count = alphabetSize - count;

        while (n < count)
            alphabet[n++] = symbol++;
    }
    else {
        // Read present symbols
        for (int i = 0; i < count; i += ckSize) {
            log = 1 + int(ibs.readBits(4));

            // Read deltas for this chunk
            for (int j = i; (j < count) && (j < i + ckSize); j++) {
                symbol += int(decodeSize(ibs, log));
                alphabet[j] = symbol;
                symbol++;
            }
        }
    }

    return count;
}

// Returns the size of the alphabet
// length is the length of the alphabet array
// 'totalFreq 'is the sum of frequencies.
// 'scale' is the target new total of frequencies
// The alphabet and freqs parameters are updated
int EntropyUtils::normalizeFrequencies(uint freqs[], uint alphabet[], int length, uint totalFreq, uint scale) THROW
{
    if (length > 256) {
        stringstream ss;
        ss << "Invalid alphabet size parameter: " << scale << " (must be less than or equal to 256)";
        throw IllegalArgumentException(ss.str());
    }

    if ((scale < 1 << 8) || (scale > 1 << 16)) {
        stringstream ss;
        ss << "Invalid scale parameter: " << scale << " (must be in [256..65536])";
        throw IllegalArgumentException(ss.str());
    }

    if ((length == 0) || (totalFreq == 0))
        return 0;

    // Number of present symbols
    int alphabetSize = 0;

    // shortcut
    if (totalFreq == scale) {
        for (int i = 0; i < 256; i++) {
            if (freqs[i] != 0)
                alphabet[alphabetSize++] = i;
        }

        return alphabetSize;
    }

    uint sumScaledFreq = 0;
    uint sumFreq = 0;
    uint freqMax = 0;
    int idxMax = -1;

    // Scale frequencies by stretching distribution over complete range
    for (int i = 0; (i < length) && (sumFreq < totalFreq); i++) {
        alphabet[i] = 0;
        _errors[i] = 0;
        const uint f = freqs[i];

        if (f == 0)
            continue;

        if (f > freqMax) {
            freqMax = f;
            idxMax = i;
        }

        sumFreq += f;
        int64 sf = int64(f) * scale;
        uint scaledFreq;

        if (sf <= int64(totalFreq)) {
            // Quantum of frequency
            scaledFreq = 1;
        }
        else {
            // Find best frequency rounding value
            scaledFreq = uint(sf / totalFreq);
            int64 errCeiling = (scaledFreq + 1) * int64(totalFreq) - sf;
            int64 errFloor = sf - scaledFreq * int64(totalFreq);

            if (errCeiling < errFloor) {
                scaledFreq++;
                _errors[i] = int(errCeiling);
            }
            else {
                _errors[i] = int(errFloor);
            }
        }

        alphabet[alphabetSize++] = i;
        sumScaledFreq += scaledFreq;
        freqs[i] = scaledFreq;
    }

    if (alphabetSize == 0)
        return 0;

    if (alphabetSize == 1) {
        freqs[alphabet[0]] = scale;
        return 1;
    }

    if (sumScaledFreq != scale) {
        if (int(freqs[idxMax]) > int(sumScaledFreq - scale)) {
            // Fast path: just adjust the max frequency
            freqs[idxMax] += int(scale - sumScaledFreq);
        }
        else {
            // Slow path: spread error across frequencies
            const int inc = (sumScaledFreq > scale) ? -1 : 1;
            vector<FreqSortData*> queue;

            // Create sorted queue of present symbols (except those with 'quantum frequency')
            for (int i = 0; i < alphabetSize; i++) {
                if ((_errors[alphabet[i]] > 0) && (int(freqs[alphabet[i]]) != -inc)) {
                    queue.push_back(new FreqSortData(_errors, freqs, alphabet[i]));
                }
            }

            make_heap(queue.begin(), queue.end(), FreqDataComparator());

            while ((sumScaledFreq != scale) && (queue.size() > 0)) {
                // Remove symbol with highest error
                pop_heap(queue.begin(), queue.end(), FreqDataComparator());
                FreqSortData* fsd = queue.back();
                queue.pop_back();

                // Do not zero out any frequency
                if (int(freqs[fsd->_symbol]) == -inc) {
                    delete fsd;
                    continue;
                }

                // Distort frequency and error
                freqs[fsd->_symbol] += inc;
                _errors[fsd->_symbol] -= scale;
                sumScaledFreq += inc;
                queue.push_back(fsd);
                push_heap(queue.begin(), queue.end(), FreqDataComparator());
            }

            while (queue.size() > 0) {
                FreqSortData* fsd = queue.back();
                queue.pop_back();
                delete fsd;
            }
        }
    }

    return alphabetSize;
}
<commit_msg>Formatting<commit_after>/*
Copyright 2011-2017 Frederic Langlet
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 <algorithm>
#include <vector>
#include <sstream>
#include "../IllegalArgumentException.hpp"
#include "EntropyUtils.hpp"

using namespace kanzi;

class FreqSortData {
public:
    uint _symbol;
    int* _errors;
    uint* _frequencies;

    FreqSortData(int errors[], uint frequencies[], int symbol)
    {
        _errors = errors;
        _frequencies = frequencies;
        _symbol = symbol;
    }
};

struct FreqDataComparator {
    bool operator()(FreqSortData* fd1, FreqSortData* fd2) const
    {
        int res = fd1->_errors[fd1->_symbol] - fd2->_errors[fd2->_symbol];

        // Increasing frequency
        if (res == 0) {
            res = fd1->_frequencies[fd1->_symbol] - fd2->_frequencies[fd2->_symbol];

            // Increasing symbol
            if (res == 0)
                return fd1->_symbol < fd2->_symbol;
        }

        return res < 0;
    }
};

// alphabet must be sorted in increasing order
// length = alphabet array length
int EntropyUtils::encodeAlphabet(OutputBitStream& obs, uint alphabet[], int length, int count)
{
    // Alphabet length must be a power of 2
    if ((length & (length - 1)) != 0)
        return -1;

    if ((count > length) || (count >= 256))
        return -1;

    // First, push alphabet encoding mode
    if ((length > 0) && (count == length)) {
        // Fuint64 alphabet
        obs.writeBit(FULL_ALPHABET);

        if (count == 256)
            obs.writeBit(ALPHABET_256); // shortcut
        else {
            int log = 1;

            while (1 << log <= count)
                log++;

            // Write alphabet size
            obs.writeBit(ALPHABET_NOT_256);
            obs.writeBits(log - 1, 5);
            obs.writeBits(count, log);
        }

        return count;
    }

    obs.writeBit(PARTIAL_ALPHABET);

    if ((length == 256) && (count >= 32) && (count <= 224)) {
        // Regular alphabet of symbols less than 256
        obs.writeBit(BIT_ENCODED_ALPHABET_256);
        uint64 masks[4] = { 0, 0, 0, 0 };

        for (int i = 0; i < count; i++)
            masks[alphabet[i] >> 6] |= ((uint64(1)) << (alphabet[i] & 63));

        for (int i = 0; i < 4; i++)
            obs.writeBits(masks[i], 64);

        return count;
    }

    obs.writeBit(DELTA_ENCODED_ALPHABET);
    int diffs[256];

    if (length - count < count) {
        // Encode all missing symbols
        count = length - count;
        int log = 1;

        while (1 << log <= count)
            log++;

        // Write length
        obs.writeBits(log - 1, 4);
        obs.writeBits(count, log);

        if (count == 0)
            return 0;

        obs.writeBit(ABSENT_SYMBOLS_MASK);
        log = 1;

        while (1 << log <= length)
            log++;

        // Write log(alphabet size)
        obs.writeBits(log - 1, 5);
        uint symbol = 0;
        int previous = 0;

        // Create deltas of missing symbols
        for (int n = 0, i = 0; n < count;) {
            if (symbol == alphabet[i]) {
                if (i < length - 1 - count)
                    i++;

                symbol++;
                continue;
            }

            diffs[n] = symbol - previous;
            symbol++;
            previous = symbol;
            n++;
        }
    }
    else {
        // Encode all present symbols
        int log = 1;

        while (1 << log <= count)
            log++;

        // Write length
        obs.writeBits(log - 1, 4);
        obs.writeBits(count, log);

        if (count == 0)
            return 0;

        obs.writeBit(PRESENT_SYMBOLS_MASK);
        int previous = 0;

        // Create deltas of present symbols
        for (int i = 0; i < count; i++) {
            diffs[i] = alphabet[i] - previous;
            previous = alphabet[i] + 1;
        }
    }

    const int ckSize = (count <= 64) ? 8 : 16;

    // Encode all deltas by chunks
    for (int i = 0; i < count; i += ckSize) {
        int max = 0;

        // Find log(max(deltas)) for this chunk
        for (int j = i; (j < count) && (j < i + ckSize); j++) {
            if (max < diffs[j])
                max = diffs[j];
        }

        int log = 1;

        while (1 << log <= max)
            log++;

        obs.writeBits(log - 1, 4);

        // Write deltas for this chunk
        for (int j = i; (j < count) && (j < i + ckSize); j++)
            encodeSize(obs, log, diffs[j]);
    }

    return count;
}

void EntropyUtils::encodeSize(OutputBitStream& obs, int log, int val)
{
    obs.writeBits(val, log);
}

uint64 EntropyUtils::decodeSize(InputBitStream& ibs, int log)
{
    return ibs.readBits(log);
}

int EntropyUtils::decodeAlphabet(InputBitStream& ibs, uint alphabet[]) THROW
{
    // Read encoding mode from bitstream
    const int alphabetType = ibs.readBit();

    if (alphabetType == FULL_ALPHABET) {
        int alphabetSize;

        if (ibs.readBit() == ALPHABET_256)
            alphabetSize = 256;
        else {
            int log = 1 + int(ibs.readBits(5));
            alphabetSize = int(ibs.readBits(log));
        }

        if (alphabetSize > 256) {
            stringstream ss;
            ss << "Invalid bitstream: incorrect alphabet size: " << alphabetSize;
            throw BitStreamException(ss.str(), BitStreamException::INVALID_STREAM);
        }

        // Full alphabet
        for (int i = 0; i < alphabetSize; i++)
            alphabet[i] = i;

        return alphabetSize;
    }

    int count = 0;
    const int mode = ibs.readBit();

    if (mode == BIT_ENCODED_ALPHABET_256) {
        // Decode presence flags
        for (int i = 0; i < 256; i += 64) {
            const uint64 val = ibs.readBits(64);

            for (int j = 0; j < 64; j++) {
                if ((val & ((uint64(1)) << j)) != 0) {
                    alphabet[count] = i + j;
                    count++;
                }
            }
        }

        return count;
    }

    // DELTA_ENCODED_ALPHABET
    int log = 1 + int(ibs.readBits(4));
    count = int(ibs.readBits(log));

    if (count == 0)
        return 0;

    const int ckSize = (count <= 64) ? 8 : 16;
    int n = 0;
    int symbol = 0;

    if (ibs.readBit() == ABSENT_SYMBOLS_MASK) {
        int alphabetSize = 1 << int(ibs.readBits(5));

        // Read missing symbols
        for (int i = 0; i < count; i += ckSize) {
            log = 1 + int(ibs.readBits(4));

            // Read deltas for this chunk
            for (int j = i; (j < count) && (j < i + ckSize); j++) {
                const int next = symbol + int(decodeSize(ibs, log));

                while ((symbol < next) && (n < alphabetSize)) {
                    alphabet[n] = symbol++;
                    n++;
                }

                symbol++;
            }
        }

        count = alphabetSize - count;

        while (n < count)
            alphabet[n++] = symbol++;
    }
    else {
        // Read present symbols
        for (int i = 0; i < count; i += ckSize) {
            log = 1 + int(ibs.readBits(4));

            // Read deltas for this chunk
            for (int j = i; (j < count) && (j < i + ckSize); j++) {
                symbol += int(decodeSize(ibs, log));
                alphabet[j] = symbol;
                symbol++;
            }
        }
    }

    return count;
}

// Returns the size of the alphabet
// length is the length of the alphabet array
// 'totalFreq 'is the sum of frequencies.
// 'scale' is the target new total of frequencies
// The alphabet and freqs parameters are updated
int EntropyUtils::normalizeFrequencies(uint freqs[], uint alphabet[], int length, uint totalFreq, uint scale) THROW
{
    if (length > 256) {
        stringstream ss;
        ss << "Invalid alphabet size parameter: " << scale << " (must be less than or equal to 256)";
        throw IllegalArgumentException(ss.str());
    }

    if ((scale < 1 << 8) || (scale > 1 << 16)) {
        stringstream ss;
        ss << "Invalid scale parameter: " << scale << " (must be in [256..65536])";
        throw IllegalArgumentException(ss.str());
    }

    if ((length == 0) || (totalFreq == 0))
        return 0;

    // Number of present symbols
    int alphabetSize = 0;

    // shortcut
    if (totalFreq == scale) {
        for (int i = 0; i < 256; i++) {
            if (freqs[i] != 0)
                alphabet[alphabetSize++] = i;
        }

        return alphabetSize;
    }

    uint sumScaledFreq = 0;
    uint sumFreq = 0;
    uint freqMax = 0;
    int idxMax = -1;

    // Scale frequencies by stretching distribution over complete range
    for (int i = 0; (i < length) && (sumFreq < totalFreq); i++) {
        alphabet[i] = 0;
        _errors[i] = 0;
        const uint f = freqs[i];

        if (f == 0)
            continue;

        if (f > freqMax) {
            freqMax = f;
            idxMax = i;
        }

        sumFreq += f;
        int64 sf = int64(f) * scale;
        uint scaledFreq;

        if (sf <= int64(totalFreq)) {
            // Quantum of frequency
            scaledFreq = 1;
        }
        else {
            // Find best frequency rounding value
            scaledFreq = uint(sf / totalFreq);
            int64 errCeiling = (scaledFreq + 1) * int64(totalFreq) - sf;
            int64 errFloor = sf - scaledFreq * int64(totalFreq);

            if (errCeiling < errFloor) {
                scaledFreq++;
                _errors[i] = int(errCeiling);
            }
            else {
                _errors[i] = int(errFloor);
            }
        }

        alphabet[alphabetSize++] = i;
        sumScaledFreq += scaledFreq;
        freqs[i] = scaledFreq;
    }

    if (alphabetSize == 0)
        return 0;

    if (alphabetSize == 1) {
        freqs[alphabet[0]] = scale;
        return 1;
    }

    if (sumScaledFreq != scale) {
        if (int(freqs[idxMax]) > int(sumScaledFreq - scale)) {
            // Fast path: just adjust the max frequency
            freqs[idxMax] += int(scale - sumScaledFreq);
        }
        else {
            // Slow path: spread error across frequencies
            const int inc = (sumScaledFreq > scale) ? -1 : 1;
            vector<FreqSortData*> queue;

            // Create sorted queue of present symbols (except those with 'quantum frequency')
            for (int i = 0; i < alphabetSize; i++) {
                if ((_errors[alphabet[i]] > 0) && (int(freqs[alphabet[i]]) != -inc)) {
                    queue.push_back(new FreqSortData(_errors, freqs, alphabet[i]));
                }
            }

            make_heap(queue.begin(), queue.end(), FreqDataComparator());

            while ((sumScaledFreq != scale) && (queue.size() > 0)) {
                // Remove symbol with highest error
                pop_heap(queue.begin(), queue.end(), FreqDataComparator());
                FreqSortData* fsd = queue.back();
                queue.pop_back();

                // Do not zero out any frequency
                if (int(freqs[fsd->_symbol]) == -inc) {
                    delete fsd;
                    continue;
                }

                // Distort frequency and error
                freqs[fsd->_symbol] += inc;
                _errors[fsd->_symbol] -= scale;
                sumScaledFreq += inc;
                queue.push_back(fsd);
                push_heap(queue.begin(), queue.end(), FreqDataComparator());
            }

            while (queue.size() > 0) {
                FreqSortData* fsd = queue.back();
                queue.pop_back();
                delete fsd;
            }
        }
    }

    return alphabetSize;
}
<|endoftext|>


<commit_before><commit_msg>Added panning via mouse movement<commit_after><|endoftext|>


<commit_before>//Copyright (c) 2018 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include "InterlockingGenerator.h"

#include <algorithm> // max

#include "Application.h"
#include "Slice.h"
#include "slicer.h"
#include "utils/polygonUtils.h"
#include "utils/VoxelUtils.h"

namespace cura
{

void InterlockingGenerator::generateInterlockingStructure(std::vector<Slicer*>& volumes)
{
    /*
    if ( ! Application::getInstance().current_slice->scene.current_mesh_group->settings.get<bool>("interlocking_structure_gen"))
    {
        return;
    }
    */

    const size_t extruder_count = Application::getInstance().current_slice->scene.extruders.size();
    if (extruder_count > 2)
    {
        // TODO: fix for > 2 extruders (apply first to extruder 0 & 1, then to 0 & 2, etc.)
        logError("generateInterlockingStructure not implemented for >2 extruders!\n");
        return;
    }
    std::vector<int> meshes_per_extruder(extruder_count);
    for (Slicer* mesh : volumes)
    {
        size_t extruder_nr = mesh->mesh->settings.get<ExtruderTrain&>("wall_0_extruder_nr").settings.get<size_t>("extruder_nr");
        meshes_per_extruder[extruder_nr]++;
        if (meshes_per_extruder[extruder_nr] > 1)
        {
            logError("Currently too buggy to operate on multiple models! Each voxel needs to be associated to 2 meshes.");
            std::exit(-1);
        }
    }

    const std::vector<ExtruderTrain>& extruders = Application::getInstance().current_slice->scene.extruders;
    std::vector<coord_t> line_width_per_extruder(extruder_count);
    for (size_t extruder_nr = 0; extruder_nr < extruder_count; extruder_nr++)
    {
        line_width_per_extruder[extruder_nr] = extruders[extruder_nr].settings.get<coord_t>("wall_line_width_0");
    }
    float bulging_angle = 10.0 / 180 * M_PI;
    coord_t cell_width;
    if (bulging_angle == 0.0)
    {
        cell_width = (line_width_per_extruder[0] + line_width_per_extruder[1]) * 2;
    }
    else
    {
        cell_width = (line_width_per_extruder[0] + line_width_per_extruder[1]) * (1.0 + 1.0 / cos(bulging_angle)) / (1.0 - tan(bulging_angle));
    }
    Point3 cell_size(cell_width, cell_width, 4 * Application::getInstance().current_slice->scene.settings.get<coord_t>("layer_height"));
    // TODO make robust against if there's only 1 extruder

    std::vector<coord_t> layer_heights;
    {
        size_t layer_idx = 0;
        for (Slicer* mesh : volumes)
        {
            layer_heights.resize(std::max(layer_heights.size(), mesh->layers.size()));
            for ( ; layer_idx < mesh->layers.size() ; layer_idx++)
            {
                layer_heights[layer_idx] = mesh->layers[layer_idx].z;
            }
        }
    }

    PointMatrix rotation(45.0);

    InterlockingGenerator gen(volumes, line_width_per_extruder, layer_heights, rotation, cell_size, bulging_angle);

    // TODO: implement lesser dilation based on translated polygons
    // TODO: make dilation user parameter

    // TODO: adjust voxel height to 2x layer height? or 4x? make adjustable?

    // TODO: implement oscillating fingers

    // TODO: option for different amount of dilation for shell removal

    // TODO: dont add all patterns to each mesh. This leads to each mesh printing the interfaces of all other meshes.

    DilationKernel interface_dilation(GridPoint3(2,2,4), true);
    std::vector<std::unordered_set<GridPoint3>> voxels_per_extruder = gen.getShellVoxels(interface_dilation);

    std::vector<Polygons> layer_regions(layer_heights.size());
    gen.computeLayerRegions(layer_regions);

    std::unordered_set<GridPoint3>& has_any_extruder = voxels_per_extruder[0];
    std::unordered_set<GridPoint3>& has_all_extruders = voxels_per_extruder[1];
    has_any_extruder.merge(has_all_extruders);

    constexpr bool air_filtering = false;
    if (air_filtering)
    {
        DilationKernel air_dilation(GridPoint3(1,1,1), true);
        std::unordered_set<GridPoint3> air_cells;
        gen.addBoundaryCells(layer_regions, air_dilation, air_cells);

        for (const GridPoint3& p : air_cells)
        {
            has_all_extruders.erase(p);
        }
    }

    std::vector<std::vector<Polygons>> cell_area_per_extruder_per_layer;
    const bool alternating_offset = bulging_angle > 0.0;
    gen.generateMicrostructure(cell_area_per_extruder_per_layer, alternating_offset);

    gen.applyMicrostructureToOutlines(has_all_extruders, cell_area_per_extruder_per_layer, layer_regions);
}

InterlockingGenerator::Cell::Cell()
: has_extruder(Application::getInstance().current_slice->scene.extruders.size(), false)
{
    assert(has_extruder.size() >= 1);
}

std::vector<std::unordered_set<GridPoint3>> InterlockingGenerator::getShellVoxels(const DilationKernel& kernel)
{
    std::vector<std::unordered_set<GridPoint3>> voxels_per_extruder(2);

    // mark all cells which contain some boundary
    for (Slicer* mesh : volumes)
    {
        size_t extruder_nr = mesh->mesh->settings.get<ExtruderTrain&>("wall_0_extruder_nr").settings.get<size_t>("extruder_nr");
        assert(extruder_nr < 2);
        std::unordered_set<GridPoint3>& mesh_voxels = voxels_per_extruder[extruder_nr];

        std::vector<Polygons> rotated_polygons_per_layer(mesh->layers.size());
        for (size_t layer_nr = 0; layer_nr < mesh->layers.size(); layer_nr++)
        {
            SlicerLayer& layer = mesh->layers[layer_nr];
            rotated_polygons_per_layer[layer_nr] = layer.polygons;
            rotated_polygons_per_layer[layer_nr].applyMatrix(rotation);
        }

        addBoundaryCells(rotated_polygons_per_layer, kernel, mesh_voxels);
    }

    return voxels_per_extruder;
}

void InterlockingGenerator::addBoundaryCells(std::vector<Polygons>& layers, const DilationKernel& kernel, std::unordered_set<GridPoint3>& cells)
{
    auto voxel_emplacer = [&cells](GridPoint3 p) { cells.emplace(p); return true; };

    for (size_t layer_nr = 0; layer_nr < layers.size(); layer_nr++)
    {
        coord_t z = layer_heights[layer_nr];
        vu.walkDilatedPolygons(layers[layer_nr], z, kernel, voxel_emplacer);
        Polygons skin = layers[layer_nr];
        if (layer_nr > 0)
        {
            skin = skin.xorPolygons(layers[layer_nr - 1]);
        }
        skin = skin.offset(-cell_size.x / 2).offset(cell_size.x / 2); // remove superfluous small areas, which would anyway be included because of walkPolygons
        vu.walkDilatedAreas(skin, z, kernel, voxel_emplacer);
    }
}

void InterlockingGenerator::computeLayerRegions(std::vector<Polygons>& layer_regions)
{
    for (unsigned int layer_nr = 0; layer_nr < layer_regions.size(); layer_nr++)
    {
        Polygons& layer_region = layer_regions[layer_nr];
        coord_t z;
        for (Slicer* mesh : volumes)
        {
            if (layer_nr >= mesh->layers.size()) break;
            SlicerLayer& layer = mesh->layers[layer_nr];
            z = layer.z;
            layer_region.add(layer.polygons);
        }
        layer_region = layer_region.offset(100).offset(-100); // TODO hardcoded value
        layer_region.applyMatrix(rotation);
        layer_heights[layer_nr] = z;
    }
}

void InterlockingGenerator::generateMicrostructure(std::vector<std::vector<Polygons>>& cell_area_per_extruder_per_layer, bool alternating_offset)
{
    cell_area_per_extruder_per_layer.resize(alternating_offset? 4 : 2);
    cell_area_per_extruder_per_layer[0].resize(2);
    if (bulging_angle == 0.0)
    {
        for (size_t extruder_nr : {0, 1})
        {
            Point offset(extruder_nr? line_width_per_extruder[extruder_nr] * 2 : 0, 0);
            Point area_size(line_width_per_extruder[extruder_nr] * 2, cell_size.x);

            PolygonRef poly = cell_area_per_extruder_per_layer[0][extruder_nr].newPoly();
            poly.emplace_back(offset);
            poly.emplace_back(offset + Point(area_size.X, 0));
            poly.emplace_back(offset + area_size);
            poly.emplace_back(offset + Point(0, area_size.Y));
        }
    }
    else
    {
        coord_t h = cell_size.x / 4;
        coord_t r = tan(bulging_angle) * h;
        coord_t w = cell_size.x * line_width_per_extruder[0] / (line_width_per_extruder[0] + line_width_per_extruder[1]);
        {
            PolygonRef poly = cell_area_per_extruder_per_layer[0][0].newPoly();
            poly.emplace_back(0, 4*h);
            poly.emplace_back(r, 3*h);
            poly.emplace_back(-r, h);
            poly.emplace_back(0, 0);
            poly.emplace_back(w, 0);
            poly.emplace_back(w+r, h);
            poly.emplace_back(w-r, 3*h);
            poly.emplace_back(w, 4*h);
        }
        {
            PolygonRef poly = cell_area_per_extruder_per_layer[0][1].newPoly();
            coord_t e = cell_size.x;
            poly.emplace_back(w, 4*h);
            poly.emplace_back(w-r, 3*h);
            poly.emplace_back(w+r, h);
            poly.emplace_back(w, 0);
            poly.emplace_back(e, 0);
            poly.emplace_back(e-r, h);
            poly.emplace_back(e+r, 3*h);
            poly.emplace_back(e, 4*h);
        }
    }
    cell_area_per_extruder_per_layer[1] = cell_area_per_extruder_per_layer[0];
    for (Polygons& polys : cell_area_per_extruder_per_layer[1])
    {
        for (PolygonRef poly : polys)
        {
            for (Point& p : poly)
            {
                std::swap(p.X, p.Y);
            }
        }
    }
    if (alternating_offset)
    {
        cell_area_per_extruder_per_layer[2] = cell_area_per_extruder_per_layer[0];
        cell_area_per_extruder_per_layer[3] = cell_area_per_extruder_per_layer[1];
        for (size_t layer_nr : {2, 3})
        {
            for (Polygons& polys : cell_area_per_extruder_per_layer[layer_nr])
            {
                for (PolygonRef poly : polys)
                {
                    for (Point& p : poly)
                    {
                        p = Point(cell_size.x, cell_size.y) - p;
                    }
                }
            }
        }
    }
}

void InterlockingGenerator::applyMicrostructureToOutlines(const std::unordered_set<GridPoint3>& cells, std::vector<std::vector<Polygons>>& cell_area_per_extruder_per_layer, const std::vector<Polygons>& layer_regions)
{
    PointMatrix unapply_rotation = rotation.inverse();

    for (const GridPoint3& grid_loc : cells)
    {
        Point3 bottom_corner = vu.toLowerCorner(grid_loc);
        for (Slicer* mesh : volumes)
        {
            size_t extruder_nr = mesh->mesh->settings.get<ExtruderTrain&>("wall_0_extruder_nr").settings.get<size_t>("extruder_nr");
            for (unsigned int layer_nr = 0; layer_nr < mesh->layers.size(); layer_nr++)
            {
                SlicerLayer& layer = mesh->layers[layer_nr];
                coord_t z = layer.z;
                if (z < bottom_corner.z) continue;
                if (z > bottom_corner.z + cell_size.z) break;

                Polygons areas_here = cell_area_per_extruder_per_layer[layer_nr % cell_area_per_extruder_per_layer.size()][extruder_nr];
                Polygons areas_other = cell_area_per_extruder_per_layer[layer_nr % cell_area_per_extruder_per_layer.size()][ ! extruder_nr];

                areas_here.translate(Point(bottom_corner.x, bottom_corner.y));
                areas_other.translate(Point(bottom_corner.x, bottom_corner.y));

                const Polygons& layer_region = layer_regions[layer_nr];
                areas_here = layer_region.intersection(areas_here);

                areas_here.applyMatrix(unapply_rotation);
                areas_other.applyMatrix(unapply_rotation);

                layer.polygons = layer.polygons.unionPolygons(areas_here).difference(areas_other);
            }
        }
    }
    // TODO: remove this code and prevent any bug which this code solves in thefirst place
    for (Slicer* mesh : volumes)
    {
        for (unsigned int layer_nr = 0; layer_nr < mesh->layers.size(); layer_nr++)
        {
            SlicerLayer& layer = mesh->layers[layer_nr];
            layer.polygons = layer.polygons.offset(10).offset(-10);
        }
        if (false)
        {
            Polygons& layer = mesh->layers[0].polygons;
            SVG svg("interlock.svg", AABB(layer));
            svg.writePolygons(layer);
        }
    }
}

}//namespace cura
<commit_msg>update TODOs<commit_after>//Copyright (c) 2018 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include "InterlockingGenerator.h"

#include <algorithm> // max

#include "Application.h"
#include "Slice.h"
#include "slicer.h"
#include "utils/polygonUtils.h"
#include "utils/VoxelUtils.h"

namespace cura
{

void InterlockingGenerator::generateInterlockingStructure(std::vector<Slicer*>& volumes)
{
    /*
    if ( ! Application::getInstance().current_slice->scene.current_mesh_group->settings.get<bool>("interlocking_structure_gen"))
    {
        return;
    }
    */

    const size_t extruder_count = Application::getInstance().current_slice->scene.extruders.size();
    if (extruder_count > 2)
    {
        // TODO: fix for > 2 extruders (apply first to extruder 0 & 1, then to 0 & 2, etc.)
        logError("generateInterlockingStructure not implemented for >2 extruders!\n");
        return;
    }
    std::vector<int> meshes_per_extruder(extruder_count);
    for (Slicer* mesh : volumes)
    {
        size_t extruder_nr = mesh->mesh->settings.get<ExtruderTrain&>("wall_0_extruder_nr").settings.get<size_t>("extruder_nr");
        meshes_per_extruder[extruder_nr]++;
        if (meshes_per_extruder[extruder_nr] > 1)
        {
            logError("Currently too buggy to operate on multiple models! Each voxel needs to be associated to 2 meshes.");
            std::exit(-1);
        }
    }

    const std::vector<ExtruderTrain>& extruders = Application::getInstance().current_slice->scene.extruders;
    std::vector<coord_t> line_width_per_extruder(extruder_count);
    for (size_t extruder_nr = 0; extruder_nr < extruder_count; extruder_nr++)
    {
        line_width_per_extruder[extruder_nr] = extruders[extruder_nr].settings.get<coord_t>("wall_line_width_0");
    }
    float bulging_angle = 10.0 / 180 * M_PI;
    coord_t cell_width;
    if (bulging_angle == 0.0)
    {
        cell_width = (line_width_per_extruder[0] + line_width_per_extruder[1]) * 2;
    }
    else
    {
        cell_width = (line_width_per_extruder[0] + line_width_per_extruder[1]) * (1.0 + 1.0 / cos(bulging_angle)) / (1.0 - tan(bulging_angle));
    }
    Point3 cell_size(cell_width, cell_width, 4 * Application::getInstance().current_slice->scene.settings.get<coord_t>("layer_height"));
    // TODO make robust against if there's only 1 extruder

    std::vector<coord_t> layer_heights;
    {
        size_t layer_idx = 0;
        for (Slicer* mesh : volumes)
        {
            layer_heights.resize(std::max(layer_heights.size(), mesh->layers.size()));
            for ( ; layer_idx < mesh->layers.size() ; layer_idx++)
            {
                layer_heights[layer_idx] = mesh->layers[layer_idx].z;
            }
        }
    }

    PointMatrix rotation(45.0);

    InterlockingGenerator gen(volumes, line_width_per_extruder, layer_heights, rotation, cell_size, bulging_angle);

    // TODO: make dilation user parameter

    // TODO: option for different amount of dilation for shell removal

    // TODO: allow for multiple models on the same extruder:
    // dont add all patterns to each mesh. This leads to each mesh printing the interfaces of all other meshes.

    // TODO: implement bulging with straight pieces

    DilationKernel interface_dilation(GridPoint3(2,2,4), true);
    std::vector<std::unordered_set<GridPoint3>> voxels_per_extruder = gen.getShellVoxels(interface_dilation);

    std::vector<Polygons> layer_regions(layer_heights.size());
    gen.computeLayerRegions(layer_regions);

    std::unordered_set<GridPoint3>& has_any_extruder = voxels_per_extruder[0];
    std::unordered_set<GridPoint3>& has_all_extruders = voxels_per_extruder[1];
    has_any_extruder.merge(has_all_extruders);

    constexpr bool air_filtering = false;
    if (air_filtering)
    {
        DilationKernel air_dilation(GridPoint3(1,1,1), true);
        std::unordered_set<GridPoint3> air_cells;
        gen.addBoundaryCells(layer_regions, air_dilation, air_cells);

        for (const GridPoint3& p : air_cells)
        {
            has_all_extruders.erase(p);
        }
    }

    std::vector<std::vector<Polygons>> cell_area_per_extruder_per_layer;
    const bool alternating_offset = bulging_angle > 0.0;
    gen.generateMicrostructure(cell_area_per_extruder_per_layer, alternating_offset);

    gen.applyMicrostructureToOutlines(has_all_extruders, cell_area_per_extruder_per_layer, layer_regions);
}

InterlockingGenerator::Cell::Cell()
: has_extruder(Application::getInstance().current_slice->scene.extruders.size(), false)
{
    assert(has_extruder.size() >= 1);
}

std::vector<std::unordered_set<GridPoint3>> InterlockingGenerator::getShellVoxels(const DilationKernel& kernel)
{
    std::vector<std::unordered_set<GridPoint3>> voxels_per_extruder(2);

    // mark all cells which contain some boundary
    for (Slicer* mesh : volumes)
    {
        size_t extruder_nr = mesh->mesh->settings.get<ExtruderTrain&>("wall_0_extruder_nr").settings.get<size_t>("extruder_nr");
        assert(extruder_nr < 2);
        std::unordered_set<GridPoint3>& mesh_voxels = voxels_per_extruder[extruder_nr];

        std::vector<Polygons> rotated_polygons_per_layer(mesh->layers.size());
        for (size_t layer_nr = 0; layer_nr < mesh->layers.size(); layer_nr++)
        {
            SlicerLayer& layer = mesh->layers[layer_nr];
            rotated_polygons_per_layer[layer_nr] = layer.polygons;
            rotated_polygons_per_layer[layer_nr].applyMatrix(rotation);
        }

        addBoundaryCells(rotated_polygons_per_layer, kernel, mesh_voxels);
    }

    return voxels_per_extruder;
}

void InterlockingGenerator::addBoundaryCells(std::vector<Polygons>& layers, const DilationKernel& kernel, std::unordered_set<GridPoint3>& cells)
{
    auto voxel_emplacer = [&cells](GridPoint3 p) { cells.emplace(p); return true; };

    for (size_t layer_nr = 0; layer_nr < layers.size(); layer_nr++)
    {
        coord_t z = layer_heights[layer_nr];
        vu.walkDilatedPolygons(layers[layer_nr], z, kernel, voxel_emplacer);
        Polygons skin = layers[layer_nr];
        if (layer_nr > 0)
        {
            skin = skin.xorPolygons(layers[layer_nr - 1]);
        }
        skin = skin.offset(-cell_size.x / 2).offset(cell_size.x / 2); // remove superfluous small areas, which would anyway be included because of walkPolygons
        vu.walkDilatedAreas(skin, z, kernel, voxel_emplacer);
    }
}

void InterlockingGenerator::computeLayerRegions(std::vector<Polygons>& layer_regions)
{
    for (unsigned int layer_nr = 0; layer_nr < layer_regions.size(); layer_nr++)
    {
        Polygons& layer_region = layer_regions[layer_nr];
        coord_t z;
        for (Slicer* mesh : volumes)
        {
            if (layer_nr >= mesh->layers.size()) break;
            SlicerLayer& layer = mesh->layers[layer_nr];
            z = layer.z;
            layer_region.add(layer.polygons);
        }
        layer_region = layer_region.offset(100).offset(-100); // TODO hardcoded value
        layer_region.applyMatrix(rotation);
        layer_heights[layer_nr] = z;
    }
}

void InterlockingGenerator::generateMicrostructure(std::vector<std::vector<Polygons>>& cell_area_per_extruder_per_layer, bool alternating_offset)
{
    cell_area_per_extruder_per_layer.resize(alternating_offset? 4 : 2);
    cell_area_per_extruder_per_layer[0].resize(2);
    if (bulging_angle == 0.0)
    {
        for (size_t extruder_nr : {0, 1})
        {
            Point offset(extruder_nr? line_width_per_extruder[extruder_nr] * 2 : 0, 0);
            Point area_size(line_width_per_extruder[extruder_nr] * 2, cell_size.x);

            PolygonRef poly = cell_area_per_extruder_per_layer[0][extruder_nr].newPoly();
            poly.emplace_back(offset);
            poly.emplace_back(offset + Point(area_size.X, 0));
            poly.emplace_back(offset + area_size);
            poly.emplace_back(offset + Point(0, area_size.Y));
        }
    }
    else
    {
        coord_t h = cell_size.x / 4;
        coord_t r = tan(bulging_angle) * h;
        coord_t w = cell_size.x * line_width_per_extruder[0] / (line_width_per_extruder[0] + line_width_per_extruder[1]);
        {
            PolygonRef poly = cell_area_per_extruder_per_layer[0][0].newPoly();
            poly.emplace_back(0, 4*h);
            poly.emplace_back(r, 3*h);
            poly.emplace_back(-r, h);
            poly.emplace_back(0, 0);
            poly.emplace_back(w, 0);
            poly.emplace_back(w+r, h);
            poly.emplace_back(w-r, 3*h);
            poly.emplace_back(w, 4*h);
        }
        {
            PolygonRef poly = cell_area_per_extruder_per_layer[0][1].newPoly();
            coord_t e = cell_size.x;
            poly.emplace_back(w, 4*h);
            poly.emplace_back(w-r, 3*h);
            poly.emplace_back(w+r, h);
            poly.emplace_back(w, 0);
            poly.emplace_back(e, 0);
            poly.emplace_back(e-r, h);
            poly.emplace_back(e+r, 3*h);
            poly.emplace_back(e, 4*h);
        }
    }
    cell_area_per_extruder_per_layer[1] = cell_area_per_extruder_per_layer[0];
    for (Polygons& polys : cell_area_per_extruder_per_layer[1])
    {
        for (PolygonRef poly : polys)
        {
            for (Point& p : poly)
            {
                std::swap(p.X, p.Y);
            }
        }
    }
    if (alternating_offset)
    {
        cell_area_per_extruder_per_layer[2] = cell_area_per_extruder_per_layer[0];
        cell_area_per_extruder_per_layer[3] = cell_area_per_extruder_per_layer[1];
        for (size_t layer_nr : {2, 3})
        {
            for (Polygons& polys : cell_area_per_extruder_per_layer[layer_nr])
            {
                for (PolygonRef poly : polys)
                {
                    for (Point& p : poly)
                    {
                        p = Point(cell_size.x, cell_size.y) - p;
                    }
                }
            }
        }
    }
}

void InterlockingGenerator::applyMicrostructureToOutlines(const std::unordered_set<GridPoint3>& cells, std::vector<std::vector<Polygons>>& cell_area_per_extruder_per_layer, const std::vector<Polygons>& layer_regions)
{
    PointMatrix unapply_rotation = rotation.inverse();

    for (const GridPoint3& grid_loc : cells)
    {
        Point3 bottom_corner = vu.toLowerCorner(grid_loc);
        for (Slicer* mesh : volumes)
        {
            size_t extruder_nr = mesh->mesh->settings.get<ExtruderTrain&>("wall_0_extruder_nr").settings.get<size_t>("extruder_nr");
            for (unsigned int layer_nr = 0; layer_nr < mesh->layers.size(); layer_nr++)
            {
                SlicerLayer& layer = mesh->layers[layer_nr];
                coord_t z = layer.z;
                if (z < bottom_corner.z) continue;
                if (z > bottom_corner.z + cell_size.z) break;

                Polygons areas_here = cell_area_per_extruder_per_layer[layer_nr % cell_area_per_extruder_per_layer.size()][extruder_nr];
                Polygons areas_other = cell_area_per_extruder_per_layer[layer_nr % cell_area_per_extruder_per_layer.size()][ ! extruder_nr];

                areas_here.translate(Point(bottom_corner.x, bottom_corner.y));
                areas_other.translate(Point(bottom_corner.x, bottom_corner.y));

                const Polygons& layer_region = layer_regions[layer_nr];
                areas_here = layer_region.intersection(areas_here);

                areas_here.applyMatrix(unapply_rotation);
                areas_other.applyMatrix(unapply_rotation);

                layer.polygons = layer.polygons.unionPolygons(areas_here).difference(areas_other);
            }
        }
    }
    // TODO: remove this code and prevent any bug which this code solves in thefirst place
    for (Slicer* mesh : volumes)
    {
        for (unsigned int layer_nr = 0; layer_nr < mesh->layers.size(); layer_nr++)
        {
            SlicerLayer& layer = mesh->layers[layer_nr];
            layer.polygons = layer.polygons.offset(10).offset(-10);
        }
        if (false)
        {
            Polygons& layer = mesh->layers[0].polygons;
            SVG svg("interlock.svg", AABB(layer));
            svg.writePolygons(layer);
        }
    }
}

}//namespace cura
<|endoftext|>


<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2014 Artem Pavlenko
 *
 * 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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

// mapnik
#include <mapnik/feature.hpp>
#include <mapnik/agg_renderer.hpp>
#include <mapnik/image_any.hpp>
#include <mapnik/symbolizer.hpp>
#include <mapnik/label_collision_detector.hpp>
#include <mapnik/image_util.hpp>
#include <mapnik/vertex_processor.hpp>
#include <mapnik/agg_rasterizer.hpp>
#include <mapnik/transform_path_adapter.hpp>
#include <mapnik/agg_helpers.hpp>
#include <mapnik/util/is_clockwise.hpp>

// agg
#include "agg_basics.h"
#include "agg_rendering_buffer.h"
#include "agg_color_rgba.h"
#include "agg_pixfmt_rgba.h"
#include "agg_scanline_u.h"
#include "agg_renderer_scanline.h"
#include "agg_conv_stroke.h"

namespace mapnik {

namespace {
template <typename T>
void draw_rect(T &pixmap, box2d<double> const& box)
{
    int x0 = static_cast<int>(box.minx());
    int x1 = static_cast<int>(box.maxx());
    int y0 = static_cast<int>(box.miny());
    int y1 = static_cast<int>(box.maxy());
    unsigned color1 = 0xff0000ff;
    for (int x=x0; x<x1; x++)
    {
        mapnik::set_pixel(pixmap, x, y0, color1);
        mapnik::set_pixel(pixmap, x, y1, color1);
    }
    for (int y=y0; y<y1; y++)
    {
        mapnik::set_pixel(pixmap, x0, y, color1);
        mapnik::set_pixel(pixmap, x1, y, color1);
    }
}

template <typename Pixmap>
struct apply_vertex_mode
{
    apply_vertex_mode(Pixmap & pixmap, view_transform const& t, proj_transform const& prj_trans)
        : pixmap_(pixmap),
          t_(t),
          prj_trans_(prj_trans) {}

    template <typename Adapter>
    void operator() (Adapter const& va) const
    {
        double x;
        double y;
        double z = 0;
        va.rewind(0);
        unsigned cmd = SEG_END;
        while ((cmd = va.vertex(&x, &y)) != mapnik::SEG_END)
        {
            if (cmd == SEG_CLOSE) continue;
            prj_trans_.backward(x,y,z);
            t_.forward(&x,&y);
            mapnik::set_pixel(pixmap_,x,y,0xff0000ff);
            mapnik::set_pixel(pixmap_,x-1,y-1,0xff0000ff);
            mapnik::set_pixel(pixmap_,x+1,y+1,0xff0000ff);
            mapnik::set_pixel(pixmap_,x-1,y+1,0xff0000ff);
            mapnik::set_pixel(pixmap_,x+1,y-1,0xff0000ff);
        }
    }

    Pixmap & pixmap_;
    view_transform const& t_;
    proj_transform const& prj_trans_;
};

template <typename BufferType>
struct RingRenderer {

    using ren_base = agg::renderer_base<agg::pixfmt_rgba32_pre>;
    using renderer = agg::renderer_scanline_aa_solid<ren_base>;
    using path_type = transform_path_adapter<view_transform, geometry::ring_vertex_adapter>;

    RingRenderer(rasterizer & ras_ptr,
                 BufferType & im,
                 view_transform const& tr,
                 proj_transform const& prj_trans) :
       ras_ptr_(ras_ptr),
       im_(im),
       tr_(tr),
       prj_trans_(prj_trans),
       sl_() {}

    void draw_ring(geometry::linear_ring const& ring,
                   agg::rgba8 const& color)
    {
        ras_ptr_.reset();
        agg::rendering_buffer buf(im_.getBytes(),im_.width(),im_.height(),im_.getRowSize());
        agg::pixfmt_rgba32_pre pixf(buf);
        ren_base renb(pixf);
        renderer ren(renb);
        geometry::ring_vertex_adapter va(ring);
        path_type path(tr_,va,prj_trans_);
        ras_ptr_.add_path(path);
        ren.color(color);
        agg::render_scanlines(ras_ptr_, sl_, ren);
    }

    void draw_outline(geometry::linear_ring const& ring,
                   agg::rgba8 const& color,
                   double stroke_width)
    {
        ras_ptr_.reset();
        agg::rendering_buffer buf(im_.getBytes(),im_.width(),im_.height(),im_.getRowSize());
        agg::pixfmt_rgba32_pre pixf(buf);
        ren_base renb(pixf);
        renderer ren(renb);
        geometry::ring_vertex_adapter va(ring);
        path_type path(tr_,va,prj_trans_);
        agg::conv_stroke<path_type> stroke(path);
        stroke.width(stroke_width);
        ras_ptr_.add_path(stroke);
        ras_ptr_.filling_rule(agg::fill_non_zero);
        ren.color(color);
        agg::render_scanlines(ras_ptr_, sl_, ren);
    }

    rasterizer & ras_ptr_;
    BufferType & im_;
    view_transform const& tr_;
    proj_transform const& prj_trans_;
    agg::scanline_u8 sl_;
};

template <typename BufferType>
struct render_ring_visitor {

    render_ring_visitor(RingRenderer<BufferType> & renderer)
     : renderer_(renderer) {}

    void operator()(mapnik::geometry::multi_polygon const& geom)
    {
        for (auto const& poly : geom)
        {
            (*this)(poly);
        }
    }

    void operator()(mapnik::geometry::polygon const& geom)
    {
        agg::rgba8 red(255, 0, 0, 255);
        agg::rgba8 green(0, 255, 255, 255);
        agg::rgba black(0,0,0,255);
        renderer_.draw_ring(geom.exterior_ring,red);
        if (mapnik::util::is_clockwise(geom.exterior_ring))
        {
            renderer_.draw_outline(geom.exterior_ring,black,2);
        }
        for (auto const& ring : geom.interior_rings)
        {
            renderer_.draw_ring(ring,green);
            if (!mapnik::util::is_clockwise(ring))
            {
                renderer_.draw_outline(ring,black,2);
            }
        }
    }

    template<typename GeomType>
    void operator()(GeomType const&) {}

    RingRenderer<BufferType> & renderer_;
};

} // anonymous namespace

template <typename T0, typename T1>
void agg_renderer<T0,T1>::process(debug_symbolizer const& sym,
                              mapnik::feature_impl & feature,
                              proj_transform const& prj_trans)
{

    debug_symbolizer_mode_enum mode = get<debug_symbolizer_mode_enum>(sym, keys::mode, feature, common_.vars_, DEBUG_SYM_MODE_COLLISION);

    if (mode == DEBUG_SYM_MODE_RINGS)
    {
        RingRenderer<buffer_type> renderer(*ras_ptr,*current_buffer_,common_.t_,prj_trans);
        render_ring_visitor<buffer_type> apply(renderer);
        mapnik::util::apply_visitor(apply,feature.get_geometry());
    }
    else if (mode == DEBUG_SYM_MODE_COLLISION)
    {
        typename detector_type::query_iterator itr = common_.detector_->begin();
        typename detector_type::query_iterator end = common_.detector_->end();
        for ( ;itr!=end; ++itr)
        {
            draw_rect(pixmap_, itr->box);
        }
    }
    else if (mode == DEBUG_SYM_MODE_VERTEX)
    {
        using apply_vertex_mode = apply_vertex_mode<buffer_type>;
        apply_vertex_mode apply(pixmap_, common_.t_, prj_trans);
        util::apply_visitor(geometry::vertex_processor<apply_vertex_mode>(apply), feature.get_geometry());
    }
}

template void agg_renderer<image_rgba8>::process(debug_symbolizer const&,
                                              mapnik::feature_impl &,
                                              proj_transform const&);
}
<commit_msg>fix outline drawing in debugsymbolizer ring mode<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2014 Artem Pavlenko
 *
 * 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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

// mapnik
#include <mapnik/feature.hpp>
#include <mapnik/agg_renderer.hpp>
#include <mapnik/image_any.hpp>
#include <mapnik/symbolizer.hpp>
#include <mapnik/label_collision_detector.hpp>
#include <mapnik/image_util.hpp>
#include <mapnik/vertex_processor.hpp>
#include <mapnik/agg_rasterizer.hpp>
#include <mapnik/transform_path_adapter.hpp>
#include <mapnik/agg_helpers.hpp>
#include <mapnik/util/is_clockwise.hpp>

// agg
#include "agg_basics.h"
#include "agg_rendering_buffer.h"
#include "agg_color_rgba.h"
#include "agg_pixfmt_rgba.h"
#include "agg_scanline_u.h"
#include "agg_renderer_scanline.h"
#include "agg_conv_stroke.h"

namespace mapnik {

namespace {
template <typename T>
void draw_rect(T &pixmap, box2d<double> const& box)
{
    int x0 = static_cast<int>(box.minx());
    int x1 = static_cast<int>(box.maxx());
    int y0 = static_cast<int>(box.miny());
    int y1 = static_cast<int>(box.maxy());
    unsigned color1 = 0xff0000ff;
    for (int x=x0; x<x1; x++)
    {
        mapnik::set_pixel(pixmap, x, y0, color1);
        mapnik::set_pixel(pixmap, x, y1, color1);
    }
    for (int y=y0; y<y1; y++)
    {
        mapnik::set_pixel(pixmap, x0, y, color1);
        mapnik::set_pixel(pixmap, x1, y, color1);
    }
}

template <typename Pixmap>
struct apply_vertex_mode
{
    apply_vertex_mode(Pixmap & pixmap, view_transform const& t, proj_transform const& prj_trans)
        : pixmap_(pixmap),
          t_(t),
          prj_trans_(prj_trans) {}

    template <typename Adapter>
    void operator() (Adapter const& va) const
    {
        double x;
        double y;
        double z = 0;
        va.rewind(0);
        unsigned cmd = SEG_END;
        while ((cmd = va.vertex(&x, &y)) != mapnik::SEG_END)
        {
            if (cmd == SEG_CLOSE) continue;
            prj_trans_.backward(x,y,z);
            t_.forward(&x,&y);
            mapnik::set_pixel(pixmap_,x,y,0xff0000ff);
            mapnik::set_pixel(pixmap_,x-1,y-1,0xff0000ff);
            mapnik::set_pixel(pixmap_,x+1,y+1,0xff0000ff);
            mapnik::set_pixel(pixmap_,x-1,y+1,0xff0000ff);
            mapnik::set_pixel(pixmap_,x+1,y-1,0xff0000ff);
        }
    }

    Pixmap & pixmap_;
    view_transform const& t_;
    proj_transform const& prj_trans_;
};

template <typename BufferType>
struct RingRenderer {

    using color_type = agg::rgba8;
    using order_type = agg::order_rgba;
    using blender_type = agg::comp_op_adaptor_rgba_pre<color_type, order_type>; // comp blender
    using pixfmt_comp_type = agg::pixfmt_custom_blend_rgba<blender_type, agg::rendering_buffer>;
    using renderer_base = agg::renderer_base<pixfmt_comp_type>;
    using renderer_type = agg::renderer_scanline_aa_solid<renderer_base>;

    using path_type = transform_path_adapter<view_transform, geometry::ring_vertex_adapter>;

    RingRenderer(rasterizer & ras_ptr,
                 BufferType & im,
                 view_transform const& tr,
                 proj_transform const& prj_trans) :
       ras_ptr_(ras_ptr),
       im_(im),
       tr_(tr),
       prj_trans_(prj_trans),
       sl_() {}

    void draw_ring(geometry::linear_ring const& ring,
                   agg::rgba8 const& color)
    {
        ras_ptr_.reset();
        agg::rendering_buffer buf(im_.getBytes(),im_.width(),im_.height(),im_.getRowSize());
        pixfmt_comp_type pixf(buf);
        renderer_base renb(pixf);
        renderer_type ren(renb);
        geometry::ring_vertex_adapter va(ring);
        path_type path(tr_,va,prj_trans_);
        ras_ptr_.add_path(path);
        ras_ptr_.filling_rule(agg::fill_non_zero);
        ren.color(color);
        agg::render_scanlines(ras_ptr_, sl_, ren);
    }

    void draw_outline(geometry::linear_ring const& ring,
                   agg::rgba8 const& color,
                   double stroke_width=3)
    {
        ras_ptr_.reset();
        agg::rendering_buffer buf(im_.getBytes(),im_.width(),im_.height(),im_.getRowSize());
        pixfmt_comp_type pixf(buf);
        renderer_base renb(pixf);
        renderer_type ren(renb);
        geometry::ring_vertex_adapter va(ring);
        path_type path(tr_,va,prj_trans_);
        agg::conv_stroke<path_type> stroke(path);
        stroke.width(stroke_width);
        ras_ptr_.add_path(stroke);
        ras_ptr_.filling_rule(agg::fill_non_zero);
        ren.color(color);
        agg::render_scanlines(ras_ptr_, sl_, ren);
    }

    rasterizer & ras_ptr_;
    BufferType & im_;
    view_transform const& tr_;
    proj_transform const& prj_trans_;
    agg::scanline_u8 sl_;
};

template <typename BufferType>
struct render_ring_visitor {

    render_ring_visitor(RingRenderer<BufferType> & renderer)
     : renderer_(renderer) {}

    void operator()(mapnik::geometry::multi_polygon const& geom)
    {
        for (auto const& poly : geom)
        {
            (*this)(poly);
        }
    }

    void operator()(mapnik::geometry::polygon const& geom)
    {
        agg::rgba8 red(255,0,0,255);
        agg::rgba8 green(0,255,255,255);
        agg::rgba8 black(0,0,0,255);
        renderer_.draw_ring(geom.exterior_ring,red);
        if (mapnik::util::is_clockwise(geom.exterior_ring))
        {
            renderer_.draw_outline(geom.exterior_ring,black);
        }
        for (auto const& ring : geom.interior_rings)
        {
            renderer_.draw_ring(ring,green);
            if (!mapnik::util::is_clockwise(ring))
            {
                renderer_.draw_outline(ring,black);
            }
        }
    }

    template<typename GeomType>
    void operator()(GeomType const&) {}

    RingRenderer<BufferType> & renderer_;
};

} // anonymous namespace

template <typename T0, typename T1>
void agg_renderer<T0,T1>::process(debug_symbolizer const& sym,
                              mapnik::feature_impl & feature,
                              proj_transform const& prj_trans)
{

    debug_symbolizer_mode_enum mode = get<debug_symbolizer_mode_enum>(sym, keys::mode, feature, common_.vars_, DEBUG_SYM_MODE_COLLISION);

    if (mode == DEBUG_SYM_MODE_RINGS)
    {
        RingRenderer<buffer_type> renderer(*ras_ptr,*current_buffer_,common_.t_,prj_trans);
        render_ring_visitor<buffer_type> apply(renderer);
        mapnik::util::apply_visitor(apply,feature.get_geometry());
    }
    else if (mode == DEBUG_SYM_MODE_COLLISION)
    {
        typename detector_type::query_iterator itr = common_.detector_->begin();
        typename detector_type::query_iterator end = common_.detector_->end();
        for ( ;itr!=end; ++itr)
        {
            draw_rect(pixmap_, itr->box);
        }
    }
    else if (mode == DEBUG_SYM_MODE_VERTEX)
    {
        using apply_vertex_mode = apply_vertex_mode<buffer_type>;
        apply_vertex_mode apply(pixmap_, common_.t_, prj_trans);
        util::apply_visitor(geometry::vertex_processor<apply_vertex_mode>(apply), feature.get_geometry());
    }
}

template void agg_renderer<image_rgba8>::process(debug_symbolizer const&,
                                              mapnik::feature_impl &,
                                              proj_transform const&);
}
<|endoftext|>


<commit_before>// See Fossen 2011, chapter 12.3.2
#include "lagrange_allocator.h"
#include <iostream>

template<typename Derived>
inline bool isFucked(const Eigen::MatrixBase<Derived>& x)
{
    return !((x.array() == x.array())).all() && !( (x - x).array() == (x - x).array()).all();
}

void printMatrix6(Eigen::MatrixXd m);

LagrangeAllocator::LagrangeAllocator()
{
    sub = nh.subscribe("rov_forces", 10, &LagrangeAllocator::callback, this);
    pub = nh.advertise<uranus_dp::ThrusterForces>("thruster_forces", 10);

    W.setIdentity(); // Default to identity (i.e. equal weights)
    K.setIdentity(); // Scaling is done on Arduino, so this can be identity

    // Thruster configuration does not allow control of roll.
    T = << 0.7071 ,  0.7071 , -0.7071 , -0.7071 ,  0     ,  0     , // Surge
           0.7071 , -0.7071 , -0.7071 ,  0.7071 ,  0     ,  0     , // Sway
           0      ,  0      ,  0      ,  0      , -1     , -1     , // Heave
          -0.04667, -0.04667,  0.04667,  0.04667, -0.1260,  0.1260, // Pitch
          -0.2143 ,  0.2143 , -0.2143 ,  0.2143 ,  0     ,  0     ; // Yaw

    K_inverse = K.inverse();
    computeGeneralizedInverse();
}

void LagrangeAllocator::callback(const geometry_msgs::Wrench& tauMsg)
{
    tau << tauMsg.force.x,
           tauMsg.force.y,
           tauMsg.force.z,
           // tauMsg.torque.x, // Roll is not controllable
           tauMsg.torque.y,
           tauMsg.torque.z;

    u = K_inverse * T_geninverse * tau;

    if (isFucked(K_inverse))
    {
        ROS_WARN("K is not invertible");
    }

    uranus_dp::ThrusterForces uMsg;
    uMsg.F1 = u(0);
    uMsg.F2 = u(1);
    uMsg.F3 = u(2);
    uMsg.F4 = u(3);
    uMsg.F5 = u(4);
    uMsg.F6 = u(5);
    pub.publish(uMsg);

    ROS_INFO_STREAM("lagrange_allocator: Sending " << u(0) << ", " << u(1) << ", " << u(2) << ", " << u(3) << ", " << u(4) << ", " << u(5));
}

void LagrangeAllocator::setWeights(const Eigen::MatrixXd &W_new)
{
    bool correctDimensions = ( W_new.rows() == r && W_new.cols() == r );
    if (!correctDimensions)
    {
        ROS_WARN_STREAM("Attempt to set weight matrix in LagrangeAllocator with wrong dimensions " << W_new.rows() << "*" << W_new.cols() << ".");
        return;
    }

    W = W_new;

    // New weights require recomputing the generalized inverse of the thrust config matrix
    computeGeneralizedInverse();
}

void LagrangeAllocator::computeGeneralizedInverse()
{
    T_geninverse = W.inverse()*T.transpose() * (T*W.inverse()*T.transpose()).inverse();

    // ROS_INFO("Printing in order: T_geninverse, W, T, K_inverse.");
    // printMatrix6(T_geninverse);
    // printMatrix6(W);
    // printMatrix6(T);
    // printMatrix6(K_inverse);

    if (isFucked(T_geninverse))
    {
        ROS_WARN("T_geninverse NAN");
    }

    if (isFucked( W.inverse() ))
    {
        ROS_WARN("W is not invertible");
    }

    if (isFucked( (T*W.inverse()*T.transpose()).inverse() ) )
    {
        ROS_WARN("T * W_inv * T transposed is not invertible");
    }
}

// Worlds worst print function :DDD
void printMatrix6(Eigen::MatrixXd m)
{
    ROS_INFO("----------------------------------");
    for(int col = 0; col < 6; col++)
    {
        char buffer[512];

        for(int row = 0; row < 6; row++)
        {
            snprintf( (buffer + (row*8) ), sizeof(buffer), "  %f  ", m(col, row));
        }
        ROS_INFO("%s", buffer);
    }
    ROS_INFO("----------------------------------\n");
}<commit_msg>Fix stupid typo<commit_after>// See Fossen 2011, chapter 12.3.2
#include "lagrange_allocator.h"
#include <iostream>

template<typename Derived>
inline bool isFucked(const Eigen::MatrixBase<Derived>& x)
{
    return !((x.array() == x.array())).all() && !( (x - x).array() == (x - x).array()).all();
}

void printMatrix6(Eigen::MatrixXd m);

LagrangeAllocator::LagrangeAllocator()
{
    sub = nh.subscribe("rov_forces", 10, &LagrangeAllocator::callback, this);
    pub = nh.advertise<uranus_dp::ThrusterForces>("thruster_forces", 10);

    W.setIdentity(); // Default to identity (i.e. equal weights)
    K.setIdentity(); // Scaling is done on Arduino, so this can be identity

    // Thruster configuration does not allow control of roll.
    T << 0.7071 ,  0.7071 , -0.7071 , -0.7071 ,  0     ,  0     , // Surge
         0.7071 , -0.7071 , -0.7071 ,  0.7071 ,  0     ,  0     , // Sway
         0      ,  0      ,  0      ,  0      , -1     , -1     , // Heave
        -0.04667, -0.04667,  0.04667,  0.04667, -0.1260,  0.1260, // Pitch
        -0.2143 ,  0.2143 , -0.2143 ,  0.2143 ,  0     ,  0     ; // Yaw

    K_inverse = K.inverse();
    computeGeneralizedInverse();
}

void LagrangeAllocator::callback(const geometry_msgs::Wrench& tauMsg)
{
    tau << tauMsg.force.x,
           tauMsg.force.y,
           tauMsg.force.z,
           // tauMsg.torque.x, // Roll is not controllable
           tauMsg.torque.y,
           tauMsg.torque.z;

    u = K_inverse * T_geninverse * tau;

    if (isFucked(K_inverse))
    {
        ROS_WARN("K is not invertible");
    }

    uranus_dp::ThrusterForces uMsg;
    uMsg.F1 = u(0);
    uMsg.F2 = u(1);
    uMsg.F3 = u(2);
    uMsg.F4 = u(3);
    uMsg.F5 = u(4);
    uMsg.F6 = u(5);
    pub.publish(uMsg);

    ROS_INFO_STREAM("lagrange_allocator: Sending " << u(0) << ", " << u(1) << ", " << u(2) << ", " << u(3) << ", " << u(4) << ", " << u(5));
}

void LagrangeAllocator::setWeights(const Eigen::MatrixXd &W_new)
{
    bool correctDimensions = ( W_new.rows() == r && W_new.cols() == r );
    if (!correctDimensions)
    {
        ROS_WARN_STREAM("Attempt to set weight matrix in LagrangeAllocator with wrong dimensions " << W_new.rows() << "*" << W_new.cols() << ".");
        return;
    }

    W = W_new;

    // New weights require recomputing the generalized inverse of the thrust config matrix
    computeGeneralizedInverse();
}

void LagrangeAllocator::computeGeneralizedInverse()
{
    T_geninverse = W.inverse()*T.transpose() * (T*W.inverse()*T.transpose()).inverse();

    // ROS_INFO("Printing in order: T_geninverse, W, T, K_inverse.");
    // printMatrix6(T_geninverse);
    // printMatrix6(W);
    // printMatrix6(T);
    // printMatrix6(K_inverse);

    if (isFucked(T_geninverse))
    {
        ROS_WARN("T_geninverse NAN");
    }

    if (isFucked( W.inverse() ))
    {
        ROS_WARN("W is not invertible");
    }

    if (isFucked( (T*W.inverse()*T.transpose()).inverse() ) )
    {
        ROS_WARN("T * W_inv * T transposed is not invertible");
    }
}

// Worlds worst print function :DDD
void printMatrix6(Eigen::MatrixXd m)
{
    ROS_INFO("----------------------------------");
    for(int col = 0; col < 6; col++)
    {
        char buffer[512];

        for(int row = 0; row < 6; row++)
        {
            snprintf( (buffer + (row*8) ), sizeof(buffer), "  %f  ", m(col, row));
        }
        ROS_INFO("%s", buffer);
    }
    ROS_INFO("----------------------------------\n");
}<|endoftext|>


<commit_before>//=======================================================================
// Copyright Baptiste Wicht 2013-2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef UNIQUE_PTR_H
#define UNIQUE_PTR_H

#include "thor.hpp"

namespace std {

template<typename T>
struct default_delete {
    default_delete() {}

    template<typename Up>
    default_delete(const default_delete<Up>&) {}

    void operator()(T* ptr) const {
        static_assert(sizeof(T) > 0, "Type must be complete");
        delete ptr;
    }
};

template <typename T, typename D = default_delete<T>>
class unique_ptr {
public:
    typedef T* pointer_type;
    typedef T element_type;
    typedef D deleter_type;

private:
    pointer_type pointer;
    deleter_type deleter;

public:
    unique_ptr() : pointer(pointer_type()), deleter(deleter_type()) {}

    explicit unique_ptr(pointer_type p) : pointer(p), deleter(deleter_type()) {}

    unique_ptr(unique_ptr&& u) : pointer(u.release()), deleter(u.get_deleter()) {}
    unique_ptr& operator=(unique_ptr&& u){
        reset(u.release());
        return *this;
    }

    ~unique_ptr(){
        reset();
    }

    // Disable copy
    unique_ptr(const unique_ptr& rhs) = delete;
    unique_ptr& operator=(const unique_ptr& rhs) = delete;

    element_type& operator*() const {
        return *get();
    }

    pointer_type operator->() const {
        return get();
    }

    pointer_type get() const {
        return pointer;
    }

    deleter_type get_deleter() const {
        return deleter;
    }

    operator bool() const {
        return get();
    }

    pointer_type release(){
        pointer_type p = pointer;
        pointer = nullptr;
        return p;
    }

    void reset(pointer_type p = pointer_type()){
        if(pointer != p){
            get_deleter()(pointer);
            pointer = nullptr;
        }
    }
};

} //end of namespace std

#endif
<commit_msg>Remove the overhead of unique_ptr when using default_deleter<commit_after>//=======================================================================
// Copyright Baptiste Wicht 2013-2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef UNIQUE_PTR_H
#define UNIQUE_PTR_H

#include <tuple.hpp>

#include "thor.hpp"

namespace std {

template<typename T>
struct default_delete {
    default_delete() {}

    template<typename Up>
    default_delete(const default_delete<Up>&) {}

    void operator()(T* ptr) const {
        static_assert(sizeof(T) > 0, "Type must be complete");
        delete ptr;
    }
};

template <typename T, typename D = default_delete<T>>
class unique_ptr {
public:
    typedef T* pointer_type;
    typedef T element_type;
    typedef D deleter_type;

private:
    typedef tuple<pointer_type, deleter_type> data_impl;

    data_impl _data;

    //pointer_type pointer;
    //deleter_type deleter;

public:
    unique_ptr() : _data(make_tuple(pointer_type(), deleter_type())) {}

    explicit unique_ptr(pointer_type p) : _data(make_tuple(p, deleter_type())) {}

    unique_ptr(unique_ptr&& u) : _data(make_tuple(u.release(), u.get_deleter())) {}
    unique_ptr& operator=(unique_ptr&& u){
        reset(u.release());
        return *this;
    }

    ~unique_ptr(){
        reset();
    }

    // Disable copy
    unique_ptr(const unique_ptr& rhs) = delete;
    unique_ptr& operator=(const unique_ptr& rhs) = delete;

    element_type& operator*() const {
        return *get();
    }

    pointer_type operator->() const {
        return get();
    }

    pointer_type get() const {
        return std::get<0>(_data);
    }

    deleter_type get_deleter() const {
        return std::get<1>(_data);
    }

    operator bool() const {
        return get();
    }

    pointer_type release(){
        pointer_type p = get();
        std::get<0>(_data) = nullptr;
        return p;
    }

    void reset(pointer_type p = pointer_type()){
        if(get() != p){
            get_deleter()(get());
            std::get<0>(_data) = nullptr;
        }
    }
};

static_assert(sizeof(unique_ptr<long>) == sizeof(long), "unique_ptr must have zero overhead with default deleter");

} //end of namespace std

#endif
<|endoftext|>


<commit_before>/// \file
/// \ingroup tutorial_hist
/// \notebook
/// Example of candle plot with 2-D histograms.
///
/// \macro_image
/// \macro_code
///
/// \author Georg Troska

void candleplot() {

   gStyle->SetTimeOffset(0);
   auto rng = new TRandom();
   auto dateBegin = new TDatime(2010,1,1,0,0,0);
   auto dateEnd = new TDatime(2011,1,1,0,0,0);

   auto h1 = new TH2I("h1","Machine A + B",12,dateBegin->Convert(),dateEnd->Convert(),1000,0,1000);
   auto h2 = new TH2I("h2","Machine B",12,dateBegin->Convert(),dateEnd->Convert(),1000,0,1000);

   h1->GetXaxis()->SetTimeDisplay(1);
   h1->GetXaxis()->SetTimeFormat("%m/%y");
   h1->GetXaxis()->SetTitle("Date [month/year]");

   float Rand;
   for (int i = dateBegin->Convert(); i < dateEnd->Convert(); i+=86400*30) {
      for (int j = 0; j < 1000; j++) {
         Rand = rng->Gaus(500+sin(i/10000000.)*100,50); h1->Fill(i,Rand);
         Rand = rng->Gaus(500+sin(i/11000000.)*100,70); h2->Fill(i,Rand);
      }
   }

   h1->SetBarWidth(0.4);
   h1->SetBarOffset(-0.25);
   h1->SetFillColor(kYellow);
   h1->SetFillStyle(1001);

   h2->SetBarWidth(0.4);
   h2->SetBarOffset(0.25);
   h2->SetLineColor(kRed);
   h2->SetFillColor(kGreen);

   auto c1 = new TCanvas();

   h1->Draw("candle2");
   h2->Draw("candle3 same");

   gPad->BuildLegend(0.78,0.695,0.980,0.935,"","f");
}
<commit_msg>[tutorials] No need to heap-allocate TDatime-s, use gRandom. Thanks, Maarten!<commit_after>/// \file
/// \ingroup tutorial_hist
/// \notebook
/// Example of candle plot with 2-D histograms.
///
/// \macro_image
/// \macro_code
///
/// \author Georg Troska

void candleplot() {

   gStyle->SetTimeOffset(0);
   TDatime dateBegin(2010,1,1,0,0,0);
   TDatime dateEnd(2011,1,1,0,0,0);

   auto h1 = new TH2I("h1","Machine A + B",12,dateBegin.Convert(),dateEnd.Convert(),1000,0,1000);
   auto h2 = new TH2I("h2","Machine B",12,dateBegin.Convert(),dateEnd.Convert(),1000,0,1000);

   h1->GetXaxis()->SetTimeDisplay(1);
   h1->GetXaxis()->SetTimeFormat("%m/%y");
   h1->GetXaxis()->SetTitle("Date [month/year]");

   float Rand;
   for (int i = dateBegin.Convert(); i < dateEnd.Convert(); i+=86400*30) {
      for (int j = 0; j < 1000; j++) {
         Rand = gRandom->Gaus(500+sin(i/10000000.)*100,50); h1->Fill(i,Rand);
         Rand = gRandom->Gaus(500+sin(i/11000000.)*100,70); h2->Fill(i,Rand);
      }
   }

   h1->SetBarWidth(0.4);
   h1->SetBarOffset(-0.25);
   h1->SetFillColor(kYellow);
   h1->SetFillStyle(1001);

   h2->SetBarWidth(0.4);
   h2->SetBarOffset(0.25);
   h2->SetLineColor(kRed);
   h2->SetFillColor(kGreen);

   auto c1 = new TCanvas();

   h1->Draw("candle2");
   h2->Draw("candle3 same");

   gPad->BuildLegend(0.78,0.695,0.980,0.935,"","f");
}
<|endoftext|>


<commit_before>// Copyright 2013 The Flutter Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "flutter/shell/platform/android/android_image_generator.h"

#include <android/bitmap.h>
#include <android/hardware_buffer.h>

#include "flutter/fml/platform/android/jni_util.h"

namespace flutter {

static fml::jni::ScopedJavaGlobalRef<jclass>* g_flutter_jni_class = nullptr;
static jmethodID g_decode_image_method = nullptr;

AndroidImageGenerator::~AndroidImageGenerator() = default;

AndroidImageGenerator::AndroidImageGenerator(sk_sp<SkData> data)
    : data_(data), image_info_(SkImageInfo::MakeUnknown(-1, -1)) {}

const SkImageInfo& AndroidImageGenerator::GetInfo() {
  header_decoded_latch_.Wait();
  return image_info_;
}

unsigned int AndroidImageGenerator::GetFrameCount() const {
  return 1;
}

unsigned int AndroidImageGenerator::GetPlayCount() const {
  return 1;
}

const ImageGenerator::FrameInfo AndroidImageGenerator::GetFrameInfo(
    unsigned int frame_index) const {
  return {.required_frame = std::nullopt,
          .duration = 0,
          .disposal_method = SkCodecAnimation::DisposalMethod::kKeep};
}

SkISize AndroidImageGenerator::GetScaledDimensions(float desired_scale) {
  return GetInfo().dimensions();
}

bool AndroidImageGenerator::GetPixels(const SkImageInfo& info,
                                      void* pixels,
                                      size_t row_bytes,
                                      unsigned int frame_index,
                                      std::optional<unsigned int> prior_frame) {
  fully_decoded_latch_.Wait();

  if (kRGBA_8888_SkColorType != info.colorType()) {
    return false;
  }

  switch (info.alphaType()) {
    case kOpaque_SkAlphaType:
      if (kOpaque_SkAlphaType != GetInfo().alphaType()) {
        return false;
      }
      break;
    case kPremul_SkAlphaType:
      break;
    default:
      return false;
  }

  // TODO(bdero): Override `GetImage()` to use `SkImage::FromAHardwareBuffer` on
  // API level 30+ once it's updated to do symbol lookups and not get
  // preprocessed out in Skia. This will allow for avoiding this copy in
  // cases where the result image doesn't need to be resized.
  memcpy(pixels, software_decoded_data_->data(),
         software_decoded_data_->size());
  return true;
}

void AndroidImageGenerator::DecodeImage() {
  DoDecodeImage();

  header_decoded_latch_.Signal();
  fully_decoded_latch_.Signal();
}

void AndroidImageGenerator::DoDecodeImage() {
  FML_DCHECK(g_flutter_jni_class);
  FML_DCHECK(g_decode_image_method);

  // Call FlutterJNI.decodeImage

  JNIEnv* env = fml::jni::AttachCurrentThread();

  fml::jni::ScopedJavaLocalRef<jobject> direct_buffer(
      env, env->NewDirectByteBuffer(const_cast<void*>(data_->data()),
                                    data_->size()));

  fml::jni::ScopedJavaGlobalRef<jobject>* bitmap =
      new fml::jni::ScopedJavaGlobalRef(
          env, env->CallStaticObjectMethod(
                   g_flutter_jni_class->obj(), g_decode_image_method,
                   direct_buffer.obj(), reinterpret_cast<long>(this)));
  FML_CHECK(fml::jni::CheckException(env));

  if (bitmap->is_null()) {
    return;
  }

  AndroidBitmapInfo info;
  [[maybe_unused]] int status;
  if ((status = AndroidBitmap_getInfo(env, bitmap->obj(), &info)) < 0) {
    FML_DLOG(ERROR) << "Failed to get bitmap info, status=" << status;
    return;
  }
  FML_DCHECK(info.format == ANDROID_BITMAP_FORMAT_RGBA_8888);

  // Lock the android buffer in a shared pointer

  void* pixel_lock;
  if ((status = AndroidBitmap_lockPixels(env, bitmap->obj(), &pixel_lock)) <
      0) {
    FML_DLOG(ERROR) << "Failed to lock pixels, error=" << status;
    return;
  }

  SkData::ReleaseProc on_release = [](const void* ptr, void* context) -> void {
    fml::jni::ScopedJavaGlobalRef<jobject>* bitmap =
        reinterpret_cast<fml::jni::ScopedJavaGlobalRef<jobject>*>(context);
    auto env = fml::jni::AttachCurrentThread();
    AndroidBitmap_unlockPixels(env, bitmap->obj());
  };

  software_decoded_data_ = SkData::MakeWithProc(
      pixel_lock, info.width * info.height * sizeof(uint32_t), on_release,
      bitmap);
}

bool AndroidImageGenerator::Register(JNIEnv* env) {
  g_flutter_jni_class = new fml::jni::ScopedJavaGlobalRef<jclass>(
      env, env->FindClass("io/flutter/embedding/engine/FlutterJNI"));
  FML_DCHECK(!g_flutter_jni_class->is_null());

  g_decode_image_method = env->GetStaticMethodID(
      g_flutter_jni_class->obj(), "decodeImage",
      "(Ljava/nio/ByteBuffer;J)Landroid/graphics/Bitmap;");
  FML_DCHECK(g_decode_image_method);

  static const JNINativeMethod header_decoded_method = {
      .name = "nativeImageHeaderCallback",
      .signature = "(JII)V",
      .fnPtr = reinterpret_cast<void*>(
          &AndroidImageGenerator::NativeImageHeaderCallback),
  };
  if (env->RegisterNatives(g_flutter_jni_class->obj(), &header_decoded_method,
                           1) != 0) {
    FML_LOG(ERROR)
        << "Failed to register FlutterJNI.nativeImageHeaderCallback method";
    return false;
  }

  return true;
}

std::shared_ptr<ImageGenerator> AndroidImageGenerator::MakeFromData(
    sk_sp<SkData> data,
    fml::RefPtr<fml::TaskRunner> task_runner) {
  std::shared_ptr<AndroidImageGenerator> generator(
      new AndroidImageGenerator(std::move(data)));

  fml::TaskRunner::RunNowOrPostTask(
      task_runner, [generator]() { generator->DecodeImage(); });

  if (generator->IsValidImageData()) {
    return generator;
  }

  return nullptr;
}

void AndroidImageGenerator::NativeImageHeaderCallback(JNIEnv* env,
                                                      jclass jcaller,
                                                      jlong generator_address,
                                                      int width,
                                                      int height) {
  AndroidImageGenerator* generator =
      reinterpret_cast<AndroidImageGenerator*>(generator_address);

  generator->image_info_ = SkImageInfo::Make(
      width, height, kRGBA_8888_SkColorType, kPremul_SkAlphaType);
  generator->header_decoded_latch_.Signal();
}

bool AndroidImageGenerator::IsValidImageData() {
  // The generator kicks off an IO task to decode everything, and calls to
  // "GetInfo()" block until either the header has been decoded or decoding has
  // failed, whichever is sooner. The decoder is initialized with a width and
  // height of -1 and will update the dimensions if the image is able to be
  // decoded.
  return GetInfo().height() != -1;
}

}  // namespace flutter
<commit_msg>[android] Fix leak of bitmap object (android.graphics.Bitmap of Java) while using platform image decoder. (#33953)<commit_after>// Copyright 2013 The Flutter Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "flutter/shell/platform/android/android_image_generator.h"

#include <memory>

#include <android/bitmap.h>
#include <android/hardware_buffer.h>

#include "flutter/fml/platform/android/jni_util.h"

namespace flutter {

static fml::jni::ScopedJavaGlobalRef<jclass>* g_flutter_jni_class = nullptr;
static jmethodID g_decode_image_method = nullptr;

AndroidImageGenerator::~AndroidImageGenerator() = default;

AndroidImageGenerator::AndroidImageGenerator(sk_sp<SkData> data)
    : data_(data), image_info_(SkImageInfo::MakeUnknown(-1, -1)) {}

const SkImageInfo& AndroidImageGenerator::GetInfo() {
  header_decoded_latch_.Wait();
  return image_info_;
}

unsigned int AndroidImageGenerator::GetFrameCount() const {
  return 1;
}

unsigned int AndroidImageGenerator::GetPlayCount() const {
  return 1;
}

const ImageGenerator::FrameInfo AndroidImageGenerator::GetFrameInfo(
    unsigned int frame_index) const {
  return {.required_frame = std::nullopt,
          .duration = 0,
          .disposal_method = SkCodecAnimation::DisposalMethod::kKeep};
}

SkISize AndroidImageGenerator::GetScaledDimensions(float desired_scale) {
  return GetInfo().dimensions();
}

bool AndroidImageGenerator::GetPixels(const SkImageInfo& info,
                                      void* pixels,
                                      size_t row_bytes,
                                      unsigned int frame_index,
                                      std::optional<unsigned int> prior_frame) {
  fully_decoded_latch_.Wait();

  if (kRGBA_8888_SkColorType != info.colorType()) {
    return false;
  }

  switch (info.alphaType()) {
    case kOpaque_SkAlphaType:
      if (kOpaque_SkAlphaType != GetInfo().alphaType()) {
        return false;
      }
      break;
    case kPremul_SkAlphaType:
      break;
    default:
      return false;
  }

  // TODO(bdero): Override `GetImage()` to use `SkImage::FromAHardwareBuffer` on
  // API level 30+ once it's updated to do symbol lookups and not get
  // preprocessed out in Skia. This will allow for avoiding this copy in
  // cases where the result image doesn't need to be resized.
  memcpy(pixels, software_decoded_data_->data(),
         software_decoded_data_->size());
  return true;
}

void AndroidImageGenerator::DecodeImage() {
  DoDecodeImage();

  header_decoded_latch_.Signal();
  fully_decoded_latch_.Signal();
}

void AndroidImageGenerator::DoDecodeImage() {
  FML_DCHECK(g_flutter_jni_class);
  FML_DCHECK(g_decode_image_method);

  // Call FlutterJNI.decodeImage

  JNIEnv* env = fml::jni::AttachCurrentThread();

  fml::jni::ScopedJavaLocalRef<jobject> direct_buffer(
      env, env->NewDirectByteBuffer(const_cast<void*>(data_->data()),
                                    data_->size()));

  auto bitmap = std::make_unique<fml::jni::ScopedJavaGlobalRef<jobject>>(
      env, env->CallStaticObjectMethod(
               g_flutter_jni_class->obj(), g_decode_image_method,
               direct_buffer.obj(), reinterpret_cast<long>(this)));
  FML_CHECK(fml::jni::CheckException(env));

  if (bitmap->is_null()) {
    return;
  }

  AndroidBitmapInfo info;
  [[maybe_unused]] int status;
  if ((status = AndroidBitmap_getInfo(env, bitmap->obj(), &info)) < 0) {
    FML_DLOG(ERROR) << "Failed to get bitmap info, status=" << status;
    return;
  }
  FML_DCHECK(info.format == ANDROID_BITMAP_FORMAT_RGBA_8888);

  // Lock the android buffer in a shared pointer

  void* pixel_lock;
  if ((status = AndroidBitmap_lockPixels(env, bitmap->obj(), &pixel_lock)) <
      0) {
    FML_DLOG(ERROR) << "Failed to lock pixels, error=" << status;
    return;
  }

  SkData::ReleaseProc on_release = [](const void* ptr, void* context) -> void {
    fml::jni::ScopedJavaGlobalRef<jobject>* bitmap =
        reinterpret_cast<fml::jni::ScopedJavaGlobalRef<jobject>*>(context);
    auto env = fml::jni::AttachCurrentThread();
    AndroidBitmap_unlockPixels(env, bitmap->obj());
    delete bitmap;
  };

  software_decoded_data_ = SkData::MakeWithProc(
      pixel_lock, info.width * info.height * sizeof(uint32_t), on_release,
      bitmap.release());
}

bool AndroidImageGenerator::Register(JNIEnv* env) {
  g_flutter_jni_class = new fml::jni::ScopedJavaGlobalRef<jclass>(
      env, env->FindClass("io/flutter/embedding/engine/FlutterJNI"));
  FML_DCHECK(!g_flutter_jni_class->is_null());

  g_decode_image_method = env->GetStaticMethodID(
      g_flutter_jni_class->obj(), "decodeImage",
      "(Ljava/nio/ByteBuffer;J)Landroid/graphics/Bitmap;");
  FML_DCHECK(g_decode_image_method);

  static const JNINativeMethod header_decoded_method = {
      .name = "nativeImageHeaderCallback",
      .signature = "(JII)V",
      .fnPtr = reinterpret_cast<void*>(
          &AndroidImageGenerator::NativeImageHeaderCallback),
  };
  if (env->RegisterNatives(g_flutter_jni_class->obj(), &header_decoded_method,
                           1) != 0) {
    FML_LOG(ERROR)
        << "Failed to register FlutterJNI.nativeImageHeaderCallback method";
    return false;
  }

  return true;
}

std::shared_ptr<ImageGenerator> AndroidImageGenerator::MakeFromData(
    sk_sp<SkData> data,
    fml::RefPtr<fml::TaskRunner> task_runner) {
  std::shared_ptr<AndroidImageGenerator> generator(
      new AndroidImageGenerator(std::move(data)));

  fml::TaskRunner::RunNowOrPostTask(
      task_runner, [generator]() { generator->DecodeImage(); });

  if (generator->IsValidImageData()) {
    return generator;
  }

  return nullptr;
}

void AndroidImageGenerator::NativeImageHeaderCallback(JNIEnv* env,
                                                      jclass jcaller,
                                                      jlong generator_address,
                                                      int width,
                                                      int height) {
  AndroidImageGenerator* generator =
      reinterpret_cast<AndroidImageGenerator*>(generator_address);

  generator->image_info_ = SkImageInfo::Make(
      width, height, kRGBA_8888_SkColorType, kPremul_SkAlphaType);
  generator->header_decoded_latch_.Signal();
}

bool AndroidImageGenerator::IsValidImageData() {
  // The generator kicks off an IO task to decode everything, and calls to
  // "GetInfo()" block until either the header has been decoded or decoding has
  // failed, whichever is sooner. The decoder is initialized with a width and
  // height of -1 and will update the dimensions if the image is able to be
  // decoded.
  return GetInfo().height() != -1;
}

}  // namespace flutter
<|endoftext|>