<commit_before>/*
 * Copyright (c) 2003-2005 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Nathan Binkert
 */

#include <fstream>
#include <iomanip>
#include <list>
#include <map>
#include <string>

#include "base/callback.hh"
#include "base/cprintf.hh"
#include "base/debug.hh"
#include "base/hostinfo.hh"
#include "base/misc.hh"
#include "base/statistics.hh"
#include "base/str.hh"
#include "base/time.hh"
#include "base/trace.hh"

using namespace std;

namespace Stats {

std::string Info::separatorString = "::";
typedef map<const void *, Info *> MapType;

// We wrap these in a function to make sure they're built in time.
list<Info *> &
statsList()
{
    static list<Info *> the_list;
    return the_list;
}

MapType &
statsMap()
{
    static MapType the_map;
    return the_map;
}

void
InfoAccess::setInfo(Info *info)
{
    if (statsMap().find(this) != statsMap().end())
        panic("shouldn't register stat twice!");

    statsList().push_back(info);

#ifndef NDEBUG
    pair<MapType::iterator, bool> result =
#endif
        statsMap().insert(make_pair(this, info));
    assert(result.second && "this should never fail");
    assert(statsMap().find(this) != statsMap().end());
}

void
InfoAccess::setParams(const StorageParams *params)
{
    info()->storageParams = params;
}

void
InfoAccess::setInit()
{
    info()->flags.set(init);
}

Info *
InfoAccess::info()
{
    MapType::const_iterator i = statsMap().find(this);
    assert(i != statsMap().end());
    return (*i).second;
}

const Info *
InfoAccess::info() const
{
    MapType::const_iterator i = statsMap().find(this);
    assert(i != statsMap().end());
    return (*i).second;
}

StorageParams::~StorageParams()
{
}

typedef map<std::string, Info *> NameMapType;
NameMapType &
nameMap()
{
    static NameMapType the_map;
    return the_map;
}

int Info::id_count = 0;

int debug_break_id = -1;

Info::Info()
    : flags(none), precision(-1), prereq(0), storageParams(NULL)
{
    id = id_count++;
    if (debug_break_id >= 0 and debug_break_id == id)
        Debug::breakpoint();
}

Info::~Info()
{
}

void
Info::setName(const string &name)
{
    pair<NameMapType::iterator, bool> p =
        nameMap().insert(make_pair(name, this));

    Info *other = p.first->second;
    bool result = p.second;

    if (!result) {
        // using other->name instead of just name to avoid a compiler
        // warning.  They should be the same.
        panic("same statistic name used twice! name=%s\n", other->name);
    }

    this->name = name;
}

bool
Info::less(Info *stat1, Info *stat2)
{
    const string &name1 = stat1->name;
    const string &name2 = stat2->name;

    vector<string> v1;
    vector<string> v2;

    tokenize(v1, name1, '.');
    tokenize(v2, name2, '.');

    size_type last = min(v1.size(), v2.size()) - 1;
    for (off_type i = 0; i < last; ++i)
        if (v1[i] != v2[i])
            return v1[i] < v2[i];

    // Special compare for last element.
    if (v1[last] == v2[last])
        return v1.size() < v2.size();
    else
        return v1[last] < v2[last];

    return false;
}

bool
Info::baseCheck() const
{
    if (!(flags & Stats::init)) {
#ifdef DEBUG
        cprintf("this is stat number %d\n", id);
#endif
        panic("Not all stats have been initialized");
        return false;
    }

    if ((flags & display) && name.empty()) {
        panic("all printable stats must be named");
        return false;
    }

    return true;
}

void
Info::enable()
{
}

void
VectorInfo::enable()
{
    size_type s = size();
    if (subnames.size() < s)
        subnames.resize(s);
    if (subdescs.size() < s)
        subdescs.resize(s);
}

void
VectorDistInfo::enable()
{
    size_type s = size();
    if (subnames.size() < s)
        subnames.resize(s);
    if (subdescs.size() < s)
        subdescs.resize(s);
}

void
Vector2dInfo::enable()
{
    if (subnames.size() < x)
        subnames.resize(x);
    if (subdescs.size() < x)
        subdescs.resize(x);
    if (y_subnames.size() < y)
        y_subnames.resize(y);
}

void
HistStor::grow_out()
{
    int size = cvec.size();
    int zero = size / 2; // round down!
    int top_half = zero + (size - zero + 1) / 2; // round up!
    int bottom_half = (size - zero) / 2; // round down!

    // grow down
    int low_pair = zero - 1;
    for (int i = zero - 1; i >= bottom_half; i--) {
        cvec[i] = cvec[low_pair];
        if (low_pair - 1 >= 0)
            cvec[i] += cvec[low_pair - 1];
        low_pair -= 2;
    }
    assert(low_pair == 0 || low_pair == -1 || low_pair == -2);

    for (int i = bottom_half - 1; i >= 0; i--)
        cvec[i] = Counter();

    // grow up
    int high_pair = zero;
    for (int i = zero; i < top_half; i++) {
        cvec[i] = cvec[high_pair];
        if (high_pair + 1 < size)
            cvec[i] += cvec[high_pair + 1];
        high_pair += 2;
    }
    assert(high_pair == size || high_pair == size + 1);

    for (int i = top_half; i < size; i++)
        cvec[i] = Counter();

    max_bucket *= 2;
    min_bucket *= 2;
    bucket_size *= 2;
}

void
HistStor::grow_convert()
{
    int size = cvec.size();
    int half = (size + 1) / 2; // round up!
    //bool even = (size & 1) == 0;

    int pair = size - 1;
    for (int i = size - 1; i >= half; --i) {
        cvec[i] = cvec[pair];
        if (pair - 1 >= 0)
            cvec[i] += cvec[pair - 1];
        pair -= 2;
    }

    for (int i = half - 1; i >= 0; i--)
        cvec[i] = Counter();

    min_bucket = -max_bucket;// - (even ? bucket_size : 0);
    bucket_size *= 2;
}

void
HistStor::grow_up()
{
    int size = cvec.size();
    int half = (size + 1) / 2; // round up!

    int pair = 0;
    for (int i = 0; i < half; i++) {
        cvec[i] = cvec[pair];
        if (pair + 1 < size)
            cvec[i] += cvec[pair + 1];
        pair += 2;
    }
    assert(pair == size || pair == size + 1);

    for (int i = half; i < size; i++)
        cvec[i] = Counter();

    max_bucket *= 2;
    bucket_size *= 2;
}

Formula::Formula()
{
}

Formula::Formula(Temp r)
{
    root = r;
    setInit();
    assert(size());
}

const Formula &
Formula::operator=(Temp r)
{
    assert(!root && "Can't change formulas");
    root = r;
    setInit();
    assert(size());
    return *this;
}

const Formula &
Formula::operator+=(Temp r)
{
    if (root)
        root = NodePtr(new BinaryNode<std::plus<Result> >(root, r));
    else {
        root = r;
        setInit();
    }

    assert(size());
    return *this;
}

void
Formula::result(VResult &vec) const
{
    if (root)
        vec = root->result();
}

Result
Formula::total() const
{
    return root ? root->total() : 0.0;
}

size_type
Formula::size() const
{
    if (!root)
        return 0;
    else
        return root->size();
}

void
Formula::reset()
{
}

bool
Formula::zero() const
{
    VResult vec;
    result(vec);
    for (VResult::size_type i = 0; i < vec.size(); ++i)
        if (vec[i] != 0.0)
            return false;
    return true;
}

string
Formula::str() const
{
    return root ? root->str() : "";
}

void
enable()
{
    typedef list<Info *>::iterator iter_t;

    iter_t i, end = statsList().end();
    for (i = statsList().begin(); i != end; ++i) {
        Info *info = *i;
        assert(info);
        if (!info->check() || !info->baseCheck())
            panic("stat check failed for '%s' %d\n", info->name, info->id);
    }

    off_t j = 0;
    for (i = statsList().begin(); i != end; ++i) {
        Info *info = *i;
        if (!(info->flags & display))
            info->name = "__Stat" + to_string(j++);
    }

    statsList().sort(Info::less);

    for (i = statsList().begin(); i != end; ++i) {
        Info *info = *i;
        info->enable();
    }
}

void
prepare()
{
    list<Info *>::iterator i = statsList().begin();
    list<Info *>::iterator end = statsList().end();
    while (i != end) {
        Info *info = *i;
        info->prepare();
        ++i;
    }
}

CallbackQueue resetQueue;

void
reset()
{
    list<Info *>::iterator i = statsList().begin();
    list<Info *>::iterator end = statsList().end();
    while (i != end) {
        Info *info = *i;
        info->reset();
        ++i;
    }

    resetQueue.process();
}

void
registerResetCallback(Callback *cb)
{
    resetQueue.add(cb);
}

} // namespace Stats
<commit_msg>stats: ensure that stat names are valid<commit_after>/*
 * Copyright (c) 2003-2005 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Nathan Binkert
 */

#include <fstream>
#include <iomanip>
#include <list>
#include <map>
#include <string>

#include "base/callback.hh"
#include "base/cprintf.hh"
#include "base/debug.hh"
#include "base/hostinfo.hh"
#include "base/misc.hh"
#include "base/statistics.hh"
#include "base/str.hh"
#include "base/time.hh"
#include "base/trace.hh"

using namespace std;

namespace Stats {

std::string Info::separatorString = "::";
typedef map<const void *, Info *> MapType;

// We wrap these in a function to make sure they're built in time.
list<Info *> &
statsList()
{
    static list<Info *> the_list;
    return the_list;
}

MapType &
statsMap()
{
    static MapType the_map;
    return the_map;
}

void
InfoAccess::setInfo(Info *info)
{
    if (statsMap().find(this) != statsMap().end())
        panic("shouldn't register stat twice!");

    statsList().push_back(info);

#ifndef NDEBUG
    pair<MapType::iterator, bool> result =
#endif
        statsMap().insert(make_pair(this, info));
    assert(result.second && "this should never fail");
    assert(statsMap().find(this) != statsMap().end());
}

void
InfoAccess::setParams(const StorageParams *params)
{
    info()->storageParams = params;
}

void
InfoAccess::setInit()
{
    info()->flags.set(init);
}

Info *
InfoAccess::info()
{
    MapType::const_iterator i = statsMap().find(this);
    assert(i != statsMap().end());
    return (*i).second;
}

const Info *
InfoAccess::info() const
{
    MapType::const_iterator i = statsMap().find(this);
    assert(i != statsMap().end());
    return (*i).second;
}

StorageParams::~StorageParams()
{
}

typedef map<std::string, Info *> NameMapType;
NameMapType &
nameMap()
{
    static NameMapType the_map;
    return the_map;
}

int Info::id_count = 0;

int debug_break_id = -1;

Info::Info()
    : flags(none), precision(-1), prereq(0), storageParams(NULL)
{
    id = id_count++;
    if (debug_break_id >= 0 and debug_break_id == id)
        Debug::breakpoint();
}

Info::~Info()
{
}

bool
validateStatName(const string &name)
{
    if (name.empty())
        return false;

    vector<string> vec;
    tokenize(vec, name, '.');
    vector<string>::const_iterator item = vec.begin();
    while (item != vec.end()) {
        if (item->empty())
            return false;

        string::const_iterator c = item->begin();

        // The first character is different
        if (!isalpha(*c) && *c != '_')
            return false;

        // The rest of the characters have different rules.
        while (++c != item->end()) {
            if (!isalnum(*c) && *c != '_')
                return false;
        }

        ++item;
    }

    return true;
}

void
Info::setName(const string &name)
{
    if (!validateStatName(name))
        panic("invalid stat name '%s'", name);

    pair<NameMapType::iterator, bool> p =
        nameMap().insert(make_pair(name, this));

    Info *other = p.first->second;
    bool result = p.second;

    if (!result) {
        // using other->name instead of just name to avoid a compiler
        // warning.  They should be the same.
        panic("same statistic name used twice! name=%s\n", other->name);
    }

    this->name = name;
}

bool
Info::less(Info *stat1, Info *stat2)
{
    const string &name1 = stat1->name;
    const string &name2 = stat2->name;

    vector<string> v1;
    vector<string> v2;

    tokenize(v1, name1, '.');
    tokenize(v2, name2, '.');

    size_type last = min(v1.size(), v2.size()) - 1;
    for (off_type i = 0; i < last; ++i)
        if (v1[i] != v2[i])
            return v1[i] < v2[i];

    // Special compare for last element.
    if (v1[last] == v2[last])
        return v1.size() < v2.size();
    else
        return v1[last] < v2[last];

    return false;
}

bool
Info::baseCheck() const
{
    if (!(flags & Stats::init)) {
#ifdef DEBUG
        cprintf("this is stat number %d\n", id);
#endif
        panic("Not all stats have been initialized");
        return false;
    }

    if ((flags & display) && name.empty()) {
        panic("all printable stats must be named");
        return false;
    }

    return true;
}

void
Info::enable()
{
}

void
VectorInfo::enable()
{
    size_type s = size();
    if (subnames.size() < s)
        subnames.resize(s);
    if (subdescs.size() < s)
        subdescs.resize(s);
}

void
VectorDistInfo::enable()
{
    size_type s = size();
    if (subnames.size() < s)
        subnames.resize(s);
    if (subdescs.size() < s)
        subdescs.resize(s);
}

void
Vector2dInfo::enable()
{
    if (subnames.size() < x)
        subnames.resize(x);
    if (subdescs.size() < x)
        subdescs.resize(x);
    if (y_subnames.size() < y)
        y_subnames.resize(y);
}

void
HistStor::grow_out()
{
    int size = cvec.size();
    int zero = size / 2; // round down!
    int top_half = zero + (size - zero + 1) / 2; // round up!
    int bottom_half = (size - zero) / 2; // round down!

    // grow down
    int low_pair = zero - 1;
    for (int i = zero - 1; i >= bottom_half; i--) {
        cvec[i] = cvec[low_pair];
        if (low_pair - 1 >= 0)
            cvec[i] += cvec[low_pair - 1];
        low_pair -= 2;
    }
    assert(low_pair == 0 || low_pair == -1 || low_pair == -2);

    for (int i = bottom_half - 1; i >= 0; i--)
        cvec[i] = Counter();

    // grow up
    int high_pair = zero;
    for (int i = zero; i < top_half; i++) {
        cvec[i] = cvec[high_pair];
        if (high_pair + 1 < size)
            cvec[i] += cvec[high_pair + 1];
        high_pair += 2;
    }
    assert(high_pair == size || high_pair == size + 1);

    for (int i = top_half; i < size; i++)
        cvec[i] = Counter();

    max_bucket *= 2;
    min_bucket *= 2;
    bucket_size *= 2;
}

void
HistStor::grow_convert()
{
    int size = cvec.size();
    int half = (size + 1) / 2; // round up!
    //bool even = (size & 1) == 0;

    int pair = size - 1;
    for (int i = size - 1; i >= half; --i) {
        cvec[i] = cvec[pair];
        if (pair - 1 >= 0)
            cvec[i] += cvec[pair - 1];
        pair -= 2;
    }

    for (int i = half - 1; i >= 0; i--)
        cvec[i] = Counter();

    min_bucket = -max_bucket;// - (even ? bucket_size : 0);
    bucket_size *= 2;
}

void
HistStor::grow_up()
{
    int size = cvec.size();
    int half = (size + 1) / 2; // round up!

    int pair = 0;
    for (int i = 0; i < half; i++) {
        cvec[i] = cvec[pair];
        if (pair + 1 < size)
            cvec[i] += cvec[pair + 1];
        pair += 2;
    }
    assert(pair == size || pair == size + 1);

    for (int i = half; i < size; i++)
        cvec[i] = Counter();

    max_bucket *= 2;
    bucket_size *= 2;
}

Formula::Formula()
{
}

Formula::Formula(Temp r)
{
    root = r;
    setInit();
    assert(size());
}

const Formula &
Formula::operator=(Temp r)
{
    assert(!root && "Can't change formulas");
    root = r;
    setInit();
    assert(size());
    return *this;
}

const Formula &
Formula::operator+=(Temp r)
{
    if (root)
        root = NodePtr(new BinaryNode<std::plus<Result> >(root, r));
    else {
        root = r;
        setInit();
    }

    assert(size());
    return *this;
}

void
Formula::result(VResult &vec) const
{
    if (root)
        vec = root->result();
}

Result
Formula::total() const
{
    return root ? root->total() : 0.0;
}

size_type
Formula::size() const
{
    if (!root)
        return 0;
    else
        return root->size();
}

void
Formula::reset()
{
}

bool
Formula::zero() const
{
    VResult vec;
    result(vec);
    for (VResult::size_type i = 0; i < vec.size(); ++i)
        if (vec[i] != 0.0)
            return false;
    return true;
}

string
Formula::str() const
{
    return root ? root->str() : "";
}

void
enable()
{
    typedef list<Info *>::iterator iter_t;

    iter_t i, end = statsList().end();
    for (i = statsList().begin(); i != end; ++i) {
        Info *info = *i;
        assert(info);
        if (!info->check() || !info->baseCheck())
            panic("stat check failed for '%s' %d\n", info->name, info->id);
    }

    off_t j = 0;
    for (i = statsList().begin(); i != end; ++i) {
        Info *info = *i;
        if (!(info->flags & display))
            info->name = "__Stat" + to_string(j++);
    }

    statsList().sort(Info::less);

    for (i = statsList().begin(); i != end; ++i) {
        Info *info = *i;
        info->enable();
    }
}

void
prepare()
{
    list<Info *>::iterator i = statsList().begin();
    list<Info *>::iterator end = statsList().end();
    while (i != end) {
        Info *info = *i;
        info->prepare();
        ++i;
    }
}

CallbackQueue resetQueue;

void
reset()
{
    list<Info *>::iterator i = statsList().begin();
    list<Info *>::iterator end = statsList().end();
    while (i != end) {
        Info *info = *i;
        info->reset();
        ++i;
    }

    resetQueue.process();
}

void
registerResetCallback(Callback *cb)
{
    resetQueue.add(cb);
}

} // namespace Stats
<|endoftext|>


<commit_before>#define WIN32_LEAN_AND_MEAN
#define STRICT_GS_ENABLED
#define _ATL_NO_AUTOMATIC_NAMESPACE
#define _ATL_NO_DEFAULT_LIBS
#define _ATL_NO_WIN_SUPPORT
#define _CRTDBG_MAP_ALLOC
#include <atlbase.h>
#include <windows.h>
#include <winioctl.h>
#include <algorithm>
#include "reflink.h"
#include <crtdbg.h>

constexpr LONG64 inline ROUNDUP(LONG64 file_size, ULONG cluster_size) noexcept
{
	return (file_size + cluster_size - 1) / cluster_size * cluster_size;
}
static_assert(ROUNDUP(5678, 4 * 1024) == 8 * 1024);
_Success_(return == true)
bool reflink(_In_z_ PCWSTR oldpath, _In_z_ PCWSTR newpath)
{
	ATL::CHandle source(CreateFileW(oldpath, GENERIC_READ, FILE_SHARE_READ, nullptr, OPEN_EXISTING, 0, nullptr));
	if (source == INVALID_HANDLE_VALUE)
	{
		source.Detach();
		return false;
	}

	ULONG fs_flags;
	if (!GetVolumeInformationByHandleW(source, nullptr, 0, nullptr, nullptr, &fs_flags, nullptr, 0))
	{
		return false;
	}
	if (!(fs_flags & FILE_SUPPORTS_BLOCK_REFCOUNTING))
	{
		SetLastError(ERROR_NOT_CAPABLE);
		return false;
	}

	FILE_END_OF_FILE_INFO file_size;
	if (!GetFileSizeEx(source, &file_size.EndOfFile))
	{
		return false;
	}
	FILE_BASIC_INFO file_basic;
	if (!GetFileInformationByHandleEx(source, FileBasicInfo, &file_basic, sizeof file_basic))
	{
		return false;
	}
	ULONG junk;
	FSCTL_GET_INTEGRITY_INFORMATION_BUFFER get_integrity;
	if (!DeviceIoControl(source, FSCTL_GET_INTEGRITY_INFORMATION, nullptr, 0, &get_integrity, sizeof get_integrity, &junk, nullptr))
	{
		return false;
	}

#ifdef _DEBUG
	SetFileAttributesW(newpath, FILE_ATTRIBUTE_NORMAL);
	ATL::CHandle destination(CreateFileW(newpath, GENERIC_READ | GENERIC_WRITE | DELETE, 0, nullptr, CREATE_ALWAYS, 0, source));
#else
	ATL::CHandle destination(CreateFileW(newpath, GENERIC_READ | GENERIC_WRITE | DELETE, 0, nullptr, CREATE_NEW, 0, source));
#endif
	if (destination == INVALID_HANDLE_VALUE)
	{
		destination.Detach();
		return false;
	}
	FILE_DISPOSITION_INFO dispose = { TRUE };
	if (!SetFileInformationByHandle(destination, FileDispositionInfo, &dispose, sizeof dispose))
	{
		return false;
	}

	if (file_basic.FileAttributes & FILE_ATTRIBUTE_SPARSE_FILE)
	{
		if (!DeviceIoControl(destination, FSCTL_SET_SPARSE, nullptr, 0, nullptr, 0, &junk, nullptr))
		{
			return false;
		}
	}
	FSCTL_SET_INTEGRITY_INFORMATION_BUFFER set_integrity = { get_integrity.ChecksumAlgorithm, get_integrity.Reserved, get_integrity.Flags };
	if (!DeviceIoControl(destination, FSCTL_SET_INTEGRITY_INFORMATION, &set_integrity, sizeof set_integrity, nullptr, 0, nullptr, nullptr))
	{
		return false;
	}
	if (!SetFileInformationByHandle(destination, FileEndOfFileInfo, &file_size, sizeof file_size))
	{
		return false;
	}

	const LONG64 split_threshold = (1LL << 32) - get_integrity.ClusterSizeInBytes;

	DUPLICATE_EXTENTS_DATA dup_extent;
	dup_extent.FileHandle = source;
	for (LONG64 offset = 0, remain = ROUNDUP(file_size.EndOfFile.QuadPart, get_integrity.ClusterSizeInBytes); remain > 0; offset += split_threshold, remain -= split_threshold)
	{
		dup_extent.SourceFileOffset.QuadPart = dup_extent.TargetFileOffset.QuadPart = offset;
		dup_extent.ByteCount.QuadPart = (std::min)(split_threshold, remain);
		_ASSERTE(dup_extent.SourceFileOffset.QuadPart % get_integrity.ClusterSizeInBytes == 0);
		_ASSERTE(dup_extent.ByteCount.QuadPart % get_integrity.ClusterSizeInBytes == 0);
		_ASSERTE(dup_extent.ByteCount.QuadPart <= UINT32_MAX);
		_RPT3(_CRT_WARN, "Remain=%llx\nOffset=%llx\nLength=%llx\n\n", remain, dup_extent.SourceFileOffset.QuadPart, dup_extent.ByteCount.QuadPart);
		if (!DeviceIoControl(destination, FSCTL_DUPLICATE_EXTENTS_TO_FILE, &dup_extent, sizeof dup_extent, nullptr, 0, &junk, nullptr))
		{
			_CrtDbgBreak();
			return false;
		}
	}

	file_basic.CreationTime.QuadPart = 0;
	if (!SetFileInformationByHandle(destination, FileBasicInfo, &file_basic, sizeof file_basic))
	{
		return false;
	}
	if (!FlushFileBuffers(destination))
	{
		return false;
	}
	dispose = { FALSE };
	return !!SetFileInformationByHandle(destination, FileDispositionInfo, &dispose, sizeof dispose);
}<commit_msg>Don't grow file all at once<commit_after>#define WIN32_LEAN_AND_MEAN
#define STRICT_GS_ENABLED
#define _ATL_NO_AUTOMATIC_NAMESPACE
#define _ATL_NO_DEFAULT_LIBS
#define _ATL_NO_WIN_SUPPORT
#define _CRTDBG_MAP_ALLOC
#include <atlbase.h>
#include <windows.h>
#include <winioctl.h>
#include <algorithm>
#include "reflink.h"
#include <crtdbg.h>

constexpr LONG64 inline ROUNDUP(LONG64 file_size, ULONG cluster_size) noexcept
{
	return (file_size + cluster_size - 1) / cluster_size * cluster_size;
}
static_assert(ROUNDUP(5678, 4 * 1024) == 8 * 1024);
_Success_(return == true)
bool reflink(_In_z_ PCWSTR oldpath, _In_z_ PCWSTR newpath)
{
	ATL::CHandle source(CreateFileW(oldpath, GENERIC_READ, FILE_SHARE_READ, nullptr, OPEN_EXISTING, 0, nullptr));
	if (source == INVALID_HANDLE_VALUE)
	{
		source.Detach();
		return false;
	}

	ULONG fs_flags;
	if (!GetVolumeInformationByHandleW(source, nullptr, 0, nullptr, nullptr, &fs_flags, nullptr, 0))
	{
		return false;
	}
	if (!(fs_flags & FILE_SUPPORTS_BLOCK_REFCOUNTING))
	{
		SetLastError(ERROR_NOT_CAPABLE);
		return false;
	}

	FILE_END_OF_FILE_INFO file_size;
	if (!GetFileSizeEx(source, &file_size.EndOfFile))
	{
		return false;
	}
	FILE_BASIC_INFO file_basic;
	if (!GetFileInformationByHandleEx(source, FileBasicInfo, &file_basic, sizeof file_basic))
	{
		return false;
	}
	ULONG junk;
	FSCTL_GET_INTEGRITY_INFORMATION_BUFFER get_integrity;
	if (!DeviceIoControl(source, FSCTL_GET_INTEGRITY_INFORMATION, nullptr, 0, &get_integrity, sizeof get_integrity, &junk, nullptr))
	{
		return false;
	}

#ifdef _DEBUG
	SetFileAttributesW(newpath, FILE_ATTRIBUTE_NORMAL);
	ATL::CHandle destination(CreateFileW(newpath, GENERIC_READ | GENERIC_WRITE | DELETE, 0, nullptr, CREATE_ALWAYS, 0, source));
#else
	ATL::CHandle destination(CreateFileW(newpath, GENERIC_READ | GENERIC_WRITE | DELETE, 0, nullptr, CREATE_NEW, 0, source));
#endif
	if (destination == INVALID_HANDLE_VALUE)
	{
		destination.Detach();
		return false;
	}
	FILE_DISPOSITION_INFO dispose = { TRUE };
	if (!SetFileInformationByHandle(destination, FileDispositionInfo, &dispose, sizeof dispose))
	{
		return false;
	}

	if (!DeviceIoControl(destination, FSCTL_SET_SPARSE, nullptr, 0, nullptr, 0, &junk, nullptr))
	{
		return false;
	}
	FSCTL_SET_INTEGRITY_INFORMATION_BUFFER set_integrity = { get_integrity.ChecksumAlgorithm, get_integrity.Reserved, get_integrity.Flags };
	if (!DeviceIoControl(destination, FSCTL_SET_INTEGRITY_INFORMATION, &set_integrity, sizeof set_integrity, nullptr, 0, nullptr, nullptr))
	{
		return false;
	}
	if (!SetFileInformationByHandle(destination, FileEndOfFileInfo, &file_size, sizeof file_size))
	{
		return false;
	}

	const LONG64 split_threshold = (1LL << 32) - get_integrity.ClusterSizeInBytes;

	DUPLICATE_EXTENTS_DATA dup_extent;
	dup_extent.FileHandle = source;
	for (LONG64 offset = 0, remain = ROUNDUP(file_size.EndOfFile.QuadPart, get_integrity.ClusterSizeInBytes); remain > 0; offset += split_threshold, remain -= split_threshold)
	{
		dup_extent.SourceFileOffset.QuadPart = dup_extent.TargetFileOffset.QuadPart = offset;
		dup_extent.ByteCount.QuadPart = (std::min)(split_threshold, remain);
		_ASSERTE(dup_extent.SourceFileOffset.QuadPart % get_integrity.ClusterSizeInBytes == 0);
		_ASSERTE(dup_extent.ByteCount.QuadPart % get_integrity.ClusterSizeInBytes == 0);
		_ASSERTE(dup_extent.ByteCount.QuadPart <= UINT32_MAX);
		_RPT3(_CRT_WARN, "Remain=%llx\nOffset=%llx\nLength=%llx\n\n", remain, dup_extent.SourceFileOffset.QuadPart, dup_extent.ByteCount.QuadPart);
		if (!DeviceIoControl(destination, FSCTL_DUPLICATE_EXTENTS_TO_FILE, &dup_extent, sizeof dup_extent, nullptr, 0, &junk, nullptr))
		{
			_CrtDbgBreak();
			return false;
		}
	}

	if (!(file_basic.FileAttributes & FILE_ATTRIBUTE_SPARSE_FILE))
	{
		FILE_SET_SPARSE_BUFFER set_sparse = { FALSE };
		if (!DeviceIoControl(destination, FSCTL_SET_SPARSE, &set_sparse, sizeof set_sparse, nullptr, 0, &junk, nullptr))
		{
			return false;
		}
	}

	file_basic.CreationTime.QuadPart = 0;
	if (!SetFileInformationByHandle(destination, FileBasicInfo, &file_basic, sizeof file_basic))
	{
		return false;
	}
	if (!FlushFileBuffers(destination))
	{
		return false;
	}
	dispose = { FALSE };
	return !!SetFileInformationByHandle(destination, FileDispositionInfo, &dispose, sizeof dispose);
}<|endoftext|>


<commit_before>//---------------------------------------------------------------------------
/// \file   es/storage.hpp
/// \brief  The entity/component data store
//
// Copyright 2013, nocte@hippie.nu            Released under the MIT License.
//---------------------------------------------------------------------------
#pragma once

#include <algorithm>
#include <bitset>
#include <cassert>
#include <cstdint>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <vector>
#include <unordered_map>

#include "component.hpp"
#include "entity.hpp"
#include "traits.hpp"

namespace es {

/** A storage ties entities and components together.
 * Storage associates two other bits of data with every entity:
 * - A 64-bit mask that keeps track of which components are defined
 * - A vector of bytes, holding the actual data
 *
 * The vector of bytes tries to pack the component data as tightly as
 * possible.  It is really fast for plain old datatypes, but it also
 * handles nontrivial types safely.  It packs a virtual table and a
 * pointer to some heap space in the vector, and calls the constructor
 * and destructor as needed.
 */
class storage
{
    // It is assumed the first few components will be accessed the
    // most often.  We keep a cache of the first 12.
    static constexpr uint32_t cache_size = 12;
    static constexpr uint32_t cache_mask = (1 << cache_size) - 1;

    /** This data gets associated with every entity. */
    struct elem
    {
        /** Bitmask to keep track of which components are held in \a data. */
        std::bitset<64>     components;
        /** Component data for this entity. */
        std::vector<char>   data;
    };

    typedef component::placeholder placeholder;

    /** Data types that do not have a flat memory layout are kept in the
     ** elem::data buffer in a placeholder object. */
    template <typename t>
    class holder : public placeholder
    {
    public:
        holder () { }
        holder (t init) : held_(std::move(init)) { }

        const t& held() const { return held_; }
        t&       held()       { return held_; }

        placeholder* clone() const { return new holder<t>(held_); }

    private:
        t held_;
    };

    typedef std::unordered_map<uint32_t, elem> stor_impl;

public:
    typedef uint8_t     component_id;

    typedef stor_impl::iterator         iterator;
    typedef stor_impl::const_iterator   const_iterator;

public:
    /** Variable references are used by systems to access an entity's data.
     *  It keeps track of the elem::data buffer and the offset inside that
     *  buffer. */
    template <typename type>
    class var_ref
    {
        friend class storage;

        type& get()
        {
            if (is_flat<type>::value)
                return *reinterpret_cast<type*>(&*e_.data.begin() + offset_);

            auto ptr (reinterpret_cast<holder<type>*>(&*e_.data.begin() + offset_));
            return ptr->held();
        }

    public:
        operator const type& () const
        {
            if (is_flat<type>::value)
                return *reinterpret_cast<const type*>(&*e_.data.begin() + offset_);

            auto ptr (reinterpret_cast<const holder<type>*>(&*e_.data.begin() + offset_));
            return ptr->held();
        }

        var_ref& operator= (type assign)
        {
            if (is_flat<type>::value)
            {
                new (&*e_.data.begin() + offset_) type(std::move(assign));
            }
            else
            {
                auto ptr (reinterpret_cast<holder<type>*>(&*e_.data.begin() + offset_));
                new (ptr) holder<type>(std::move(assign));
            }
            return *this;
        }

        template <typename s>
        var_ref& operator+= (s val)
            { get() += val; return *this; }

        template <typename s>
        var_ref& operator-= (s val)
            { get() -= val; return *this; }

        template <typename s>
        var_ref& operator*= (s val)
            { get() *= val; return *this; }

        template <typename s>
        var_ref& operator/= (s val)
            { get() /= val; return *this; }

    protected:
        var_ref (size_t offset, elem& e)
            : offset_(offset), e_(e)
            { }

    private:
        size_t offset_;
        elem&  e_;
    };

public:
    storage()
        : next_id_(0)
        {
            component_offsets_.push_back(0);
        }

    template <typename type>
    component_id register_component (std::string name)
        {
            size_t size;

            if (is_flat<type>::value)
            {
                size = sizeof(type);
                components_.emplace_back(name, size, nullptr);
            }
            else
            {
                flat_mask_.set(components_.size());
                size = sizeof(holder<type>);
                components_.emplace_back(name, size,
                                         std::unique_ptr<placeholder>(new holder<type>()));
            }

            if (components_.size() < cache_size)
            {
                size_t i (component_offsets_.size());
                component_offsets_.resize(i * 2);
                for (size_t j (i); j != i * 2; ++j)
                    component_offsets_[j] = component_offsets_[j-i] + size;
            }

            return components_.size() - 1;
        }

    component_id find_component (const std::string& name) const
        {
            auto found (std::find(components_.begin(), components_.end(), name));
            if (found == components_.end())
                throw std::logic_error("component does not exist");

            return std::distance(components_.begin(), found);
        }

    const component& operator[] (component_id id) const
        { return components_[id]; }

    const std::vector<component>& components() const
        { return components_; }

public:
    entity new_entity()
        {
            entities_[next_id_++];
            return next_id_ - 1;
        }

    /** Get an entity with a given ID, or create it if it didn't exist yet. */
    iterator make (uint32_t id)
        {
            if (next_id_ <= id)
                next_id_ = id + 1;

            return entities_.insert(entities_.end(), std::make_pair(id, elem()));
        }

    /** Create a whole bunch of empty entities in one go.
     * @param count     The number of entities to create
     * @return The range of entities created */
    std::pair<entity, entity> new_entities (size_t count)
        {
            auto range_begin (next_id_);
            for (; count > 0; --count)
                entities_[next_id_++];

            return std::make_pair(range_begin, next_id_);
        }

    entity clone_entity (iterator f)
        {
            elem& e (f->second);
            entities_[next_id_++] = e;

            // Quick check if we need to make deep copies
            if ((e.components & flat_mask_).any())
            {
                size_t off (0);
                for (int c_id (0); c_id < 64 && off < e.data.size(); ++c_id)
                {
                    if (e.components[c_id])
                    {
                        if (!components_[c_id].is_flat())
                        {
                            auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                            ptr = ptr->clone();
                        }
                        off += components_[c_id].size();
                    }
                }
            }
            return next_id_ - 1;
        }

    iterator find (entity en)
        {
            auto found (entities_.find(en.id()));
            if (found == entities_.end())
                throw std::logic_error("unknown entity");

            return found;
        }

    const_iterator find (entity en) const
        {
            auto found (entities_.find(en.id()));
            if (found == entities_.end())
                throw std::logic_error("unknown entity");

            return found;
        }

    void delete_entity (entity en)
        { delete_entity(find(en)); }

    void delete_entity (iterator f)
        {
            elem& e (f->second);

            // Quick check if we'll have to call any destructors.
            if ((e.components & flat_mask_).any())
            {
                size_t off (0);
                for (int search (0); search < 64 && off < e.data.size(); ++search)
                {
                    if (e.components[search])
                    {
                        if (!components_[search].is_flat())
                        {
                            auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                            ptr->~placeholder();
                        }
                        off += components_[search].size();
                    }
                }
            }

            entities_.erase(f);
        }

    void remove_component_from_entity (iterator en, component_id c)
        {
            auto& e (en->second);
            if (!e.components[c])
                return;

            size_t off (offset(e, c));
            auto& comp_info (components_[c]);
            if (!comp_info.is_flat())
            {
                auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                ptr->~placeholder();
            }
            auto o (e.data.begin() + off);
            e.data.erase(o, o + comp_info.size());
            e.components.reset(c);
        }

    /** Call a function for every entity that has a given component.
     *  The callee can then query and change the value of the component through
     *  a var_ref object, or remove the entity.
     * @param c     The component to look for.
     * @param func  The function to call.  This function will be passed an
     *              iterator to the current entity, and a var_ref corresponding
     *              to the component value in this entity. */
    template <typename t>
    void for_each (component_id c, std::function<void(iterator, var_ref<t>)> func)
        {
            std::bitset<64> mask;
            mask.set(c);
            for (auto i (entities_.begin()); i != entities_.end(); )
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                    func(i, var_ref<t>(offset(e, c), e));

                i = next;
            }
        }

    template <typename t1, typename t2>
    void for_each (component_id c1, component_id c2,
                   std::function<void(iterator, var_ref<t1>, var_ref<t2>)> func)
        {
            std::bitset<64> mask;
            mask.set(c1);
            mask.set(c2);
            for (auto i (entities_.begin()); i != entities_.end(); ++i)
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                {
                    func(i, var_ref<t1>(offset(e, c1), e),
                            var_ref<t2>(offset(e, c2), e));
                }
                i = next;
            }
        }

    template <typename t1, typename t2, typename t3>
    void for_each (component_id c1, component_id c2, component_id c3,
                   std::function<void(iterator, var_ref<t1>, var_ref<t2>, var_ref<t3>)> func)
        {
            std::bitset<64> mask;
            mask.set(c1);
            mask.set(c2);
            mask.set(c3);
            for (auto i (entities_.begin()); i != entities_.end(); ++i)
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                {
                    func(i, var_ref<t1>(offset(e, c1), e),
                            var_ref<t2>(offset(e, c2), e),
                            var_ref<t3>(offset(e, c3), e));
                }
                i = next;
            }
        }

    bool exists (entity en) const
        { return entities_.count(en.id()); }

    template <typename type>
    void set (entity en, component_id c_id, type val)
        { return set<type>(find(en), c_id, std::move(val)); }

    template <typename type>
    void set (iterator en, component_id c_id, type val)
        {
            const component& c (components_[c_id]);
            elem&  e   (en->second);
            size_t off (offset(e, c_id));

            if (!e.components[c_id])
            {
                if (e.data.size() < off)
                    e.data.resize(off + c.size());
                else
                    e.data.insert(e.data.begin() + off, c.size(), 0);

                e.components.set(c_id);
            }

            if (is_flat<type>::value)
            {
                assert(e.data.size() >= off + sizeof(type));
                new (&*e.data.begin() + off) type(std::move(val));
            }
            else
            {
                assert(e.data.size() >= off + sizeof(holder<type>));
                auto ptr (reinterpret_cast<holder<type>*>(&*e.data.begin() + off));
                new (ptr) holder<type>(std::move(val));
            }
        }

    template <typename type>
    const type& get (entity en, component_id c_id) const
        { return get<type>(find(en), c_id); }

    template <typename type>
    const type& get (const_iterator en, component_id c_id) const
        {
            auto& e (en->second);
            if (!e.components[c_id])
                throw std::logic_error("entity does not have component");

            return get<type>(e, c_id);
        }

private:
    template <typename type>
    const type& get (const elem& e, component_id c_id) const
        {
            size_t off (offset(e, c_id));
            if (is_flat<type>::value)
                return *reinterpret_cast<const type*>(&*e.data.begin() + off);

            auto ptr (reinterpret_cast<const holder<type>*>(&*e.data.begin() + off));

            return ptr->held();
        }

    size_t offset (const elem& e, component_id c) const
        {
            assert(c < components_.size());
            assert((c & cache_mask) < component_offsets_.size());
            size_t result (component_offsets_[c & cache_mask]);

            for (component_id search (cache_size); search < c; ++search)
            {
                if (e.components[search])
                    result += components_[search].size();
            }
            return result;
        }

private:
    /** Keeps track of entity IDs to give out. */
    uint32_t                            next_id_;
    /** The list of registered components. */
    std::vector<component>              components_;
    /** Mapping entity IDs to their data. */
    std::unordered_map<uint32_t, elem>  entities_;
    /** A lookup table for the data offsets of components.
     *  The index is the bitmask as used in elem::components. */
    std::vector<size_t>                 component_offsets_;
    /** A bitmask to quickly determine whether a certain combination of
     ** components has a flat memory layout or not. */
    std::bitset<64>                     flat_mask_;
};

} // namespace es
<commit_msg>Applied #e2928c1b32 again (?)<commit_after>//---------------------------------------------------------------------------
/// \file   es/storage.hpp
/// \brief  The entity/component data store
//
// Copyright 2013, nocte@hippie.nu            Released under the MIT License.
//---------------------------------------------------------------------------
#pragma once

#include <algorithm>
#include <bitset>
#include <cassert>
#include <cstdint>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <vector>
#include <unordered_map>

#include "component.hpp"
#include "entity.hpp"
#include "traits.hpp"

namespace es {

/** A storage ties entities and components together.
 * Storage associates two other bits of data with every entity:
 * - A 64-bit mask that keeps track of which components are defined
 * - A vector of bytes, holding the actual data
 *
 * The vector of bytes tries to pack the component data as tightly as
 * possible.  It is really fast for plain old datatypes, but it also
 * handles nontrivial types safely.  It packs a virtual table and a
 * pointer to some heap space in the vector, and calls the constructor
 * and destructor as needed.
 */
class storage
{
    // It is assumed the first few components will be accessed the
    // most often.  We keep a cache of the first 12.
    static constexpr uint32_t cache_size = 12;
    static constexpr uint32_t cache_mask = (1 << cache_size) - 1;

    /** This data gets associated with every entity. */
    struct elem
    {
        /** Bitmask to keep track of which components are held in \a data. */
        std::bitset<64>     components;
        /** Component data for this entity. */
        std::vector<char>   data;
    };

    typedef component::placeholder placeholder;

    /** Data types that do not have a flat memory layout are kept in the
     ** elem::data buffer in a placeholder object. */
    template <typename t>
    class holder : public placeholder
    {
    public:
        holder () { }
        holder (t init) : held_(std::move(init)) { }

        const t& held() const { return held_; }
        t&       held()       { return held_; }

        placeholder* clone() const { return new holder<t>(held_); }

    private:
        t held_;
    };

    typedef std::unordered_map<uint32_t, elem> stor_impl;

public:
    typedef uint8_t     component_id;

    typedef stor_impl::iterator         iterator;
    typedef stor_impl::const_iterator   const_iterator;

public:
    /** Variable references are used by systems to access an entity's data.
     *  It keeps track of the elem::data buffer and the offset inside that
     *  buffer. */
    template <typename type>
    class var_ref
    {
        friend class storage;

        type& get()
        {
            if (is_flat<type>::value)
                return *reinterpret_cast<type*>(&*e_.data.begin() + offset_);

            auto ptr (reinterpret_cast<holder<type>*>(&*e_.data.begin() + offset_));
            return ptr->held();
        }

    public:
        operator const type& () const
        {
            if (is_flat<type>::value)
                return *reinterpret_cast<const type*>(&*e_.data.begin() + offset_);

            auto ptr (reinterpret_cast<const holder<type>*>(&*e_.data.begin() + offset_));
            return ptr->held();
        }

        var_ref& operator= (type assign)
        {
            if (is_flat<type>::value)
            {
                new (&*e_.data.begin() + offset_) type(std::move(assign));
            }
            else
            {
                auto ptr (reinterpret_cast<holder<type>*>(&*e_.data.begin() + offset_));
                new (ptr) holder<type>(std::move(assign));
            }
            return *this;
        }

        template <typename s>
        var_ref& operator+= (s val)
            { get() += val; return *this; }

        template <typename s>
        var_ref& operator-= (s val)
            { get() -= val; return *this; }

        template <typename s>
        var_ref& operator*= (s val)
            { get() *= val; return *this; }

        template <typename s>
        var_ref& operator/= (s val)
            { get() /= val; return *this; }

    protected:
        var_ref (size_t offset, elem& e)
            : offset_(offset), e_(e)
            { }

    private:
        size_t offset_;
        elem&  e_;
    };

public:
    storage()
        : next_id_(0)
        {
            component_offsets_.push_back(0);
        }

    template <typename type>
    component_id register_component (std::string name)
        {
            size_t size;

            if (is_flat<type>::value)
            {
                size = sizeof(type);
                components_.emplace_back(name, size, nullptr);
            }
            else
            {
                flat_mask_.set(components_.size());
                size = sizeof(holder<type>);
                components_.emplace_back(name, size,
                                         std::unique_ptr<placeholder>(new holder<type>()));
            }

            if (components_.size() < cache_size)
            {
                size_t i (component_offsets_.size());
                component_offsets_.resize(i * 2);
                for (size_t j (i); j != i * 2; ++j)
                    component_offsets_[j] = component_offsets_[j-i] + size;
            }

            return components_.size() - 1;
        }

    component_id find_component (const std::string& name) const
        {
            auto found (std::find(components_.begin(), components_.end(), name));
            if (found == components_.end())
                throw std::logic_error("component does not exist");

            return std::distance(components_.begin(), found);
        }

    const component& operator[] (component_id id) const
        { return components_[id]; }

    const std::vector<component>& components() const
        { return components_; }

public:
    entity new_entity()
        {
            entities_[next_id_++];
            return next_id_ - 1;
        }

    /** Get an entity with a given ID, or create it if it didn't exist yet. */
    iterator make (uint32_t id)
        {
            if (next_id_ <= id)
                next_id_ = id + 1;

            return entities_.insert(entities_.end(), std::make_pair(id, elem()));
        }

    /** Create a whole bunch of empty entities in one go.
     * @param count     The number of entities to create
     * @return The range of entities created */
    std::pair<entity, entity> new_entities (size_t count)
        {
            auto range_begin (next_id_);
            for (; count > 0; --count)
                entities_[next_id_++];

            return std::make_pair(range_begin, next_id_);
        }

    entity clone_entity (iterator f)
        {
            elem& e (f->second);
            entities_[next_id_++] = e;

            // Quick check if we need to make deep copies
            if ((e.components & flat_mask_).any())
            {
                size_t off (0);
                for (int c_id (0); c_id < 64 && off < e.data.size(); ++c_id)
                {
                    if (e.components[c_id])
                    {
                        if (!components_[c_id].is_flat())
                        {
                            auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                            ptr = ptr->clone();
                        }
                        off += components_[c_id].size();
                    }
                }
            }
            return next_id_ - 1;
        }

    iterator find (entity en)
        {
            auto found (entities_.find(en.id()));
            if (found == entities_.end())
                throw std::logic_error("unknown entity");

            return found;
        }

    const_iterator find (entity en) const
        {
            auto found (entities_.find(en.id()));
            if (found == entities_.end())
                throw std::logic_error("unknown entity");

            return found;
        }

    void delete_entity (entity en)
        { delete_entity(find(en)); }

    void delete_entity (iterator f)
        {
            elem& e (f->second);

            // Quick check if we'll have to call any destructors.
            if ((e.components & flat_mask_).any())
            {
                size_t off (0);
                for (int search (0); search < 64 && off < e.data.size(); ++search)
                {
                    if (e.components[search])
                    {
                        if (!components_[search].is_flat())
                        {
                            auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                            ptr->~placeholder();
                        }
                        off += components_[search].size();
                    }
                }
            }

            entities_.erase(f);
        }

    void remove_component_from_entity (iterator en, component_id c)
        {
            auto& e (en->second);
            if (!e.components[c])
                return;

            size_t off (offset(e, c));
            auto& comp_info (components_[c]);
            if (!comp_info.is_flat())
            {
                auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                ptr->~placeholder();
            }
            auto o (e.data.begin() + off);
            e.data.erase(o, o + comp_info.size());
            e.components.reset(c);
        }

    /** Call a function for every entity that has a given component.
     *  The callee can then query and change the value of the component through
     *  a var_ref object, or remove the entity.
     * @param c     The component to look for.
     * @param func  The function to call.  This function will be passed an
     *              iterator to the current entity, and a var_ref corresponding
     *              to the component value in this entity. */
    template <typename t>
    void for_each (component_id c, std::function<void(iterator, var_ref<t>)> func)
        {
            std::bitset<64> mask;
            mask.set(c);
            for (auto i (entities_.begin()); i != entities_.end(); )
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                    func(i, var_ref<t>(offset(e, c), e));

                i = next;
            }
        }

    template <typename t1, typename t2>
    void for_each (component_id c1, component_id c2,
                   std::function<void(iterator, var_ref<t1>, var_ref<t2>)> func)
        {
            std::bitset<64> mask;
            mask.set(c1);
            mask.set(c2);
            for (auto i (entities_.begin()); i != entities_.end(); )
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                {
                    func(i, var_ref<t1>(offset(e, c1), e),
                            var_ref<t2>(offset(e, c2), e));
                }
                i = next;
            }
        }

    template <typename t1, typename t2, typename t3>
    void for_each (component_id c1, component_id c2, component_id c3,
                   std::function<void(iterator, var_ref<t1>, var_ref<t2>, var_ref<t3>)> func)
        {
            std::bitset<64> mask;
            mask.set(c1);
            mask.set(c2);
            mask.set(c3);
            for (auto i (entities_.begin()); i != entities_.end(); )
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                {
                    func(i, var_ref<t1>(offset(e, c1), e),
                            var_ref<t2>(offset(e, c2), e),
                            var_ref<t3>(offset(e, c3), e));
                }
                i = next;
            }
        }

    bool exists (entity en) const
        { return entities_.count(en.id()); }

    template <typename type>
    void set (entity en, component_id c_id, type val)
        { return set<type>(find(en), c_id, std::move(val)); }

    template <typename type>
    void set (iterator en, component_id c_id, type val)
        {
            const component& c (components_[c_id]);
            elem&  e   (en->second);
            size_t off (offset(e, c_id));

            if (!e.components[c_id])
            {
                if (e.data.size() < off)
                    e.data.resize(off + c.size());
                else
                    e.data.insert(e.data.begin() + off, c.size(), 0);

                e.components.set(c_id);
            }

            if (is_flat<type>::value)
            {
                assert(e.data.size() >= off + sizeof(type));
                new (&*e.data.begin() + off) type(std::move(val));
            }
            else
            {
                assert(e.data.size() >= off + sizeof(holder<type>));
                auto ptr (reinterpret_cast<holder<type>*>(&*e.data.begin() + off));
                new (ptr) holder<type>(std::move(val));
            }
        }

    template <typename type>
    const type& get (entity en, component_id c_id) const
        { return get<type>(find(en), c_id); }

    template <typename type>
    const type& get (const_iterator en, component_id c_id) const
        {
            auto& e (en->second);
            if (!e.components[c_id])
                throw std::logic_error("entity does not have component");

            return get<type>(e, c_id);
        }

private:
    template <typename type>
    const type& get (const elem& e, component_id c_id) const
        {
            size_t off (offset(e, c_id));
            if (is_flat<type>::value)
                return *reinterpret_cast<const type*>(&*e.data.begin() + off);

            auto ptr (reinterpret_cast<const holder<type>*>(&*e.data.begin() + off));

            return ptr->held();
        }

    size_t offset (const elem& e, component_id c) const
        {
            assert(c < components_.size());
            assert((c & cache_mask) < component_offsets_.size());
            size_t result (component_offsets_[c & cache_mask]);

            for (component_id search (cache_size); search < c; ++search)
            {
                if (e.components[search])
                    result += components_[search].size();
            }
            return result;
        }

private:
    /** Keeps track of entity IDs to give out. */
    uint32_t                            next_id_;
    /** The list of registered components. */
    std::vector<component>              components_;
    /** Mapping entity IDs to their data. */
    std::unordered_map<uint32_t, elem>  entities_;
    /** A lookup table for the data offsets of components.
     *  The index is the bitmask as used in elem::components. */
    std::vector<size_t>                 component_offsets_;
    /** A bitmask to quickly determine whether a certain combination of
     ** components has a flat memory layout or not. */
    std::bitset<64>                     flat_mask_;
};

} // namespace es
<|endoftext|>


<commit_before>/*
 * GridMapRosConverter.cpp
 *
 *  Created on: Jul 14, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "grid_map/GridMapRosConverter.hpp"
#include "grid_map/GridMapMsgHelpers.hpp"
#include <grid_map_core/GridMapMath.hpp>
#include <grid_map_core/iterators/GridMapIterator.hpp>

// ROS
#include <sensor_msgs/point_cloud2_iterator.h>
#include <geometry_msgs/Point.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

// STL
#include <limits>
#include <algorithm>

using namespace std;
using namespace Eigen;

namespace grid_map {

GridMapRosConverter::GridMapRosConverter()
{
}

GridMapRosConverter::~GridMapRosConverter()
{
}

bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap)
{
  gridMap.setTimestamp(message.info.header.stamp.toNSec());
  gridMap.setFrameId(message.info.header.frame_id);
  gridMap.setGeometry(Length(message.info.length_x, message.info.length_y), message.info.resolution,
                      Position(message.info.pose.position.x, message.info.pose.position.y));

  if (message.layers.size() != message.data.size()) {
    ROS_ERROR("Different number of layers and data in grid map message.");
    return false;
  }

  for (unsigned int i = 0; i < message.layers.size(); i++) {
    Matrix data;
    multiArrayMessageCopyToMatrixEigen(message.data[i], data); // TODO Could we use the data mapping (instead of copying) method here?
    // TODO Check if size is good.   size_ << getRows(message.data[0]), getCols(message.data[0]);
    gridMap.add(message.layers[i], data);
  }

  gridMap.setBasicLayers(message.basic_layers);
  gridMap.setStartIndex(Index(message.outer_start_index, message.inner_start_index));
  return true;
}

void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message)
{
  toMessage(gridMap, gridMap.getLayers(), message);
}

void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
                      grid_map_msgs::GridMap& message)
{
  message.info.header.stamp.fromNSec(gridMap.getTimestamp());
  message.info.header.frame_id = gridMap.getFrameId();
  message.info.resolution = gridMap.getResolution();
  message.info.length_x = gridMap.getLength().x();
  message.info.length_y = gridMap.getLength().y();
  message.info.pose.position.x = gridMap.getPosition().x();
  message.info.pose.position.y = gridMap.getPosition().y();
  message.info.pose.position.z = 0.0;
  message.info.pose.orientation.x = 0.0;
  message.info.pose.orientation.y = 0.0;
  message.info.pose.orientation.z = 0.0;
  message.info.pose.orientation.w = 1.0;

  message.layers = layers;
  message.basic_layers = gridMap.getBasicLayers();

  for (const auto& layer : layers) {
    std_msgs::Float32MultiArray dataArray;
    matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray);
    message.data.push_back(dataArray);
  }

  message.outer_start_index = gridMap.getStartIndex()(0);
  message.inner_start_index = gridMap.getStartIndex()(1);
}

void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
                                       const std::string& pointLayer,
                                       sensor_msgs::PointCloud2& pointCloud)
{
  toPointCloud(gridMap, gridMap.getLayers(), pointLayer, pointCloud);
}

void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
                                       const std::vector<std::string>& layers,
                                       const std::string& pointLayer,
                                       sensor_msgs::PointCloud2& pointCloud)
{
// Header.
  pointCloud.header.frame_id = gridMap.getFrameId();
  pointCloud.header.stamp.fromNSec(gridMap.getTimestamp());
  pointCloud.is_dense = false;

// Fields.
  std::vector <std::string> fieldNames;

  for (const auto& layer : layers) {
    if (layer == pointLayer) {
      fieldNames.push_back("x");
      fieldNames.push_back("y");
      fieldNames.push_back("z");
    } else if (layer == "color") {
      fieldNames.push_back("rgb");
    } else {
      fieldNames.push_back(layer);
    }
  }

  pointCloud.fields.clear();
  pointCloud.fields.reserve(fieldNames.size());
  int offset = 0;

  for (auto& name : fieldNames) {
    sensor_msgs::PointField point_field;
    point_field.name = name;
    point_field.count = 1;
    point_field.datatype = sensor_msgs::PointField::FLOAT32;
    point_field.offset = offset;
    pointCloud.fields.push_back(point_field);
    offset = offset + point_field.count * 4;  // 4 for sensor_msgs::PointField::FLOAT32
  }

// Resize.
  size_t nPoints = gridMap.getSize().prod();
  pointCloud.height = 1;
  pointCloud.width = nPoints;
  pointCloud.point_step = offset;
  pointCloud.row_step = pointCloud.width * pointCloud.point_step;
  pointCloud.data.resize(pointCloud.height * pointCloud.row_step);

// Points
  std::unordered_map<std::string, sensor_msgs::PointCloud2Iterator<float>> fieldIterators;
  for (auto& name : fieldNames) {
    fieldIterators.insert(
        std::pair<std::string, sensor_msgs::PointCloud2Iterator<float>>(
            name, sensor_msgs::PointCloud2Iterator<float>(pointCloud, name)));
  }

  GridMapIterator mapIterator(gridMap);

  for (size_t i = 0; i < nPoints; ++i) {
    Position3 position;
    position.setConstant(NAN);
    gridMap.getPosition3(pointLayer, *mapIterator, position);

    for (auto& iterator : fieldIterators) {
      if (iterator.first == "x") {
        *iterator.second = (float) position.x();
      } else if (iterator.first == "y") {
        *iterator.second = (float) position.y();
      } else if (iterator.first == "z") {
        *iterator.second = (float) position.z();
      } else if (iterator.first == "rgb") {
        *iterator.second = gridMap.at("color", *mapIterator);
        ;
      } else {
        *iterator.second = gridMap.at(iterator.first, *mapIterator);
      }
    }

    ++mapIterator;
    for (auto& iterator : fieldIterators) {
      ++iterator.second;
    }
  }
}

void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap,
                                          const std::string& layer, float dataMin, float dataMax,
                                          nav_msgs::OccupancyGrid& occupancyGrid)
{
  occupancyGrid.header.frame_id = gridMap.getFrameId();
  occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp());
  occupancyGrid.info.map_load_time = occupancyGrid.header.stamp;  // Same as header stamp as we do not load the map.
  occupancyGrid.info.resolution = gridMap.getResolution();
  occupancyGrid.info.width = gridMap.getSize()(0);
  occupancyGrid.info.height = gridMap.getSize()(1);
  Position positionOfOrigin;
  getPositionOfDataStructureOrigin(gridMap.getPosition(), gridMap.getLength(), positionOfOrigin);
  occupancyGrid.info.origin.position.x = positionOfOrigin.x();
  occupancyGrid.info.origin.position.y = positionOfOrigin.y();
  occupancyGrid.info.origin.position.z = 0.0;
  occupancyGrid.info.origin.orientation.x = 0.0;
  occupancyGrid.info.origin.orientation.y = 0.0;
  occupancyGrid.info.origin.orientation.z = 1.0;  // yes, this is correct.
  occupancyGrid.info.origin.orientation.w = 0.0;
  occupancyGrid.data.resize(occupancyGrid.info.width * occupancyGrid.info.height);

  // Occupancy probabilities are in the range [0,100].  Unknown is -1.
  const float cellMin = 0;
  const float cellMax = 100;
  const float cellRange = cellMax - cellMin;

  for (GridMapIterator iterator(gridMap); !iterator.isPassedEnd(); ++iterator) {
    float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin);
    if (isnan(value))
      value = -1;
    else
      value = cellMin + min(max(0.0f, value), 1.0f) * cellRange;
    // Occupancy grid claims to be row-major order, but it does not seem that way.
    // http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html.
    unsigned int index = get1dIndexFrom2dIndex(*iterator, gridMap.getSize(), false);
    occupancyGrid.data[index] = value;
  }
}

void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const std::string& layer, float lowerThreshold,
                 float upperThreshold, nav_msgs::GridCells& gridCells)
{
  gridCells.header.frame_id = gridMap.getFrameId();
  gridCells.header.stamp.fromNSec(gridMap.getTimestamp());
  gridCells.cell_width = gridMap.getResolution();
  gridCells.cell_height = gridMap.getResolution();

  for (GridMapIterator iterator(gridMap); !iterator.isPassedEnd(); ++iterator) {
    if (!gridMap.isValid(*iterator, layer)) continue;
    if (gridMap.at(layer, *iterator) >= lowerThreshold
        && gridMap.at(layer, *iterator) <= upperThreshold) {
      Position position;
      gridMap.getPosition(*iterator, position);
      geometry_msgs::Point point;
      point.x = position.x();
      point.y = position.y();
      gridCells.cells.push_back(point);
    }
  }
}

bool GridMapRosConverter::saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag,
                                    const std::string& topic)
{
  grid_map_msgs::GridMap message;
  toMessage(gridMap, message);
  ros::Time time(gridMap.getTimestamp());

  if (!time.isValid()) {
    if (!ros::Time::isValid()) ros::Time::init();
    time = ros::Time::now();
  }

  rosbag::Bag bag;
  bag.open(pathToBag, rosbag::bagmode::Write);
  bag.write(topic, time, message);
  bag.close();
  return true;
}

bool GridMapRosConverter::loadFromBag(const std::string& pathToBag, const std::string& topic,
                                      grid_map::GridMap& gridMap)
{
  rosbag::Bag bag;
  bag.open(pathToBag, rosbag::bagmode::Read);
  rosbag::View view(bag, rosbag::TopicQuery(topic));

  for (const auto& messageInstance : view) {
    grid_map_msgs::GridMap::ConstPtr message = messageInstance.instantiate<grid_map_msgs::GridMap>();
    if (message != NULL) {
      fromMessage(*message, gridMap);
    } else {
      bag.close();
      ROS_WARN("Unable to load data from ROS bag.");
      return false;
    }
  }
  bag.close();
  return true;
}

} /* namespace */
<commit_msg>Bugfix.<commit_after>/*
 * GridMapRosConverter.cpp
 *
 *  Created on: Jul 14, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "grid_map/GridMapRosConverter.hpp"
#include "grid_map/GridMapMsgHelpers.hpp"
#include <grid_map_core/GridMapMath.hpp>
#include <grid_map_core/iterators/GridMapIterator.hpp>

// ROS
#include <sensor_msgs/point_cloud2_iterator.h>
#include <geometry_msgs/Point.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

// STL
#include <limits>
#include <algorithm>

using namespace std;
using namespace Eigen;

namespace grid_map {

GridMapRosConverter::GridMapRosConverter()
{
}

GridMapRosConverter::~GridMapRosConverter()
{
}

bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap)
{
  gridMap.setTimestamp(message.info.header.stamp.toNSec());
  gridMap.setFrameId(message.info.header.frame_id);
  gridMap.setGeometry(Length(message.info.length_x, message.info.length_y), message.info.resolution,
                      Position(message.info.pose.position.x, message.info.pose.position.y));

  if (message.layers.size() != message.data.size()) {
    ROS_ERROR("Different number of layers and data in grid map message.");
    return false;
  }

  for (unsigned int i = 0; i < message.layers.size(); i++) {
    Matrix data;
    multiArrayMessageCopyToMatrixEigen(message.data[i], data); // TODO Could we use the data mapping (instead of copying) method here?
    // TODO Check if size is good.   size_ << getRows(message.data[0]), getCols(message.data[0]);
    gridMap.add(message.layers[i], data);
  }

  gridMap.setBasicLayers(message.basic_layers);
  gridMap.setStartIndex(Index(message.outer_start_index, message.inner_start_index));
  return true;
}

void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message)
{
  toMessage(gridMap, gridMap.getLayers(), message);
}

void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
                      grid_map_msgs::GridMap& message)
{
  message.info.header.stamp.fromNSec(gridMap.getTimestamp());
  message.info.header.frame_id = gridMap.getFrameId();
  message.info.resolution = gridMap.getResolution();
  message.info.length_x = gridMap.getLength().x();
  message.info.length_y = gridMap.getLength().y();
  message.info.pose.position.x = gridMap.getPosition().x();
  message.info.pose.position.y = gridMap.getPosition().y();
  message.info.pose.position.z = 0.0;
  message.info.pose.orientation.x = 0.0;
  message.info.pose.orientation.y = 0.0;
  message.info.pose.orientation.z = 0.0;
  message.info.pose.orientation.w = 1.0;

  message.layers = layers;
  message.basic_layers = gridMap.getBasicLayers();

  message.data.clear();
  for (const auto& layer : layers) {
    std_msgs::Float32MultiArray dataArray;
    matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray);
    message.data.push_back(dataArray);
  }

  message.outer_start_index = gridMap.getStartIndex()(0);
  message.inner_start_index = gridMap.getStartIndex()(1);
}

void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
                                       const std::string& pointLayer,
                                       sensor_msgs::PointCloud2& pointCloud)
{
  toPointCloud(gridMap, gridMap.getLayers(), pointLayer, pointCloud);
}

void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
                                       const std::vector<std::string>& layers,
                                       const std::string& pointLayer,
                                       sensor_msgs::PointCloud2& pointCloud)
{
// Header.
  pointCloud.header.frame_id = gridMap.getFrameId();
  pointCloud.header.stamp.fromNSec(gridMap.getTimestamp());
  pointCloud.is_dense = false;

// Fields.
  std::vector <std::string> fieldNames;

  for (const auto& layer : layers) {
    if (layer == pointLayer) {
      fieldNames.push_back("x");
      fieldNames.push_back("y");
      fieldNames.push_back("z");
    } else if (layer == "color") {
      fieldNames.push_back("rgb");
    } else {
      fieldNames.push_back(layer);
    }
  }

  pointCloud.fields.clear();
  pointCloud.fields.reserve(fieldNames.size());
  int offset = 0;

  for (auto& name : fieldNames) {
    sensor_msgs::PointField point_field;
    point_field.name = name;
    point_field.count = 1;
    point_field.datatype = sensor_msgs::PointField::FLOAT32;
    point_field.offset = offset;
    pointCloud.fields.push_back(point_field);
    offset = offset + point_field.count * 4;  // 4 for sensor_msgs::PointField::FLOAT32
  }

// Resize.
  size_t nPoints = gridMap.getSize().prod();
  pointCloud.height = 1;
  pointCloud.width = nPoints;
  pointCloud.point_step = offset;
  pointCloud.row_step = pointCloud.width * pointCloud.point_step;
  pointCloud.data.resize(pointCloud.height * pointCloud.row_step);

// Points
  std::unordered_map<std::string, sensor_msgs::PointCloud2Iterator<float>> fieldIterators;
  for (auto& name : fieldNames) {
    fieldIterators.insert(
        std::pair<std::string, sensor_msgs::PointCloud2Iterator<float>>(
            name, sensor_msgs::PointCloud2Iterator<float>(pointCloud, name)));
  }

  GridMapIterator mapIterator(gridMap);

  for (size_t i = 0; i < nPoints; ++i) {
    Position3 position;
    position.setConstant(NAN);
    gridMap.getPosition3(pointLayer, *mapIterator, position);

    for (auto& iterator : fieldIterators) {
      if (iterator.first == "x") {
        *iterator.second = (float) position.x();
      } else if (iterator.first == "y") {
        *iterator.second = (float) position.y();
      } else if (iterator.first == "z") {
        *iterator.second = (float) position.z();
      } else if (iterator.first == "rgb") {
        *iterator.second = gridMap.at("color", *mapIterator);
        ;
      } else {
        *iterator.second = gridMap.at(iterator.first, *mapIterator);
      }
    }

    ++mapIterator;
    for (auto& iterator : fieldIterators) {
      ++iterator.second;
    }
  }
}

void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap,
                                          const std::string& layer, float dataMin, float dataMax,
                                          nav_msgs::OccupancyGrid& occupancyGrid)
{
  occupancyGrid.header.frame_id = gridMap.getFrameId();
  occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp());
  occupancyGrid.info.map_load_time = occupancyGrid.header.stamp;  // Same as header stamp as we do not load the map.
  occupancyGrid.info.resolution = gridMap.getResolution();
  occupancyGrid.info.width = gridMap.getSize()(0);
  occupancyGrid.info.height = gridMap.getSize()(1);
  Position positionOfOrigin;
  getPositionOfDataStructureOrigin(gridMap.getPosition(), gridMap.getLength(), positionOfOrigin);
  occupancyGrid.info.origin.position.x = positionOfOrigin.x();
  occupancyGrid.info.origin.position.y = positionOfOrigin.y();
  occupancyGrid.info.origin.position.z = 0.0;
  occupancyGrid.info.origin.orientation.x = 0.0;
  occupancyGrid.info.origin.orientation.y = 0.0;
  occupancyGrid.info.origin.orientation.z = 1.0;  // yes, this is correct.
  occupancyGrid.info.origin.orientation.w = 0.0;
  occupancyGrid.data.resize(occupancyGrid.info.width * occupancyGrid.info.height);

  // Occupancy probabilities are in the range [0,100].  Unknown is -1.
  const float cellMin = 0;
  const float cellMax = 100;
  const float cellRange = cellMax - cellMin;

  for (GridMapIterator iterator(gridMap); !iterator.isPassedEnd(); ++iterator) {
    float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin);
    if (isnan(value))
      value = -1;
    else
      value = cellMin + min(max(0.0f, value), 1.0f) * cellRange;
    // Occupancy grid claims to be row-major order, but it does not seem that way.
    // http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html.
    unsigned int index = get1dIndexFrom2dIndex(*iterator, gridMap.getSize(), false);
    occupancyGrid.data[index] = value;
  }
}

void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const std::string& layer, float lowerThreshold,
                 float upperThreshold, nav_msgs::GridCells& gridCells)
{
  gridCells.header.frame_id = gridMap.getFrameId();
  gridCells.header.stamp.fromNSec(gridMap.getTimestamp());
  gridCells.cell_width = gridMap.getResolution();
  gridCells.cell_height = gridMap.getResolution();

  for (GridMapIterator iterator(gridMap); !iterator.isPassedEnd(); ++iterator) {
    if (!gridMap.isValid(*iterator, layer)) continue;
    if (gridMap.at(layer, *iterator) >= lowerThreshold
        && gridMap.at(layer, *iterator) <= upperThreshold) {
      Position position;
      gridMap.getPosition(*iterator, position);
      geometry_msgs::Point point;
      point.x = position.x();
      point.y = position.y();
      gridCells.cells.push_back(point);
    }
  }
}

bool GridMapRosConverter::saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag,
                                    const std::string& topic)
{
  grid_map_msgs::GridMap message;
  toMessage(gridMap, message);
  ros::Time time(gridMap.getTimestamp());

  if (!time.isValid()) {
    if (!ros::Time::isValid()) ros::Time::init();
    time = ros::Time::now();
  }

  rosbag::Bag bag;
  bag.open(pathToBag, rosbag::bagmode::Write);
  bag.write(topic, time, message);
  bag.close();
  return true;
}

bool GridMapRosConverter::loadFromBag(const std::string& pathToBag, const std::string& topic,
                                      grid_map::GridMap& gridMap)
{
  rosbag::Bag bag;
  bag.open(pathToBag, rosbag::bagmode::Read);
  rosbag::View view(bag, rosbag::TopicQuery(topic));

  for (const auto& messageInstance : view) {
    grid_map_msgs::GridMap::ConstPtr message = messageInstance.instantiate<grid_map_msgs::GridMap>();
    if (message != NULL) {
      fromMessage(*message, gridMap);
    } else {
      bag.close();
      ROS_WARN("Unable to load data from ROS bag.");
      return false;
    }
  }
  bag.close();
  return true;
}

} /* namespace */
<|endoftext|>


<commit_before><commit_msg>Temporarily whitelist DirectoryLister, which is blocking the IO thread.<commit_after><|endoftext|>


<commit_before>/*******************************************************************
 KNotes -- Notes for the KDE project

 Copyright (c) 2002-2004, Michael Brade <brade@kde.org>

 This program is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License
 as published by the Free Software Foundation; either version 2
 of the License, or (at your option) any later version.

 This program 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 General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*******************************************************************/

#include <QFile>
#include <QFont>
#include <QPoint>
#include <QColor>
#include <QStringList>
#include <QTextStream>

#include <kdebug.h>
#include <kapplication.h>
#include <kglobal.h>
#include <kurl.h>
#include <kstandarddirs.h>
#include <kconfig.h>
#include <kio/netaccess.h>

#include <unistd.h>

#include "knoteslegacy.h"
#include "knoteconfig.h"
#include "version.h"

#include <kcal/calendarlocal.h>
#include <kcal/journal.h>

#include <netwm.h>

using namespace KCal;

void KNotesLegacy::cleanUp()
{
  // remove old (KDE 1.x) local config file if it still exists
  QString configfile = KGlobal::dirs()->saveLocation( "config" ) + "knotesrc";
  if ( QFile::exists( configfile ) ) {
    KConfigGroup test(
      KSharedConfig::openConfig( configfile, KConfig::SimpleConfig ),
      "General" );
    double version = test.readEntry( "version", 1.0 );

    if ( version == 1.0 ) {
      if ( !( KStandardDirs::checkAccess( configfile, W_OK ) &&
        QFile::remove( configfile ) ) ) {
          kError( 5500 ) <<"Could not delete old config file" << configfile;
      }
    }
  }
}

bool KNotesLegacy::convert( CalendarLocal *calendar )
{
  bool converted = false;

  QDir noteDir( KGlobal::dirs()->saveLocation( "appdata", "notes/" ) );
  QStringList notes = noteDir.entryList( QDir::Files, QDir::Name );
  for ( QStringList::Iterator note = notes.begin(); note != notes.end();
        note++ ) {
    QString file = noteDir.absoluteFilePath( *note );
    KConfig *test = new KConfig( file, KConfig::SimpleConfig );
    KConfigGroup grp( test, "General" );
    double version = grp.readEntry( "version", 1.0 );

    if ( version < 3.0 ) {

      // create the new note
      Journal *journal = new Journal();
      bool success;

      if ( version < 2.0 ) {
        success = convertKNotes1Config( journal, noteDir, *note );
      } else {
        success = convertKNotes2Config( journal, noteDir, *note );
      }

      // could not convert file => do not add a new note
      if ( !success ) {
        delete journal;
      } else {
        calendar->addJournal( journal );
        converted = true;
      }
    } else if ( version < 3.2 ) { // window state changed for version 3.2
#ifdef Q_WS_X11
      uint state = grp.readEntry( "state", uint( NET::SkipTaskbar ) );

      grp.writeEntry( "ShowInTaskbar",
                        ( state & NET::SkipTaskbar ) ? false : true );
      grp.writeEntry( "KeepAbove",
                        ( state & NET::KeepAbove ) ? true : false );
#endif
      grp.deleteEntry( "state" );
    }
    delete test;
  }

  return converted;
}

bool KNotesLegacy::convertKNotes1Config( Journal *journal, QDir &noteDir,
                                         const QString &file )
{
    QFile infile( noteDir.absoluteFilePath( file ) );
    if ( !infile.open( QIODevice::ReadOnly ) ) {
        kError( 5500 ) << "Could not open input file: \""
                       << infile.fileName() << "\"";
        return false;
    }

    QTextStream input( &infile );

    // get the name
    journal->setSummary( input.readLine() );

    QStringList props = input.readLine().split( '+', QString::SkipEmptyParts );

    // robustness
    if ( props.count() != 13 ) {
      kWarning( 5500 ) <<"The file \"" << infile.fileName()
                       << "\" lacks version information but is not a valid"
                       << "KNotes 1 config file either!";
      return false;
    }

    // the new configfile's name
    QString configFile = noteDir.absoluteFilePath( journal->uid() );

    // set the defaults
    KIO::NetAccess::file_copy(
        KUrl( KGlobal::dirs()->saveLocation( "config" ) + "knotesrc" ),
        KUrl( configFile ),
        0
    );

    KNoteConfig config( KSharedConfig::openConfig( configFile,
                                                   KConfig::NoGlobals ) );
    config.readConfig();
    config.setVersion( KNOTES_VERSION );

    // get the geometry
    config.setWidth( props[3].toUInt() );
    config.setHeight( props[4].toUInt() );

    // get the background color
    uint red = input.readLine().toUInt();
    uint green = input.readLine().toUInt();
    uint blue = input.readLine().toUInt();
    config.setBgColor( QColor( Qt::red, Qt::green, Qt::blue ) );

    // get the foreground color
    red = input.readLine().toUInt();
    green = input.readLine().toUInt();
    blue = input.readLine().toUInt();
    config.setFgColor( QColor( Qt::red, Qt::green, Qt::blue ) );

    // get the font
    QString fontfamily = input.readLine();
    if ( fontfamily.isEmpty() )
        fontfamily = QString( "Sans Serif" );
    uint size = input.readLine().toUInt();
    size = qMax( size, ( uint ) 4 );
    uint weight = input.readLine().toUInt();
    bool italic = ( input.readLine().toUInt() == 1 );
    QFont font( fontfamily, size, weight, italic );

    config.setTitleFont( font );
    config.setFont( font );

    // 3d frame? Not supported yet!
    input.readLine();

    // autoindent
    config.setAutoIndent( input.readLine().toUInt() == 1 );

    // KNotes 1 never had rich text
    config.setRichText( false );

    int note_desktop = props[0].toUInt();

    // hidden or on all desktops?
    if ( input.readLine().toUInt() == 1 )
        note_desktop = 0;
#ifdef Q_WS_X11
    else if ( props[11].toUInt() == 1 )
        note_desktop = NETWinInfo::OnAllDesktops;
#endif

    config.setDesktop( note_desktop );
    config.setPosition( QPoint( props[1].toUInt(), props[2].toUInt() ) );
    config.setKeepAbove( props[12].toUInt() & 2048 );

    config.writeConfig();

    // get the text
    QString text;
    while ( !input.atEnd() ) {
      text.append( input.readLine() );
      if ( !input.atEnd() ) {
        text.append( '\n' );
      }
    }

    journal->setDescription( text );

    if ( !infile.remove() )
    {
      kWarning( 5500 ) << "Could not delete input file: \"" << infile.fileName()
                       << "\"";
    }

    return true;
}

bool KNotesLegacy::convertKNotes2Config( Journal *journal, QDir &noteDir,
                                         const QString &file )
{
  QString configFile = noteDir.absoluteFilePath( journal->uid() );

  // new name for config file
  if ( !noteDir.rename( file, journal->uid() ) ) {
    kError( 5500 ) << "Could not rename input file: \""
                   << noteDir.absoluteFilePath( file ) << "\" to \""
                   << configFile << "\"!";
    return false;
  }

  // update the config
  KConfig config( configFile );
  KConfigGroup grp( &config, "Data" );
  journal->setSummary( grp.readEntry( "name" ) );
  config.deleteGroup( "Data", KConfig::Localized );
  KConfigGroup grp2(&config, "General" ); // XXX right?
  grp2.writeEntry( "version", KNOTES_VERSION );
  KConfigGroup grp3(&config, "WindowDisplay" ); // XXX right?
#ifdef Q_WS_X11
  uint state = grp3.readEntry( "state", uint( NET::SkipTaskbar ) );
  grp3.writeEntry( "ShowInTaskbar",
                     ( state & NET::SkipTaskbar ) ? false : true );
  grp3.writeEntry( "KeepAbove",
                      ( state & NET::KeepAbove ) ? true : false );
#endif
  grp3.deleteEntry( "state" );

  // load the saved text and put it in the journal
  QFile infile( noteDir.absoluteFilePath( "." + file + "_data" ) );
  if ( infile.open( QIODevice::ReadOnly ) ) {
    QTextStream input( &infile );
    input.setCodec( "UTF-8" );
    journal->setDescription( input.readAll() );
    if ( !infile.remove() ) {
      kWarning( 5500 ) << "Could not delete data file: \"" << infile.fileName()
                       << "\"";
    }
  }
  else
    kWarning( 5500 ) << "Could not open data file: \"" << infile.fileName()
                   << "\"";

    return true;
}
<commit_msg>Const'ify<commit_after>/*******************************************************************
 KNotes -- Notes for the KDE project

 Copyright (c) 2002-2004, Michael Brade <brade@kde.org>

 This program is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License
 as published by the Free Software Foundation; either version 2
 of the License, or (at your option) any later version.

 This program 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 General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*******************************************************************/

#include <QFile>
#include <QFont>
#include <QPoint>
#include <QColor>
#include <QStringList>
#include <QTextStream>

#include <kdebug.h>
#include <kapplication.h>
#include <kglobal.h>
#include <kurl.h>
#include <kstandarddirs.h>
#include <kconfig.h>
#include <kio/netaccess.h>

#include <unistd.h>

#include "knoteslegacy.h"
#include "knoteconfig.h"
#include "version.h"

#include <kcal/calendarlocal.h>
#include <kcal/journal.h>

#include <netwm.h>

using namespace KCal;

void KNotesLegacy::cleanUp()
{
  // remove old (KDE 1.x) local config file if it still exists
  QString configfile = KGlobal::dirs()->saveLocation( "config" ) + "knotesrc";
  if ( QFile::exists( configfile ) ) {
    KConfigGroup test(
      KSharedConfig::openConfig( configfile, KConfig::SimpleConfig ),
      "General" );
    double version = test.readEntry( "version", 1.0 );

    if ( version == 1.0 ) {
      if ( !( KStandardDirs::checkAccess( configfile, W_OK ) &&
        QFile::remove( configfile ) ) ) {
          kError( 5500 ) <<"Could not delete old config file" << configfile;
      }
    }
  }
}

bool KNotesLegacy::convert( CalendarLocal *calendar )
{
  bool converted = false;

  QDir noteDir( KGlobal::dirs()->saveLocation( "appdata", "notes/" ) );
  const QStringList notes = noteDir.entryList( QDir::Files, QDir::Name );
  for ( QStringList::ConstIterator note = notes.constBegin(); note != notes.constEnd();
        ++note ) {
    QString file = noteDir.absoluteFilePath( *note );
    KConfig *test = new KConfig( file, KConfig::SimpleConfig );
    KConfigGroup grp( test, "General" );
    double version = grp.readEntry( "version", 1.0 );

    if ( version < 3.0 ) {

      // create the new note
      Journal *journal = new Journal();
      bool success;

      if ( version < 2.0 ) {
        success = convertKNotes1Config( journal, noteDir, *note );
      } else {
        success = convertKNotes2Config( journal, noteDir, *note );
      }

      // could not convert file => do not add a new note
      if ( !success ) {
        delete journal;
      } else {
        calendar->addJournal( journal );
        converted = true;
      }
    } else if ( version < 3.2 ) { // window state changed for version 3.2
#ifdef Q_WS_X11
      uint state = grp.readEntry( "state", uint( NET::SkipTaskbar ) );

      grp.writeEntry( "ShowInTaskbar",
                        ( state & NET::SkipTaskbar ) ? false : true );
      grp.writeEntry( "KeepAbove",
                        ( state & NET::KeepAbove ) ? true : false );
#endif
      grp.deleteEntry( "state" );
    }
    delete test;
  }

  return converted;
}

bool KNotesLegacy::convertKNotes1Config( Journal *journal, QDir &noteDir,
                                         const QString &file )
{
    QFile infile( noteDir.absoluteFilePath( file ) );
    if ( !infile.open( QIODevice::ReadOnly ) ) {
        kError( 5500 ) << "Could not open input file: \""
                       << infile.fileName() << "\"";
        return false;
    }

    QTextStream input( &infile );

    // get the name
    journal->setSummary( input.readLine() );

    QStringList props = input.readLine().split( '+', QString::SkipEmptyParts );

    // robustness
    if ( props.count() != 13 ) {
      kWarning( 5500 ) <<"The file \"" << infile.fileName()
                       << "\" lacks version information but is not a valid"
                       << "KNotes 1 config file either!";
      return false;
    }

    // the new configfile's name
    QString configFile = noteDir.absoluteFilePath( journal->uid() );

    // set the defaults
    KIO::NetAccess::file_copy(
        KUrl( KGlobal::dirs()->saveLocation( "config" ) + "knotesrc" ),
        KUrl( configFile ),
        0
    );

    KNoteConfig config( KSharedConfig::openConfig( configFile,
                                                   KConfig::NoGlobals ) );
    config.readConfig();
    config.setVersion( KNOTES_VERSION );

    // get the geometry
    config.setWidth( props[3].toUInt() );
    config.setHeight( props[4].toUInt() );

    // get the background color
    uint red = input.readLine().toUInt();
    uint green = input.readLine().toUInt();
    uint blue = input.readLine().toUInt();
    config.setBgColor( QColor( Qt::red, Qt::green, Qt::blue ) );

    // get the foreground color
    red = input.readLine().toUInt();
    green = input.readLine().toUInt();
    blue = input.readLine().toUInt();
    config.setFgColor( QColor( Qt::red, Qt::green, Qt::blue ) );

    // get the font
    QString fontfamily = input.readLine();
    if ( fontfamily.isEmpty() )
        fontfamily = QString( "Sans Serif" );
    uint size = input.readLine().toUInt();
    size = qMax( size, ( uint ) 4 );
    uint weight = input.readLine().toUInt();
    bool italic = ( input.readLine().toUInt() == 1 );
    QFont font( fontfamily, size, weight, italic );

    config.setTitleFont( font );
    config.setFont( font );

    // 3d frame? Not supported yet!
    input.readLine();

    // autoindent
    config.setAutoIndent( input.readLine().toUInt() == 1 );

    // KNotes 1 never had rich text
    config.setRichText( false );

    int note_desktop = props[0].toUInt();

    // hidden or on all desktops?
    if ( input.readLine().toUInt() == 1 )
        note_desktop = 0;
#ifdef Q_WS_X11
    else if ( props[11].toUInt() == 1 )
        note_desktop = NETWinInfo::OnAllDesktops;
#endif

    config.setDesktop( note_desktop );
    config.setPosition( QPoint( props[1].toUInt(), props[2].toUInt() ) );
    config.setKeepAbove( props[12].toUInt() & 2048 );

    config.writeConfig();

    // get the text
    QString text;
    while ( !input.atEnd() ) {
      text.append( input.readLine() );
      if ( !input.atEnd() ) {
        text.append( '\n' );
      }
    }

    journal->setDescription( text );

    if ( !infile.remove() )
    {
      kWarning( 5500 ) << "Could not delete input file: \"" << infile.fileName()
                       << "\"";
    }

    return true;
}

bool KNotesLegacy::convertKNotes2Config( Journal *journal, QDir &noteDir,
                                         const QString &file )
{
  QString configFile = noteDir.absoluteFilePath( journal->uid() );

  // new name for config file
  if ( !noteDir.rename( file, journal->uid() ) ) {
    kError( 5500 ) << "Could not rename input file: \""
                   << noteDir.absoluteFilePath( file ) << "\" to \""
                   << configFile << "\"!";
    return false;
  }

  // update the config
  KConfig config( configFile );
  KConfigGroup grp( &config, "Data" );
  journal->setSummary( grp.readEntry( "name" ) );
  config.deleteGroup( "Data", KConfig::Localized );
  KConfigGroup grp2(&config, "General" ); // XXX right?
  grp2.writeEntry( "version", KNOTES_VERSION );
  KConfigGroup grp3(&config, "WindowDisplay" ); // XXX right?
#ifdef Q_WS_X11
  uint state = grp3.readEntry( "state", uint( NET::SkipTaskbar ) );
  grp3.writeEntry( "ShowInTaskbar",
                     ( state & NET::SkipTaskbar ) ? false : true );
  grp3.writeEntry( "KeepAbove",
                      ( state & NET::KeepAbove ) ? true : false );
#endif
  grp3.deleteEntry( "state" );

  // load the saved text and put it in the journal
  QFile infile( noteDir.absoluteFilePath( '.' + file + "_data" ) );
  if ( infile.open( QIODevice::ReadOnly ) ) {
    QTextStream input( &infile );
    input.setCodec( "UTF-8" );
    journal->setDescription( input.readAll() );
    if ( !infile.remove() ) {
      kWarning( 5500 ) << "Could not delete data file: \"" << infile.fileName()
                       << "\"";
    }
  }
  else
    kWarning( 5500 ) << "Could not open data file: \"" << infile.fileName()
                   << "\"";

    return true;
}
<|endoftext|>