<commit_before>#include <taichi/lang.h>
#include <taichi/testing.h>
#include <numeric>

TLANG_NAMESPACE_BEGIN

TC_TEST("gpu_gc_basics") {
  for (auto arch : {Arch::gpu}) {
    int n = 32;
    Program prog(arch);

    Global(x, i32);
    layout([&]() {
      auto i = Index(0);
      auto j = Index(1);
      root.dense(i, n).pointer().dense(j, n).place(x);
    });

    kernel([&]() {
      For(0, n, [&](Expr i) {
        For(0, i, [&](Expr j) { Activate(x.snode(), {i, j}); });
      });
    })();

    kernel([&]() {
      For(0, n, [&](Expr i) { For(0, i, [&](Expr j) { x[i, j] = i + j; }); });
    })();

    auto stat = x.parent().parent().snode()->stat();
    TC_CHECK(stat.num_resident_blocks == n - 1);
    for (int i = 0; i < n; i++) {
      for (int j = 0; j < i; j++) {
        TC_CHECK(x.val<int>(i, j) == i + j);
      }
    }
    x.parent().parent().snode()->clear_data_and_deactivate();
    stat = x.parent().parent().snode()->stat();
    TC_CHECK(stat.num_resident_blocks == 0);
    TC_CHECK(stat.num_recycled_blocks == 0);

    for (int i = 0; i < n; i++) {
      for (int j = 0; j < i; j++) {
        TC_CHECK(x.val<int>(i, j) == 0);
      }
    }
  }
};

TC_TEST("parallel_particle_sort") {
  Program prog(Arch::gpu);
  CoreState::set_trigger_gdb_when_crash(true);

  constexpr int n = 256;
  const real dx = 1.0_f / n, inv_dx = 1.0_f / dx;

  constexpr int dim = 3;

  auto f32 = DataType::f32;
  int grid_block_size = 4;

  Vector particle_x(f32, dim);
  Global(l, i32);

  Vector grid_v(f32, dim);
  Global(grid_m, f32);

  int max_n_particles = 1024 * 1024;

  int n_particles = 0;
  std::vector<float> benchmark_particles;
  std::vector<Vector3> p_x;
  n_particles = max_n_particles;
  p_x.resize(n_particles);
  for (int i = 0; i < n_particles; i++) {
    Vector3 offset = Vector3::rand() - Vector3(0.5_f);
    p_x[i] = Vector3(0.5_f) + offset * 0.7f;
  }

  TC_ASSERT(n_particles <= max_n_particles);

  auto i = Index(0), j = Index(1), k = Index(2);
  auto p = Index(3);

  layout([&]() {
    auto &fork = root.dynamic(p, max_n_particles);
    for (int i = 0; i < dim; i++) {
      fork.place(particle_x(i));
    }

    TC_ASSERT(n % grid_block_size == 0);
    auto &block = root.dense({i, j, k}, n / grid_block_size).pointer();

    block.dense({i, j, k}, grid_block_size)
        .place(grid_v(0), grid_v(1), grid_v(2), grid_m);
  });

  TC_ASSERT(bit::is_power_of_two(n));

  Kernel(sort).def([&] {
    BlockDim(256);
    For(particle_x(0), [&](Expr p) {
      auto node_coord = floor(particle_x[p] * inv_dx - 0.5_f);
      grid_v[cast<int32>(node_coord(0)), cast<int32>(node_coord(1)),
             cast<int32>(node_coord(2))](0) = 1;
    });
  });

  for (int i = 0; i < n_particles; i++) {
    for (int d = 0; d < dim; d++) {
      particle_x(d).val<float32>(i) = p_x[i][d];
    }
  }

  int last_nb = -1;
  for (int i = 0; i < 2048; i++) {
    grid_m.parent().parent().snode()->clear_data_and_deactivate();
    sort();
    prog.synchronize();
    auto stat = grid_m.parent().parent().snode()->stat();
    int nb = stat.num_resident_blocks;
    if (last_nb == -1) {
      last_nb = nb;
      TC_P(last_nb);
    } else {
      if (last_nb != nb) {
        TC_P(i);
      }
      TC_CHECK(last_nb == nb);
    }
  }
};

TLANG_NAMESPACE_END
<commit_msg>simplified sort test<commit_after>#include <taichi/lang.h>
#include <taichi/testing.h>
#include <numeric>

TLANG_NAMESPACE_BEGIN

TC_TEST("gpu_gc_basics") {
  for (auto arch : {Arch::gpu}) {
    int n = 32;
    Program prog(arch);

    Global(x, i32);
    layout([&]() {
      auto i = Index(0);
      auto j = Index(1);
      root.dense(i, n).pointer().dense(j, n).place(x);
    });

    kernel([&]() {
      For(0, n, [&](Expr i) {
        For(0, i, [&](Expr j) { Activate(x.snode(), {i, j}); });
      });
    })();

    kernel([&]() {
      For(0, n, [&](Expr i) { For(0, i, [&](Expr j) { x[i, j] = i + j; }); });
    })();

    auto stat = x.parent().parent().snode()->stat();
    TC_CHECK(stat.num_resident_blocks == n - 1);
    for (int i = 0; i < n; i++) {
      for (int j = 0; j < i; j++) {
        TC_CHECK(x.val<int>(i, j) == i + j);
      }
    }
    x.parent().parent().snode()->clear_data_and_deactivate();
    stat = x.parent().parent().snode()->stat();
    TC_CHECK(stat.num_resident_blocks == 0);
    TC_CHECK(stat.num_recycled_blocks == 0);

    for (int i = 0; i < n; i++) {
      for (int j = 0; j < i; j++) {
        TC_CHECK(x.val<int>(i, j) == 0);
      }
    }
  }
};

TC_TEST("parallel_particle_sort") {
  Program prog(Arch::gpu);
  CoreState::set_trigger_gdb_when_crash(true);

  constexpr int n = 256;
  const real dx = 1.0_f / n, inv_dx = 1.0_f / dx;

  constexpr int dim = 3;

  auto f32 = DataType::f32;
  auto i32 = DataType::i32;
  int grid_block_size = 4;

  Vector particle_x(i32, dim);

  Vector grid_v(f32, dim);
  Global(grid_m, f32);

  int max_n_particles = 1024 * 1024;

  int n_particles = 0;
  std::vector<float> benchmark_particles;
  std::vector<Vector3> p_x;
  n_particles = max_n_particles;
  p_x.resize(n_particles);
  for (int i = 0; i < n_particles; i++) {
    Vector3 offset = Vector3::rand() - Vector3(0.5_f);
    p_x[i] = Vector3(0.5_f) + offset * 0.7f;
  }

  TC_ASSERT(n_particles <= max_n_particles);

  auto i = Index(0), j = Index(1), k = Index(2);
  auto p = Index(3);

  layout([&]() {
    auto &fork = root.dynamic(p, max_n_particles);
    for (int i = 0; i < dim; i++) {
      fork.place(particle_x(i));
    }

    TC_ASSERT(n % grid_block_size == 0);
    auto &block = root.dense({i, j, k}, n / grid_block_size).pointer();

    block.dense({i, j, k}, grid_block_size)
        .place(grid_v(0), grid_v(1), grid_v(2), grid_m);
  });

  TC_ASSERT(bit::is_power_of_two(n));

  Kernel(sort).def([&] {
    BlockDim(256);
    For(particle_x(0), [&](Expr p) { grid_v[particle_x[p]](0) = 1; });
  });

  for (int i = 0; i < n_particles; i++) {
    for (int d = 0; d < dim; d++) {
      particle_x(d).val<int32>(i) = p_x[i][d] * inv_dx;
    }
  }

  int last_nb = -1;
  for (int i = 0; i < 2048; i++) {
    grid_m.parent().parent().snode()->clear_data_and_deactivate();
    sort();
    prog.synchronize();
    auto stat = grid_m.parent().parent().snode()->stat();
    int nb = stat.num_resident_blocks;
    if (last_nb == -1) {
      last_nb = nb;
      TC_P(last_nb);
    } else {
      if (last_nb != nb) {
        TC_P(i);
      }
      TC_CHECK(last_nb == nb);
    }
  }
};

TLANG_NAMESPACE_END
<|endoftext|>


<commit_before>// -------------------------------------------------------------------------
//    @FileName         :    NFCProperty.cpp
//    @Author           :    LvSheng.Huang
//    @Date             :    2012-03-01
//    @Module           :    NFCProperty
//
// -------------------------------------------------------------------------

#include "NFCProperty.h"
#include <complex>

NFCProperty::NFCProperty()
{
    mbPublic = false;
    mbPrivate = false;
    mbSave = false;
    mSelf = NFGUID();
    eType = TDATA_UNKNOWN;

    msPropertyName = "";
}

NFCProperty::NFCProperty(const NFGUID& self, const std::string& strPropertyName, const TDATA_TYPE varType, bool bPublic,  bool bPrivate,  bool bSave, const std::string& strRelationValue)
{

    mbPublic = bPublic;
    mbPrivate = bPrivate;
    mbSave = bSave;
    mSelf = self;

    msPropertyName = strPropertyName;
    mstrRelationValue = strRelationValue;
    eType = varType;
}

NFCProperty::~NFCProperty()
{
    for (TPROPERTYCALLBACKEX::iterator iter = mtPropertyCallback.begin(); iter != mtPropertyCallback.end(); ++iter)
    {
        iter->reset();
    }

    mtPropertyCallback.clear();
    mxData.reset();
}

void NFCProperty::SetValue(const NFIDataList::TData& TData)
{
    if (eType != TData.GetType())
    {
        return;
    }

    if (!mxData.get())
    {
        if (!TData.IsNullValue())
        {
            return;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TData));
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->variantData = TData.variantData;

    NFCDataList::TData newValue;
    newValue = *mxData;

    OnEventHandler(oldValue , newValue);
}

void NFCProperty::SetValue(const NFIProperty* pProperty)
{
    SetValue(pProperty->GetValue());
}

const NFIDataList::TData& NFCProperty::GetValue() const
{
    if (mxData.get())
    {
        return *mxData;
    }

    return NULL_TDATA;
}

const std::string&  NFCProperty::GetKey() const
{
    return msPropertyName;
}

const bool NFCProperty::GetSave() const
{
    return mbSave;
}

const bool NFCProperty::GetPublic() const
{
    return mbPublic;
}

const bool NFCProperty::GetPrivate() const
{
    return mbPrivate;
}

const std::string& NFCProperty::GetRelationValue() const
{
    return mstrRelationValue;
}

void NFCProperty::SetSave(bool bSave)
{
    mbSave = bSave;
}

void NFCProperty::SetPublic(bool bPublic)
{
    mbPublic = bPublic;
}

void NFCProperty::SetPrivate(bool bPrivate)
{
    mbPrivate = bPrivate;
}

void NFCProperty::SetRelationValue(const std::string& strRelationValue)
{
    mstrRelationValue = strRelationValue;
}

NFINT64 NFCProperty::GetInt() const
{
    if (!mxData.get())
    {
        return 0;
    }

    return mxData->GetInt();
}

double NFCProperty::GetFloat() const
{
    if (!mxData.get())
    {
        return 0.0;
    }

    return mxData->GetFloat();
}

const std::string& NFCProperty::GetString() const
{
    if (!mxData.get())
    {
        return NULL_STR;
    }

    return mxData->GetString();
}

const NFGUID& NFCProperty::GetObject() const
{
    if (!mxData.get())
    {
        return NULL_OBJECT;
    }

    return mxData->GetObject();
}

void NFCProperty::RegisterCallback(const PROPERTY_EVENT_FUNCTOR_PTR& cb)
{
    mtPropertyCallback.push_back(cb);
}

int NFCProperty::OnEventHandler(const NFIDataList::TData& oldVar, const NFIDataList::TData& newVar)
{
    if (mtPropertyCallback.size() <= 0)
    {
        return 0;
    }

    TPROPERTYCALLBACKEX::iterator it = mtPropertyCallback.begin();
    TPROPERTYCALLBACKEX::iterator end = mtPropertyCallback.end();
    for (it; it != end; ++it)
    {
        //NFIDataList:OLDֵNEWֵ, ARG(pKernel,self)
        PROPERTY_EVENT_FUNCTOR_PTR& pFunPtr = *it;
        PROPERTY_EVENT_FUNCTOR* pFunc = pFunPtr.get();
        int nTemRet = pFunc->operator()(mSelf, msPropertyName, oldVar, newVar);
    }

    return 0;
}

bool NFCProperty::SetInt(const NFINT64 value)
{
    if (eType != TDATA_INT)
    {
        return false;
    }

    if (!mxData.get())
    {
        //ǿվΪûݣûݵľͲ
        if (0 == value)
        {
            return false;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TDATA_INT));
        mxData->SetInt(0);
    }

    if (value == mxData->GetInt())
    {
        return false;
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->SetInt(value);

    OnEventHandler(oldValue, *mxData);

    return true;
}

bool NFCProperty::SetFloat(const double value)
{
    if (eType != TDATA_FLOAT)
    {
        return false;
    }

    if (!mxData.get())
    {
        //ǿվΪûݣûݵľͲ
        if (std::abs(value) < 0.001)
        {
            return false;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TDATA_FLOAT));
        mxData->SetFloat(0.0);
    }

    if (value - mxData->GetFloat() < 0.001)
    {
        return false;
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->SetFloat(value);

    OnEventHandler(oldValue, *mxData);

    return true;
}

bool NFCProperty::SetString(const std::string& value)
{
    if (eType != TDATA_STRING)
    {
        return false;
    }

    if (!mxData.get())
    {
        //ǿվΪûݣûݵľͲ
        if (value.empty())
        {
            return false;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TDATA_STRING));
        mxData->SetString(NULL_STR);
    }

    if (value == mxData->GetString())
    {
        return false;
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->SetString(value);

    OnEventHandler(oldValue, *mxData);

    return true;
}

bool NFCProperty::SetObject(const NFGUID& value)
{
    if (eType != TDATA_OBJECT)
    {
        return false;
    }

    if (!mxData.get())
    {
        //ǿվΪûݣûݵľͲ
        if (value.IsNull())
        {
            return false;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TDATA_OBJECT));
        mxData->SetObject(NFGUID());

    }

    if (value == mxData->GetObject())
    {
        return false;
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->SetObject(value);

    OnEventHandler(oldValue , *mxData);

    return true;
}

bool NFCProperty::Changed() const
{
    return GetValue().IsNullValue();
}

const TDATA_TYPE NFCProperty::GetType() const
{
    return eType;
}

const bool NFCProperty::GeUsed() const
{
    if (mxData.get())
    {
        return true;
    }

    return false;
}

std::string NFCProperty::ToString()
{
    std::string strData;
    const TDATA_TYPE eType = GetType();
    switch (eType)
    {
        case TDATA_INT:
            strData = lexical_cast<std::string> (GetInt());
            break;
        case TDATA_FLOAT:
            strData = lexical_cast<std::string> (GetFloat());
            break;
        case TDATA_STRING:
            strData = GetString();
            break;
		case TDATA_OBJECT:
            strData = GetObject().ToString();
            break;
        default:
            strData = NULL_STR;
            break;
    }

    return strData;
}

bool NFCProperty::FromString(const std::string& strData)
{
    const TDATA_TYPE eType = GetType();
    bool bRet = false;
    switch (eType)
    {
        case TDATA_INT:
        {
            NFINT64  nValue = 0;
            bRet = NF_StrTo(strData, nValue);
            SetInt(nValue);
        }
        break;

        case TDATA_FLOAT:
        {
            double  dValue = 0;
            bRet = NF_StrTo(strData, dValue);
            SetFloat(dValue);
        }
        break;

        case TDATA_STRING:
        {
            SetString(strData);
            bRet = true;
        }
        break;
        case TDATA_OBJECT:
        {
            NFGUID xID;

            bRet = xID.FromString(strData);
            SetObject(xID);
        }
        break;
        default:
            break;
    }

    return bRet;
}

bool NFCProperty::DeSerialization()
{
    bool bRet = false;

    const TDATA_TYPE eType = GetType();
    if (eType == TDATA_STRING)
    {
        NFCDataList xDataList;
        const std::string& strData = mxData->GetString();

        xDataList.Split(strData.c_str(), ";");
        for (int i = 0; i < xDataList.GetCount(); ++i)
        {
            if (nullptr == mxEmbeddedList)
            {
                mxEmbeddedList = NF_SHARE_PTR<NFList<std::string>>(NF_NEW NFList<std::string>());
            }
            else
            {
                mxEmbeddedList->ClearAll();
            }

            if(xDataList.String(i).empty())
            {
                NFASSERT(0, strData, __FILE__, __FUNCTION__);
            }

            mxEmbeddedList->Add(xDataList.String(i));
        }

        if (nullptr != mxEmbeddedList && mxEmbeddedList->Count() > 0)
        {
            std::string strTemData;
            for (bool bListRet = mxEmbeddedList->First(strTemData); bListRet == true; bListRet = mxEmbeddedList->Next(strTemData))
            {
                NFCDataList xTemDataList;
                xTemDataList.Split(strTemData.c_str(), ",");
                if (xTemDataList.GetCount() > 0)
                {
                    if (xTemDataList.GetCount() != 2)
                    {
                        NFASSERT(0, strTemData, __FILE__, __FUNCTION__);
                    }

                    const std::string& strKey = xTemDataList.String(0);
                    const std::string& strValue = xTemDataList.String(0);

                    if (strKey.empty() || strValue.empty())
                    {
                        NFASSERT(0, strTemData, __FILE__, __FUNCTION__);
                    }

                    if (nullptr == mxEmbeddedMap)
                    {
                        mxEmbeddedMap = NF_SHARE_PTR<NFMapEx<std::string, std::string>>(NF_NEW NFMapEx<std::string, std::string>());
                    }
                    else
                    {
                        mxEmbeddedMap->ClearAll();
                    }

                    mxEmbeddedMap->AddElement(strKey, NF_SHARE_PTR<std::string>(NF_NEW std::string(strValue)))
                }
            }

            bRet = true;
        }
    }

    return bRet;
}

const NF_SHARE_PTR<NFList<std::string>> NFCProperty::GetEmbeddedList() const
{
    return this->mxEmbeddedList;
}

const NF_SHARE_PTR<NFMapEx<std::string, std::string>> NFCProperty::GetEmbeddedMap() const
{
    return this->mxEmbeddedMap;
}
<commit_msg>fixed<commit_after>// -------------------------------------------------------------------------
//    @FileName         :    NFCProperty.cpp
//    @Author           :    LvSheng.Huang
//    @Date             :    2012-03-01
//    @Module           :    NFCProperty
//
// -------------------------------------------------------------------------

#include "NFCProperty.h"
#include <complex>

NFCProperty::NFCProperty()
{
    mbPublic = false;
    mbPrivate = false;
    mbSave = false;
    mSelf = NFGUID();
    eType = TDATA_UNKNOWN;

    msPropertyName = "";
}

NFCProperty::NFCProperty(const NFGUID& self, const std::string& strPropertyName, const TDATA_TYPE varType, bool bPublic,  bool bPrivate,  bool bSave, const std::string& strRelationValue)
{

    mbPublic = bPublic;
    mbPrivate = bPrivate;
    mbSave = bSave;
    mSelf = self;

    msPropertyName = strPropertyName;
    mstrRelationValue = strRelationValue;
    eType = varType;
}

NFCProperty::~NFCProperty()
{
    for (TPROPERTYCALLBACKEX::iterator iter = mtPropertyCallback.begin(); iter != mtPropertyCallback.end(); ++iter)
    {
        iter->reset();
    }

    mtPropertyCallback.clear();
    mxData.reset();
}

void NFCProperty::SetValue(const NFIDataList::TData& TData)
{
    if (eType != TData.GetType())
    {
        return;
    }

    if (!mxData.get())
    {
        if (!TData.IsNullValue())
        {
            return;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TData));
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->variantData = TData.variantData;

    NFCDataList::TData newValue;
    newValue = *mxData;

    OnEventHandler(oldValue , newValue);
}

void NFCProperty::SetValue(const NFIProperty* pProperty)
{
    SetValue(pProperty->GetValue());
}

const NFIDataList::TData& NFCProperty::GetValue() const
{
    if (mxData.get())
    {
        return *mxData;
    }

    return NULL_TDATA;
}

const std::string&  NFCProperty::GetKey() const
{
    return msPropertyName;
}

const bool NFCProperty::GetSave() const
{
    return mbSave;
}

const bool NFCProperty::GetPublic() const
{
    return mbPublic;
}

const bool NFCProperty::GetPrivate() const
{
    return mbPrivate;
}

const std::string& NFCProperty::GetRelationValue() const
{
    return mstrRelationValue;
}

void NFCProperty::SetSave(bool bSave)
{
    mbSave = bSave;
}

void NFCProperty::SetPublic(bool bPublic)
{
    mbPublic = bPublic;
}

void NFCProperty::SetPrivate(bool bPrivate)
{
    mbPrivate = bPrivate;
}

void NFCProperty::SetRelationValue(const std::string& strRelationValue)
{
    mstrRelationValue = strRelationValue;
}

NFINT64 NFCProperty::GetInt() const
{
    if (!mxData.get())
    {
        return 0;
    }

    return mxData->GetInt();
}

double NFCProperty::GetFloat() const
{
    if (!mxData.get())
    {
        return 0.0;
    }

    return mxData->GetFloat();
}

const std::string& NFCProperty::GetString() const
{
    if (!mxData.get())
    {
        return NULL_STR;
    }

    return mxData->GetString();
}

const NFGUID& NFCProperty::GetObject() const
{
    if (!mxData.get())
    {
        return NULL_OBJECT;
    }

    return mxData->GetObject();
}

void NFCProperty::RegisterCallback(const PROPERTY_EVENT_FUNCTOR_PTR& cb)
{
    mtPropertyCallback.push_back(cb);
}

int NFCProperty::OnEventHandler(const NFIDataList::TData& oldVar, const NFIDataList::TData& newVar)
{
    if (mtPropertyCallback.size() <= 0)
    {
        return 0;
    }

    TPROPERTYCALLBACKEX::iterator it = mtPropertyCallback.begin();
    TPROPERTYCALLBACKEX::iterator end = mtPropertyCallback.end();
    for (it; it != end; ++it)
    {
        //NFIDataList:OLDֵNEWֵ, ARG(pKernel,self)
        PROPERTY_EVENT_FUNCTOR_PTR& pFunPtr = *it;
        PROPERTY_EVENT_FUNCTOR* pFunc = pFunPtr.get();
        int nTemRet = pFunc->operator()(mSelf, msPropertyName, oldVar, newVar);
    }

    return 0;
}

bool NFCProperty::SetInt(const NFINT64 value)
{
    if (eType != TDATA_INT)
    {
        return false;
    }

    if (!mxData.get())
    {
        //ǿվΪûݣûݵľͲ
        if (0 == value)
        {
            return false;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TDATA_INT));
        mxData->SetInt(0);
    }

    if (value == mxData->GetInt())
    {
        return false;
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->SetInt(value);

    OnEventHandler(oldValue, *mxData);

    return true;
}

bool NFCProperty::SetFloat(const double value)
{
    if (eType != TDATA_FLOAT)
    {
        return false;
    }

    if (!mxData.get())
    {
        //ǿվΪûݣûݵľͲ
        if (std::abs(value) < 0.001)
        {
            return false;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TDATA_FLOAT));
        mxData->SetFloat(0.0);
    }

    if (value - mxData->GetFloat() < 0.001)
    {
        return false;
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->SetFloat(value);

    OnEventHandler(oldValue, *mxData);

    return true;
}

bool NFCProperty::SetString(const std::string& value)
{
    if (eType != TDATA_STRING)
    {
        return false;
    }

    if (!mxData.get())
    {
        //ǿվΪûݣûݵľͲ
        if (value.empty())
        {
            return false;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TDATA_STRING));
        mxData->SetString(NULL_STR);
    }

    if (value == mxData->GetString())
    {
        return false;
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->SetString(value);

    OnEventHandler(oldValue, *mxData);

    return true;
}

bool NFCProperty::SetObject(const NFGUID& value)
{
    if (eType != TDATA_OBJECT)
    {
        return false;
    }

    if (!mxData.get())
    {
        //ǿվΪûݣûݵľͲ
        if (value.IsNull())
        {
            return false;
        }

        mxData = NF_SHARE_PTR<NFIDataList::TData>(NF_NEW NFIDataList::TData(TDATA_OBJECT));
        mxData->SetObject(NFGUID());

    }

    if (value == mxData->GetObject())
    {
        return false;
    }

    NFCDataList::TData oldValue;
    oldValue = *mxData;

    mxData->SetObject(value);

    OnEventHandler(oldValue , *mxData);

    return true;
}

bool NFCProperty::Changed() const
{
    return GetValue().IsNullValue();
}

const TDATA_TYPE NFCProperty::GetType() const
{
    return eType;
}

const bool NFCProperty::GeUsed() const
{
    if (mxData.get())
    {
        return true;
    }

    return false;
}

std::string NFCProperty::ToString()
{
    std::string strData;
    const TDATA_TYPE eType = GetType();
    switch (eType)
    {
        case TDATA_INT:
            strData = lexical_cast<std::string> (GetInt());
            break;
        case TDATA_FLOAT:
            strData = lexical_cast<std::string> (GetFloat());
            break;
        case TDATA_STRING:
            strData = GetString();
            break;
		case TDATA_OBJECT:
            strData = GetObject().ToString();
            break;
        default:
            strData = NULL_STR;
            break;
    }

    return strData;
}

bool NFCProperty::FromString(const std::string& strData)
{
    const TDATA_TYPE eType = GetType();
    bool bRet = false;
    switch (eType)
    {
        case TDATA_INT:
        {
            NFINT64  nValue = 0;
            bRet = NF_StrTo(strData, nValue);
            SetInt(nValue);
        }
        break;

        case TDATA_FLOAT:
        {
            double  dValue = 0;
            bRet = NF_StrTo(strData, dValue);
            SetFloat(dValue);
        }
        break;

        case TDATA_STRING:
        {
            SetString(strData);
            bRet = true;
        }
        break;
        case TDATA_OBJECT:
        {
            NFGUID xID;

            bRet = xID.FromString(strData);
            SetObject(xID);
        }
        break;
        default:
            break;
    }

    return bRet;
}

bool NFCProperty::DeSerialization()
{
    bool bRet = false;

    const TDATA_TYPE eType = GetType();
    if (eType == TDATA_STRING)
    {
        NFCDataList xDataList;
        const std::string& strData = mxData->GetString();

        xDataList.Split(strData.c_str(), ";");
        for (int i = 0; i < xDataList.GetCount(); ++i)
        {
            if (nullptr == mxEmbeddedList)
            {
                mxEmbeddedList = NF_SHARE_PTR<NFList<std::string>>(NF_NEW NFList<std::string>());
            }
            else
            {
                mxEmbeddedList->ClearAll();
            }

            if(xDataList.String(i).empty())
            {
                NFASSERT(0, strData, __FILE__, __FUNCTION__);
            }

            mxEmbeddedList->Add(xDataList.String(i));
        }

        if (nullptr != mxEmbeddedList && mxEmbeddedList->Count() > 0)
        {
            std::string strTemData;
            for (bool bListRet = mxEmbeddedList->First(strTemData); bListRet == true; bListRet = mxEmbeddedList->Next(strTemData))
            {
                NFCDataList xTemDataList;
                xTemDataList.Split(strTemData.c_str(), ",");
                if (xTemDataList.GetCount() > 0)
                {
                    if (xTemDataList.GetCount() != 2)
                    {
                        NFASSERT(0, strTemData, __FILE__, __FUNCTION__);
                    }

                    const std::string& strKey = xTemDataList.String(0);
                    const std::string& strValue = xTemDataList.String(0);

                    if (strKey.empty() || strValue.empty())
                    {
                        NFASSERT(0, strTemData, __FILE__, __FUNCTION__);
                    }

                    if (nullptr == mxEmbeddedMap)
                    {
                        mxEmbeddedMap = NF_SHARE_PTR<NFMapEx<std::string, std::string>>(NF_NEW NFMapEx<std::string, std::string>());
                    }
                    else
                    {
                        mxEmbeddedMap->ClearAll();
                    }

                    mxEmbeddedMap->AddElement(strKey, NF_SHARE_PTR<std::string>(NF_NEW std::string(strValue)));
                }
            }

            bRet = true;
        }
    }

    return bRet;
}

const NF_SHARE_PTR<NFList<std::string>> NFCProperty::GetEmbeddedList() const
{
    return this->mxEmbeddedList;
}

const NF_SHARE_PTR<NFMapEx<std::string, std::string>> NFCProperty::GetEmbeddedMap() const
{
    return this->mxEmbeddedMap;
}
<|endoftext|>


<commit_before>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "QmitkStdMultiWidgetEditor.h"

#include <cherryUIException.h>
#include <cherryIWorkbenchPage.h>

#include <QWidget>

#include <mitkGlobalInteraction.h>

#include <mitkDataStorageEditorInput.h>

const std::string QmitkStdMultiWidgetEditor::EDITOR_ID = "org.mitk.editors.stdmultiwidget";

QmitkStdMultiWidgetEditor::QmitkStdMultiWidgetEditor()
 : m_StdMultiWidget(0)
{

}

QmitkStdMultiWidgetEditor::~QmitkStdMultiWidgetEditor()
{
  // we need to wrap the RemovePartListener call inside a
  // register/unregister block to prevent infinite recursion
  // due to the destruction of temporary smartpointer to this
  this->Register();
  this->GetSite()->GetPage()->RemovePartListener(cherry::IPartListener::Pointer(this));
  this->UnRegister(false);
}

QmitkStdMultiWidget* QmitkStdMultiWidgetEditor::GetStdMultiWidget()
{
  return m_StdMultiWidget;
}

void QmitkStdMultiWidgetEditor::Init(cherry::IEditorSite::Pointer site, cherry::IEditorInput::Pointer input)
{
  if (input.Cast<mitk::DataStorageEditorInput>().IsNull())
     throw cherry::PartInitException("Invalid Input: Must be IFileEditorInput");

  this->SetSite(site);
  this->SetInput(input);
}

void QmitkStdMultiWidgetEditor::CreateQtPartControl(QWidget* parent)
{
  if (m_StdMultiWidget == 0)
  {
    m_DndFrameWidget = new QmitkDnDFrameWidget(parent);
    QVBoxLayout* layout = new QVBoxLayout(parent);
    layout->addWidget(m_DndFrameWidget);
    layout->setContentsMargins(0,0,0,0);

    m_StdMultiWidget = new QmitkStdMultiWidget(m_DndFrameWidget);
    QVBoxLayout* layout2 = new QVBoxLayout(m_DndFrameWidget);
    layout2->addWidget(m_StdMultiWidget);
    layout2->setContentsMargins(0,0,0,0);

    mitk::DataStorage::Pointer ds = this->GetEditorInput().Cast<mitk::DataStorageEditorInput>()
      ->GetDataStorageReference()->GetDataStorage();

    // Tell the multiWidget which (part of) the tree to render
    m_StdMultiWidget->SetDataStorage(ds);

    // Initialize views as transversal, sagittal, coronar to all data objects in DataStorage
    // (from top-left to bottom)
    mitk::TimeSlicedGeometry::Pointer geo = ds->ComputeBoundingGeometry3D(ds->GetAll());
    mitk::RenderingManager::GetInstance()->InitializeViews(geo);

    // Initialize bottom-right view as 3D view
    m_StdMultiWidget->GetRenderWindow4()->GetRenderer()->SetMapperID(
      mitk::BaseRenderer::Standard3D );

    // Enable standard handler for levelwindow-slider
    m_StdMultiWidget->EnableStandardLevelWindow();

    // Add the displayed views to the tree to see their positions
    // in 2D and 3D
    m_StdMultiWidget->AddDisplayPlaneSubTree();

    m_StdMultiWidget->EnableNavigationControllerEventListening();

    mitk::GlobalInteraction::GetInstance()->AddListener(
        m_StdMultiWidget->GetMoveAndZoomInteractor()
      );
    this->GetSite()->GetPage()->AddPartListener(cherry::IPartListener::Pointer(this));
  }
}

cherry::IPartListener::Events::Types QmitkStdMultiWidgetEditor::GetPartEventTypes() const
{
  return Events::CLOSED | Events::HIDDEN | Events::VISIBLE;
}

void QmitkStdMultiWidgetEditor::PartClosed( cherry::IWorkbenchPartReference::Pointer partRef )
{
  if (partRef->GetId() == QmitkStdMultiWidgetEditor::EDITOR_ID)
  {
    QmitkStdMultiWidgetEditor::Pointer stdMultiWidgetEditor = partRef->GetPart(false).Cast<QmitkStdMultiWidgetEditor>();

    if (m_StdMultiWidget == stdMultiWidgetEditor->GetStdMultiWidget())
    {
      m_StdMultiWidget->RemovePlanesFromDataStorage();
    }
  }
}

void QmitkStdMultiWidgetEditor::PartVisible( cherry::IWorkbenchPartReference::Pointer partRef )
{
  if (partRef->GetId() == QmitkStdMultiWidgetEditor::EDITOR_ID)
  {
    QmitkStdMultiWidgetEditor::Pointer stdMultiWidgetEditor = partRef->GetPart(false).Cast<QmitkStdMultiWidgetEditor>();

    if (m_StdMultiWidget == stdMultiWidgetEditor->GetStdMultiWidget())
    {
      m_StdMultiWidget->AddPlanesToDataStorage();
    }
  }
}

void QmitkStdMultiWidgetEditor::PartHidden( cherry::IWorkbenchPartReference::Pointer partRef )
{
  if (partRef->GetId() == QmitkStdMultiWidgetEditor::EDITOR_ID)
  {
    QmitkStdMultiWidgetEditor::Pointer stdMultiWidgetEditor = partRef->GetPart(false).Cast<QmitkStdMultiWidgetEditor>();

    if (m_StdMultiWidget == stdMultiWidgetEditor->GetStdMultiWidget())
    {
      m_StdMultiWidget->RemovePlanesFromDataStorage();
    }
  }
}

void QmitkStdMultiWidgetEditor::SetFocus()
{
  if (m_StdMultiWidget != 0)
    m_StdMultiWidget->setFocus();
}
<commit_msg>ENH (#1850): added preference for logo file location with ID "DepartmentLogo"<commit_after>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "QmitkStdMultiWidgetEditor.h"

#include <cherryUIException.h>
#include <cherryIWorkbenchPage.h>
#include <cherryIPreferencesService.h>

#include <QWidget>

#include <mitkGlobalInteraction.h>

#include <mitkDataStorageEditorInput.h>

const std::string QmitkStdMultiWidgetEditor::EDITOR_ID = "org.mitk.editors.stdmultiwidget";

QmitkStdMultiWidgetEditor::QmitkStdMultiWidgetEditor()
 : m_StdMultiWidget(0)
{

}

QmitkStdMultiWidgetEditor::~QmitkStdMultiWidgetEditor()
{
  // we need to wrap the RemovePartListener call inside a
  // register/unregister block to prevent infinite recursion
  // due to the destruction of temporary smartpointer to this
  this->Register();
  this->GetSite()->GetPage()->RemovePartListener(cherry::IPartListener::Pointer(this));
  this->UnRegister(false);
}

QmitkStdMultiWidget* QmitkStdMultiWidgetEditor::GetStdMultiWidget()
{
  return m_StdMultiWidget;
}

void QmitkStdMultiWidgetEditor::Init(cherry::IEditorSite::Pointer site, cherry::IEditorInput::Pointer input)
{
  if (input.Cast<mitk::DataStorageEditorInput>().IsNull())
     throw cherry::PartInitException("Invalid Input: Must be IFileEditorInput");

  this->SetSite(site);
  this->SetInput(input);
}

void QmitkStdMultiWidgetEditor::CreateQtPartControl(QWidget* parent)
{
  if (m_StdMultiWidget == 0)
  {
    m_DndFrameWidget = new QmitkDnDFrameWidget(parent);
    QVBoxLayout* layout = new QVBoxLayout(parent);
    layout->addWidget(m_DndFrameWidget);
    layout->setContentsMargins(0,0,0,0);

    m_StdMultiWidget = new QmitkStdMultiWidget(m_DndFrameWidget);
    QVBoxLayout* layout2 = new QVBoxLayout(m_DndFrameWidget);
    layout2->addWidget(m_StdMultiWidget);
    layout2->setContentsMargins(0,0,0,0);

    mitk::DataStorage::Pointer ds = this->GetEditorInput().Cast<mitk::DataStorageEditorInput>()
      ->GetDataStorageReference()->GetDataStorage();

    // Tell the multiWidget which (part of) the tree to render
    m_StdMultiWidget->SetDataStorage(ds);

    // Initialize views as transversal, sagittal, coronar to all data objects in DataStorage
    // (from top-left to bottom)
    mitk::TimeSlicedGeometry::Pointer geo = ds->ComputeBoundingGeometry3D(ds->GetAll());
    mitk::RenderingManager::GetInstance()->InitializeViews(geo);

    // Initialize bottom-right view as 3D view
    m_StdMultiWidget->GetRenderWindow4()->GetRenderer()->SetMapperID(
      mitk::BaseRenderer::Standard3D );

    // Enable standard handler for levelwindow-slider
    m_StdMultiWidget->EnableStandardLevelWindow();

    // Add the displayed views to the tree to see their positions
    // in 2D and 3D
    m_StdMultiWidget->AddDisplayPlaneSubTree();

    m_StdMultiWidget->EnableNavigationControllerEventListening();

    mitk::GlobalInteraction::GetInstance()->AddListener(
        m_StdMultiWidget->GetMoveAndZoomInteractor()
      );
    this->GetSite()->GetPage()->AddPartListener(cherry::IPartListener::Pointer(this));

    // enable change of logo
    cherry::IPreferencesService::Pointer prefService
      = cherry::Platform::GetServiceRegistry()
      .GetServiceById<cherry::IPreferencesService>(cherry::IPreferencesService::ID);

    cherry::IPreferences::Pointer logoPref = prefService->GetSystemPreferences()->Node("DepartmentLogo");
    std::string departmentLogoLocation = logoPref->Get("DepartmentLogo","");

    m_StdMultiWidget->SetDepartmentLogoPath(departmentLogoLocation.c_str());
    m_StdMultiWidget->DisableDepartmentLogo();
    m_StdMultiWidget->EnableDepartmentLogo();
  }
}

cherry::IPartListener::Events::Types QmitkStdMultiWidgetEditor::GetPartEventTypes() const
{
  return Events::CLOSED | Events::HIDDEN | Events::VISIBLE;
}

void QmitkStdMultiWidgetEditor::PartClosed( cherry::IWorkbenchPartReference::Pointer partRef )
{
  if (partRef->GetId() == QmitkStdMultiWidgetEditor::EDITOR_ID)
  {
    QmitkStdMultiWidgetEditor::Pointer stdMultiWidgetEditor = partRef->GetPart(false).Cast<QmitkStdMultiWidgetEditor>();

    if (m_StdMultiWidget == stdMultiWidgetEditor->GetStdMultiWidget())
    {
      m_StdMultiWidget->RemovePlanesFromDataStorage();
    }
  }
}

void QmitkStdMultiWidgetEditor::PartVisible( cherry::IWorkbenchPartReference::Pointer partRef )
{
  if (partRef->GetId() == QmitkStdMultiWidgetEditor::EDITOR_ID)
  {
    QmitkStdMultiWidgetEditor::Pointer stdMultiWidgetEditor = partRef->GetPart(false).Cast<QmitkStdMultiWidgetEditor>();

    if (m_StdMultiWidget == stdMultiWidgetEditor->GetStdMultiWidget())
    {
      m_StdMultiWidget->AddPlanesToDataStorage();
    }
  }
}

void QmitkStdMultiWidgetEditor::PartHidden( cherry::IWorkbenchPartReference::Pointer partRef )
{
  if (partRef->GetId() == QmitkStdMultiWidgetEditor::EDITOR_ID)
  {
    QmitkStdMultiWidgetEditor::Pointer stdMultiWidgetEditor = partRef->GetPart(false).Cast<QmitkStdMultiWidgetEditor>();

    if (m_StdMultiWidget == stdMultiWidgetEditor->GetStdMultiWidget())
    {
      m_StdMultiWidget->RemovePlanesFromDataStorage();
    }
  }
}

void QmitkStdMultiWidgetEditor::SetFocus()
{
  if (m_StdMultiWidget != 0)
    m_StdMultiWidget->setFocus();
}
<|endoftext|>


<commit_before>/**
 * \file timeout.cc
 *
 *
 *
 * \author Ethan Burns
 * \date 2008-12-16
 */

#include <iostream>

#include <string.h>
#include <signal.h>
#include <stdlib.h>
#include <unistd.h>

#include "../search.h"

using namespace std;

static volatile bool timeout_stopped = false;

extern "C" void alarm_action(int sig)
{
	if (timeout_stopped)
		return;

	output_search_stats_on_timeout();

	cout << "# Time out" << endl
	     << "cost: infinity" << endl
	     << "length: infinity" << endl
	     << "wall_time: infinity" << endl
	     << "CPU_time: infinity" << endl
	     << "generated: infinity" << endl
	     << "expanded: infinity" << endl;

	exit(EXIT_SUCCESS);
}

void timeout(unsigned int sec)
{
	struct sigaction sa;

	memset(&sa, '\0', sizeof(sa));

	sa.sa_handler = alarm_action;
	sigfillset(&sa.sa_mask);
	sigaction(SIGALRM, &sa, NULL);

	timeout_stopped = false;
	alarm(sec);
}

void timeout_stop(void)
{
	timeout_stopped = true;
}
<commit_msg>Output what the timelimit was when there is a timeout.<commit_after>/**
 * \file timeout.cc
 *
 *
 *
 * \author Ethan Burns
 * \date 2008-12-16
 */

#include <iostream>

#include <string.h>
#include <signal.h>
#include <stdlib.h>
#include <unistd.h>

#include "../search.h"

using namespace std;

static volatile bool timeout_stopped = false;

static unsigned int timelimit = 0;

extern "C" void alarm_action(int sig)
{
	if (timeout_stopped)
		return;

	output_search_stats_on_timeout();

	cout << "# Time out" << endl
	     << "time-limit: " << timelimit << endl
	     << "cost: infinity" << endl
	     << "length: infinity" << endl
	     << "wall_time: infinity" << endl
	     << "CPU_time: infinity" << endl
	     << "generated: infinity" << endl
	     << "expanded: infinity" << endl;

	exit(EXIT_SUCCESS);
}

void timeout(unsigned int sec)
{
	struct sigaction sa;

	timelimit = sec;

	memset(&sa, '\0', sizeof(sa));
	sa.sa_handler = alarm_action;
	sigfillset(&sa.sa_mask);
	sigaction(SIGALRM, &sa, NULL);

	timeout_stopped = false;
	alarm(sec);
}

void timeout_stop(void)
{
	timeout_stopped = true;
}
<|endoftext|>


<commit_before>#ifndef _UTILS_ALIGN_HPP_
#define _UTILS_ALIGN_HPP_

template<typename T>
static inline T align_downto(T n, T align) { return n - (n % align); }

template<typename T>
static inline T align_upto(T n, T align) { return n % align ? align_downto(n, align) + align : n; }

#endif
<commit_msg>Remove utils/align.hpp because it duplicates functionality found in utils.hpp and also it's not being used anywhere.<commit_after><|endoftext|>


<commit_before>#include <iostream>
#include <math.h>
#include <string.h>
#include <vector>
#include "mex.h"

#include <nix.hpp>

#include "handle.h"
#include "arguments.h"
#include "struct.h"
#include "datatypes.h"

void check_arg_type(const mxArray *arr, nix::DataType dtype) {
    if (dtype_mex2nix(arr) != dtype) {
        throw std::invalid_argument("wrong type");
    }
}

mxArray *nmCreateScalar(uint32_t val) {
    mxArray *arr = mxCreateNumericMatrix(1, 1, mxUINT32_CLASS, mxREAL);
    void *data = mxGetData(arr);
    memcpy(data, &val, sizeof(uint32_t));
    return arr;
}

mxArray *dim_to_struct(nix::SetDimension dim) {

    struct_builder sb({ 1 }, { "type", "type_id", "labels" });

    sb.set("set");
    sb.set(1);
    sb.set(dim.labels());

    return sb.array();
}

mxArray *dim_to_struct(nix::SampledDimension dim) {

    struct_builder sb({ 1 }, { "type", "type_id", "interval", "label", "unit" });

    sb.set("sampled");
    sb.set(2);
    sb.set(dim.samplingInterval());
    sb.set(dim.label());
    sb.set(dim.unit());

    return sb.array();
}

mxArray *dim_to_struct(nix::RangeDimension dim) {

    struct_builder sb({ 1 }, { "type", "type_id", "ticks", "unit" });

    sb.set("range");
    sb.set(3);
    sb.set(dim.ticks());
    sb.set(dim.unit());

    return sb.array();
}

nix::NDSize mx_array_to_ndsize(const mxArray *arr) {

    size_t m = mxGetM(arr);
    size_t n = mxGetN(arr);

    //if (m != 1 && n != 1)

    size_t k = std::max(n, m);
    nix::NDSize size(k);

    double *data = mxGetPr(arr);
    for (size_t i = 0; i < size.size(); i++) {
        size[i] = static_cast<nix::NDSize::value_type>(data[i]);
    }

    return size;
}

std::string mx_array_to_str(const mxArray *arr) {
    check_arg_type(arr, nix::DataType::String);

    char *tmp = mxArrayToString(arr);
    std::string the_string(tmp);
    mxFree(tmp);
    return the_string;
}

template<typename T>
T mx_array_to_num(const mxArray *arr) {
    check_arg_type(arr, nix::to_data_type<T>::value);

    if (mxGetNumberOfElements(arr) < 1) {
        throw std::runtime_error("array empty");
    }

    const void *data = mxGetData(arr);
    T res;
    memcpy(&res, data, sizeof(T));
    return res;
}

bool mx_array_to_bool(const mxArray *arr) {
    check_arg_type(arr, nix::DataType::Bool);

    const mxLogical *l = mxGetLogicals(arr);
    return l[0];
}

nix::Value mx_array_to_value(const mxArray *arr) {
    nix::Value val;

    switch (mxGetClassID(arr)) {
    case mxLOGICAL_CLASS: val.set(mx_array_to_bool(arr)); break;
    case mxCHAR_CLASS: val.set(mx_array_to_str(arr)); break;
    case mxDOUBLE_CLASS: val.set(mx_array_to_num<double>(arr)); break;
    case mxUINT32_CLASS: val.set(mx_array_to_num<uint32_t>(arr)); break;
    case mxINT32_CLASS:  val.set(mx_array_to_num<int32_t>(arr)); break;
    case mxUINT64_CLASS: val.set(mx_array_to_num<uint64_t>(arr)); break;
    case mxINT64_CLASS:  val.set(mx_array_to_num<int64_t>(arr)); break;

    case mxSINGLE_CLASS: throw std::invalid_argument("Element type is not supported");
    case mxUINT8_CLASS:  throw std::invalid_argument("Element type is not supported");
    case mxINT8_CLASS:   throw std::invalid_argument("Element type is not supported");
    case mxUINT16_CLASS: throw std::invalid_argument("Element type is not supported");
    case mxINT16_CLASS:  throw std::invalid_argument("Element type is not supported");

    default: throw std::invalid_argument("Element type is not recognized");
    }

    return val;
}

<commit_msg>string construction fix<commit_after>#include <iostream>
#include <math.h>
#include <string.h>
#include <vector>
#include "mex.h"

#include <nix.hpp>

#include "handle.h"
#include "arguments.h"
#include "struct.h"
#include "datatypes.h"

void check_arg_type(const mxArray *arr, nix::DataType dtype) {
    if (dtype_mex2nix(arr) != dtype) {
        throw std::invalid_argument("wrong type");
    }
}

mxArray *nmCreateScalar(uint32_t val) {
    mxArray *arr = mxCreateNumericMatrix(1, 1, mxUINT32_CLASS, mxREAL);
    void *data = mxGetData(arr);
    memcpy(data, &val, sizeof(uint32_t));
    return arr;
}

mxArray *dim_to_struct(nix::SetDimension dim) {

    struct_builder sb({ 1 }, { "type", "type_id", "labels" });

    sb.set("set");
    sb.set(1);
    sb.set(dim.labels());

    return sb.array();
}

mxArray *dim_to_struct(nix::SampledDimension dim) {

    struct_builder sb({ 1 }, { "type", "type_id", "interval", "label", "unit" });

    sb.set("sampled");
    sb.set(2);
    sb.set(dim.samplingInterval());
    sb.set(dim.label());
    sb.set(dim.unit());

    return sb.array();
}

mxArray *dim_to_struct(nix::RangeDimension dim) {

    struct_builder sb({ 1 }, { "type", "type_id", "ticks", "unit" });

    sb.set("range");
    sb.set(3);
    sb.set(dim.ticks());
    sb.set(dim.unit());

    return sb.array();
}

nix::NDSize mx_array_to_ndsize(const mxArray *arr) {

    size_t m = mxGetM(arr);
    size_t n = mxGetN(arr);

    //if (m != 1 && n != 1)

    size_t k = std::max(n, m);
    nix::NDSize size(k);

    double *data = mxGetPr(arr);
    for (size_t i = 0; i < size.size(); i++) {
        size[i] = static_cast<nix::NDSize::value_type>(data[i]);
    }

    return size;
}

std::string mx_array_to_str(const mxArray *arr) {
    check_arg_type(arr, nix::DataType::String);

    std::string the_string = mxArrayToString(arr);
    return the_string;
}

template<typename T>
T mx_array_to_num(const mxArray *arr) {
    check_arg_type(arr, nix::to_data_type<T>::value);

    if (mxGetNumberOfElements(arr) < 1) {
        throw std::runtime_error("array empty");
    }

    const void *data = mxGetData(arr);
    T res;
    memcpy(&res, data, sizeof(T));
    return res;
}

bool mx_array_to_bool(const mxArray *arr) {
    check_arg_type(arr, nix::DataType::Bool);

    const mxLogical *l = mxGetLogicals(arr);
    return l[0];
}

nix::Value mx_array_to_value(const mxArray *arr) {
    nix::Value val;

    switch (mxGetClassID(arr)) {
    case mxLOGICAL_CLASS: val.set(mx_array_to_bool(arr)); break;
    case mxCHAR_CLASS: val.set(mx_array_to_str(arr)); break;
    case mxDOUBLE_CLASS: val.set(mx_array_to_num<double>(arr)); break;
    case mxUINT32_CLASS: val.set(mx_array_to_num<uint32_t>(arr)); break;
    case mxINT32_CLASS:  val.set(mx_array_to_num<int32_t>(arr)); break;
    case mxUINT64_CLASS: val.set(mx_array_to_num<uint64_t>(arr)); break;
    case mxINT64_CLASS:  val.set(mx_array_to_num<int64_t>(arr)); break;

    case mxSINGLE_CLASS: throw std::invalid_argument("Element type is not supported");
    case mxUINT8_CLASS:  throw std::invalid_argument("Element type is not supported");
    case mxINT8_CLASS:   throw std::invalid_argument("Element type is not supported");
    case mxUINT16_CLASS: throw std::invalid_argument("Element type is not supported");
    case mxINT16_CLASS:  throw std::invalid_argument("Element type is not supported");

    default: throw std::invalid_argument("Element type is not recognized");
    }

    return val;
}

<|endoftext|>


<commit_before>/*
 * Stock of connections to a memcached server.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "memcached_stock.hxx"
#include "memcached_client.hxx"
#include "stock/GetHandler.hxx"
#include "stock/Item.hxx"
#include "tcp_stock.hxx"
#include "tcp_balancer.hxx"
#include "address_list.hxx"
#include "lease.hxx"
#include "pool.hxx"
#include "istream/istream.hxx"
#include "net/SocketAddress.hxx"

#include <glib.h>

#include <assert.h>
#include <string.h>
#include <sys/socket.h>

struct MemachedStock {
    TcpBalancer &tcp_balancer;

    const AddressList &address;

    MemachedStock(TcpBalancer &_tcp_balancer,
                  const AddressList &_address)
        :tcp_balancer(_tcp_balancer),
         address(_address) {}
};

MemachedStock *
memcached_stock_new(TcpBalancer *tcp_balancer,
                    const AddressList *address)
{
    return new MemachedStock(*tcp_balancer, *address);
}

void
memcached_stock_free(MemachedStock *stock)
{
    delete stock;
}

struct MemcachedStockRequest final : public StockGetHandler, Lease {
    struct pool *pool;

    StockItem *item;

    enum memcached_opcode opcode;

    const void *extras;
    size_t extras_length;

    const void *key;
    size_t key_length;

    Istream *value;

    const struct memcached_client_handler *handler;
    void *handler_ctx;

    struct async_operation_ref *async_ref;

    /* virtual methods from class StockGetHandler */
    void OnStockItemReady(StockItem &item) override;
    void OnStockItemError(GError *error) override;

    /* virtual methods from class Lease */
    void ReleaseLease(bool reuse) override {
        item->Put(!reuse);
    }
};

/*
 * stock callback
 *
 */

void
MemcachedStockRequest::OnStockItemReady(StockItem &_item)
{
    item = &_item;

    memcached_client_invoke(pool, tcp_stock_item_get(_item),
                            tcp_stock_item_get_domain(_item) == AF_LOCAL
                            ? FdType::FD_SOCKET : FdType::FD_TCP,
                            *this,
                            opcode,
                            extras, extras_length,
                            key, key_length,
                            value,
                            handler, handler_ctx,
                            async_ref);
}

void
MemcachedStockRequest::OnStockItemError(GError *error)
{
    handler->error(error, handler_ctx);

    if (value != nullptr)
        value->CloseUnused();
}

void
memcached_stock_invoke(struct pool *pool, MemachedStock *stock,
                       enum memcached_opcode opcode,
                       const void *extras, size_t extras_length,
                       const void *key, size_t key_length,
                       Istream *value,
                       const struct memcached_client_handler *handler,
                       void *handler_ctx,
                       struct async_operation_ref *async_ref)
{
    auto request = PoolAlloc<MemcachedStockRequest>(*pool);

    assert(extras_length <= MEMCACHED_EXTRAS_MAX);
    assert(key_length <= MEMCACHED_KEY_MAX);

    request->pool = pool;
    request->opcode = opcode;
    request->extras = extras;
    request->extras_length = extras_length;
    request->key = key;
    request->key_length = key_length;
    request->value = value;
    request->handler = handler;
    request->handler_ctx = handler_ctx;
    request->async_ref = async_ref;

    tcp_balancer_get(stock->tcp_balancer, *pool,
                     false, SocketAddress::Null(),
                     0, stock->address,
                     10,
                     *request, *async_ref);
}
<commit_msg>memcached/stock: add MemcachedStockRequest constructor<commit_after>/*
 * Stock of connections to a memcached server.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "memcached_stock.hxx"
#include "memcached_client.hxx"
#include "stock/GetHandler.hxx"
#include "stock/Item.hxx"
#include "tcp_stock.hxx"
#include "tcp_balancer.hxx"
#include "address_list.hxx"
#include "lease.hxx"
#include "pool.hxx"
#include "istream/istream.hxx"
#include "net/SocketAddress.hxx"

#include <glib.h>

#include <assert.h>
#include <string.h>
#include <sys/socket.h>

struct MemachedStock {
    TcpBalancer &tcp_balancer;

    const AddressList &address;

    MemachedStock(TcpBalancer &_tcp_balancer,
                  const AddressList &_address)
        :tcp_balancer(_tcp_balancer),
         address(_address) {}
};

MemachedStock *
memcached_stock_new(TcpBalancer *tcp_balancer,
                    const AddressList *address)
{
    return new MemachedStock(*tcp_balancer, *address);
}

void
memcached_stock_free(MemachedStock *stock)
{
    delete stock;
}

struct MemcachedStockRequest final : public StockGetHandler, Lease {
    struct pool &pool;

    StockItem *item;

    const enum memcached_opcode opcode;

    const void *const extras;
    const size_t extras_length;

    const void *const key;
    const size_t key_length;

    Istream *const value;

    const struct memcached_client_handler &handler;
    void *const handler_ctx;

    struct async_operation_ref &async_ref;

    MemcachedStockRequest(struct pool &_pool,
                          enum memcached_opcode _opcode,
                          const void *_extras, size_t _extras_length,
                          const void *_key, size_t _key_length,
                          Istream *_value,
                          const struct memcached_client_handler &_handler,
                          void *_handler_ctx,
                          struct async_operation_ref &_async_ref)
        :pool(_pool),
         opcode(_opcode),
         extras(_extras), extras_length(_extras_length),
         key(_key), key_length(_key_length),
         value(_value),
         handler(_handler), handler_ctx(_handler_ctx),
         async_ref(_async_ref) {}

    /* virtual methods from class StockGetHandler */
    void OnStockItemReady(StockItem &item) override;
    void OnStockItemError(GError *error) override;

    /* virtual methods from class Lease */
    void ReleaseLease(bool reuse) override {
        item->Put(!reuse);
    }
};

/*
 * stock callback
 *
 */

void
MemcachedStockRequest::OnStockItemReady(StockItem &_item)
{
    item = &_item;

    memcached_client_invoke(&pool, tcp_stock_item_get(_item),
                            tcp_stock_item_get_domain(_item) == AF_LOCAL
                            ? FdType::FD_SOCKET : FdType::FD_TCP,
                            *this,
                            opcode,
                            extras, extras_length,
                            key, key_length,
                            value,
                            &handler, handler_ctx,
                            &async_ref);
}

void
MemcachedStockRequest::OnStockItemError(GError *error)
{
    handler.error(error, handler_ctx);

    if (value != nullptr)
        value->CloseUnused();
}

void
memcached_stock_invoke(struct pool *pool, MemachedStock *stock,
                       enum memcached_opcode opcode,
                       const void *extras, size_t extras_length,
                       const void *key, size_t key_length,
                       Istream *value,
                       const struct memcached_client_handler *handler,
                       void *handler_ctx,
                       struct async_operation_ref *async_ref)
{
    assert(extras_length <= MEMCACHED_EXTRAS_MAX);
    assert(key_length <= MEMCACHED_KEY_MAX);

    auto request = NewFromPool<MemcachedStockRequest>(*pool, *pool,
                                                      opcode,
                                                      extras, extras_length,
                                                      key, key_length,
                                                      value,
                                                      *handler, handler_ctx,
                                                      *async_ref);

    tcp_balancer_get(stock->tcp_balancer, *pool,
                     false, SocketAddress::Null(),
                     0, stock->address,
                     10,
                     *request, *async_ref);
}
<|endoftext|>


<commit_before>#pragma once

#include <string>
#include <vector>

#include "config.hpp"
#include "span.hpp"

namespace utki{

/**
 * @brief Format string.
 * @param format - printf-like string format.
 * @return formatted string.
 */
#if M_COMPILER != M_COMPILER_MSVC
__attribute__((format(printf, 1, 2)))
#endif
std::string make_string(const char* format, ...);

/**
 * @brief Format string.
 * va_list version of make_string().
 * @param format - printf-like string format.
 * @param args - va_list of string arguments.
 * @return formatted string.
 */
std::string make_string_va_list(const char* format, va_list args);

/**
 * @brief Make string out of char buffer.
 * @param buf - char buffer to make the string from.
 * @return string representing the contents of the char buffer.
 */
std::string make_string(const utki::span<char> buf){
	return std::string(buf.data(), buf.size());
}

/**
 * @brief Split string using give delimiter.
 * @param str - string to split.
 * @param delimiter - delimiter character to use as a splitter.
 * @return vector of splitted strings.
 */
std::vector<std::string> split(const std::string& str, char delimiter);

/**
 * @brief Word wrap string.
 * @param str - string to word wrap.
 * @param width - maximum number of characters per line.
 * @return list of word wrapped lines.
 */
std::vector<std::string> word_wrap(const std::string& str, unsigned width);

}
<commit_msg>build fix<commit_after>#pragma once

#include <string>
#include <vector>

#include "config.hpp"
#include "span.hpp"

namespace utki{

/**
 * @brief Format string.
 * @param format - printf-like string format.
 * @return formatted string.
 */
#if M_COMPILER != M_COMPILER_MSVC
__attribute__((format(printf, 1, 2)))
#endif
std::string make_string(const char* format, ...);

/**
 * @brief Format string.
 * va_list version of make_string().
 * @param format - printf-like string format.
 * @param args - va_list of string arguments.
 * @return formatted string.
 */
std::string make_string_va_list(const char* format, va_list args);

/**
 * @brief Make string out of char buffer.
 * @param buf - char buffer to make the string from.
 * @return string representing the contents of the char buffer.
 */
inline std::string make_string(const utki::span<char> buf){
	return std::string(buf.data(), buf.size());
}

/**
 * @brief Split string using give delimiter.
 * @param str - string to split.
 * @param delimiter - delimiter character to use as a splitter.
 * @return vector of splitted strings.
 */
std::vector<std::string> split(const std::string& str, char delimiter);

/**
 * @brief Word wrap string.
 * @param str - string to word wrap.
 * @param width - maximum number of characters per line.
 * @return list of word wrapped lines.
 */
std::vector<std::string> word_wrap(const std::string& str, unsigned width);

}
<|endoftext|>


<commit_before>/*
 * madgwick.cpp
 * Copyright (c) 2018, Robot Mitya.
 * 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 Robot Mitya 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.
 *
 *  Created on: Feb 17, 2018
 *      Author: Dmitry Dzakhov
 *    Based on: Implementation of Madgwick's IMU and AHRS algorithms.
 *              See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
 */

#include <math.h>
#include "madgwick.h"

static tf2::Matrix3x3 m_;
static tf2::Quaternion eulerQuaternion_;
static tf2::Quaternion rotationZMinus90_(tf2::Vector3(0, 0, 1), -MadgwickImu::PI / 2.0f);

MadgwickImu::MadgwickImu()
{
  qSource_.setValue(0, 0, 0, 1);
  qCenter_.setValue(0, 0, 0, 1);
  qResult_ = qSource_ * qCenter_;

  tf2::Vector3 z(0, 0, 1);
  rotationZMinus90_.setRotation(z, -PI / 2.0f);
}

void MadgwickImu::center()
{
  qCenter_ = qSource_.inverse();
}

void MadgwickImu::update(float deltaTime,
            float gx, float gy, float gz,
            float ax, float ay, float az)
{
  float q1 = qSource_.w();
  float q2 = qSource_.x();
  float q3 = qSource_.y();
  float q4 = qSource_.z();
  float norm;
  float s1, s2, s3, s4;
  float qDot1, qDot2, qDot3, qDot4;

  // Auxiliary variables to avoid repeated arithmetic
  float _2q1 = 2.0f * q1;
  float _2q2 = 2.0f * q2;
  float _2q3 = 2.0f * q3;
  float _2q4 = 2.0f * q4;
  float _4q1 = 4.0f * q1;
  float _4q2 = 4.0f * q2;
  float _4q3 = 4.0f * q3;
  float _8q2 = 8.0f * q2;
  float _8q3 = 8.0f * q3;
  float q1q1 = q1 * q1;
  float q2q2 = q2 * q2;
  float q3q3 = q3 * q3;
  float q4q4 = q4 * q4;

  // Normalize accelerometer measurement
  norm = (float) sqrt(ax * ax + ay * ay + az * az);
  if (norm < 0.0001f) return; // handle NaN
  norm = 1.0f / norm;         // use reciprocal for division
  ax *= norm;
  ay *= norm;
  az *= norm;

  // Gradient decent algorithm corrective step
  s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
  s2 = _4q2 * q4q4 - _2q4 * ax + 4.0f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
  s3 = 4.0f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
  s4 = 4.0f * q2q2 * q4 - _2q2 * ax + 4.0f * q3q3 * q4 - _2q3 * ay;
  norm = 1.0f / (float) sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalize step magnitude
  s1 *= norm;
  s2 *= norm;
  s3 *= norm;
  s4 *= norm;

  // Compute rate of change of quaternion
  qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - BETA * s1;
  qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - BETA * s2;
  qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - BETA * s3;
  qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - BETA * s4;

  // Integrate to yield quaternion
  q1 += qDot1 * deltaTime;
  q2 += qDot2 * deltaTime;
  q3 += qDot3 * deltaTime;
  q4 += qDot4 * deltaTime;
  norm = 1.0f / (float) sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalize quaternion
  q1 *= norm;
  q2 *= norm;
  q3 *= norm;
  q4 *= norm;
  qSource_.setValue(q2, q3, q4, q1);
  qResult_ = qSource_ * qCenter_;
}

void MadgwickImu::getQuaternion(tf2Scalar& x, tf2Scalar& y, tf2Scalar& z, tf2Scalar& w)
{
  x = qResult_.x();
  y = qResult_.y();
  z = qResult_.z();
  w = qResult_.w();
}

void MadgwickImu::getEulerYP(tf2::Quaternion & quaternion, tf2Scalar& yaw, tf2Scalar& pitch)
{
//  tf2::Vector3 z(0, 0, 1);
//  tf2::Quaternion rotationZMinus90_(z, -MadgwickImu::PI / 2.0f);

  eulerQuaternion_ = quaternion * rotationZMinus90_;
  m_.setRotation(eulerQuaternion_);
  tf2Scalar temp;
  m_.getEulerYPR(yaw, temp, pitch, 1);
  yaw *= TO_DEG;
  yaw += 90;
  if (yaw > 180) yaw -= 360;
  pitch *= TO_DEG;
}

void MadgwickImu::getEulerYP(tf2Scalar& yaw, tf2Scalar& pitch)
{
  MadgwickImu::getEulerYP(qResult_, yaw, pitch);
}

/**
 * Fast inverse square-root
 * See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
 */
float MadgwickImu::invSqrt(float x)
{
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;
  i = 0x5f3759df - (i >> 1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));
  return y;
//  return 1.0f / sqrt(x);
}
<commit_msg>#15 Madgwick center<commit_after>/*
 * madgwick.cpp
 * Copyright (c) 2018, Robot Mitya.
 * 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 Robot Mitya 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.
 *
 *  Created on: Feb 17, 2018
 *      Author: Dmitry Dzakhov
 *    Based on: Implementation of Madgwick's IMU and AHRS algorithms.
 *              See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
 */

#include <math.h>
#include "madgwick.h"

static tf2::Matrix3x3 m_;
static tf2::Quaternion eulerQuaternion_;
static tf2::Quaternion rotationZMinus90_(tf2::Vector3(0, 0, 1), -MadgwickImu::PI / 2.0f);

MadgwickImu::MadgwickImu()
{
  qSource_.setValue(0, 0, 0, 1);
  qCenter_.setValue(0, 0, 0, 1);
  qResult_ = qSource_ * qCenter_;

  tf2::Vector3 z(0, 0, 1);
  rotationZMinus90_.setRotation(z, -PI / 2.0f);
}

void MadgwickImu::center()
{
  tf2::Matrix3x3 matrix;
  matrix.setRotation(qSource_);
  tf2Scalar yaw, pitch, roll;
  matrix.getEulerYPR(yaw, pitch, roll);
  matrix.setEulerYPR(yaw, 0, 0);
  matrix.getRotation(qCenter_);
  qCenter_ = qCenter_.inverse();

  //qCenter_ = qSource_.inverse();
}

void MadgwickImu::update(float deltaTime,
            float gx, float gy, float gz,
            float ax, float ay, float az)
{
  float q1 = qSource_.w();
  float q2 = qSource_.x();
  float q3 = qSource_.y();
  float q4 = qSource_.z();
  float norm;
  float s1, s2, s3, s4;
  float qDot1, qDot2, qDot3, qDot4;

  // Auxiliary variables to avoid repeated arithmetic
  float _2q1 = 2.0f * q1;
  float _2q2 = 2.0f * q2;
  float _2q3 = 2.0f * q3;
  float _2q4 = 2.0f * q4;
  float _4q1 = 4.0f * q1;
  float _4q2 = 4.0f * q2;
  float _4q3 = 4.0f * q3;
  float _8q2 = 8.0f * q2;
  float _8q3 = 8.0f * q3;
  float q1q1 = q1 * q1;
  float q2q2 = q2 * q2;
  float q3q3 = q3 * q3;
  float q4q4 = q4 * q4;

  // Normalize accelerometer measurement
  norm = (float) sqrt(ax * ax + ay * ay + az * az);
  if (norm < 0.0001f) return; // handle NaN
  norm = 1.0f / norm;         // use reciprocal for division
  ax *= norm;
  ay *= norm;
  az *= norm;

  // Gradient decent algorithm corrective step
  s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
  s2 = _4q2 * q4q4 - _2q4 * ax + 4.0f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
  s3 = 4.0f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
  s4 = 4.0f * q2q2 * q4 - _2q2 * ax + 4.0f * q3q3 * q4 - _2q3 * ay;
  norm = 1.0f / (float) sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalize step magnitude
  s1 *= norm;
  s2 *= norm;
  s3 *= norm;
  s4 *= norm;

  // Compute rate of change of quaternion
  qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - BETA * s1;
  qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - BETA * s2;
  qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - BETA * s3;
  qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - BETA * s4;

  // Integrate to yield quaternion
  q1 += qDot1 * deltaTime;
  q2 += qDot2 * deltaTime;
  q3 += qDot3 * deltaTime;
  q4 += qDot4 * deltaTime;
  norm = 1.0f / (float) sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalize quaternion
  q1 *= norm;
  q2 *= norm;
  q3 *= norm;
  q4 *= norm;
  qSource_.setValue(q2, q3, q4, q1);
  qResult_ = qSource_ * qCenter_;
}

void MadgwickImu::getQuaternion(tf2Scalar& x, tf2Scalar& y, tf2Scalar& z, tf2Scalar& w)
{
  x = qResult_.x();
  y = qResult_.y();
  z = qResult_.z();
  w = qResult_.w();
}

void MadgwickImu::getEulerYP(tf2::Quaternion & quaternion, tf2Scalar& yaw, tf2Scalar& pitch)
{
//  tf2::Vector3 z(0, 0, 1);
//  tf2::Quaternion rotationZMinus90_(z, -MadgwickImu::PI / 2.0f);

  eulerQuaternion_ = quaternion * rotationZMinus90_;
  m_.setRotation(eulerQuaternion_);
  tf2Scalar temp;
  m_.getEulerYPR(yaw, temp, pitch, 1);
  yaw *= TO_DEG;
  yaw += 90;
  if (yaw > 180) yaw -= 360;
  pitch *= TO_DEG;
}

void MadgwickImu::getEulerYP(tf2Scalar& yaw, tf2Scalar& pitch)
{
  MadgwickImu::getEulerYP(qResult_, yaw, pitch);
}

/**
 * Fast inverse square-root
 * See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
 */
float MadgwickImu::invSqrt(float x)
{
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;
  i = 0x5f3759df - (i >> 1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));
  return y;
//  return 1.0f / sqrt(x);
}
<|endoftext|>


<commit_before>//
//  IsingTest.cpp
//  jdemon
//
//  Created by Mark Larus on 3/4/13.
//  Copyright (c) 2013 Kenyon College. All rights reserved.
//

#include <boost/test/unit_test.hpp>
#include <set>
#include <algorithm>
#include <stack>

#include "Ising.h"
#include "Utilities.h"

#include "TestFixtures.h"
#include "MockObjects.h"

class TransitionRuleTestFixture : \
    public DefaultTransitionRuleTestFixture, \
    public ValidStatesTestFixture \
{};

BOOST_FIXTURE_TEST_SUITE(TransitionRuleTest, TransitionRuleTestFixture);

BOOST_AUTO_TEST_CASE ( testTableSize ) {
    for (TransitionRule::iterator it = defaultRule.begin(); it!=defaultRule.end(); ++it) {
        BOOST_REQUIRE(it->second.size() == 8 && "Transition table size should be 8");
    }
}

BOOST_AUTO_TEST_CASE ( testTableDeadEnds ) {
    typedef TransitionRule::iterator RuleIterator;
    typedef TransitionTable::iterator TableIterator;

    for (RuleIterator currentRuleIterator = defaultRule.begin(); \
         currentRuleIterator!=defaultRule.end(); ++currentRuleIterator) {

        TransitionTable currentRule = currentRuleIterator->second;

        for (TableIterator currentTableIterator = currentRule.begin();\
             currentTableIterator != currentRule.end(); ++currentTableIterator) {
            BOOST_CHECK(currentTableIterator->second);
        }
    }
}

BOOST_AUTO_TEST_CASE ( testValidTargetStates ) {
    typedef TransitionRule::iterator RuleIterator;
    typedef TransitionTable::iterator TableIterator;

    for (RuleIterator currentRuleIterator = defaultRule.begin(); \
         currentRuleIterator!=defaultRule.end(); ++currentRuleIterator) {

        TransitionTable currentRule = currentRuleIterator->second;

        for (TableIterator currentTableIterator = currentRule.begin();\
             currentTableIterator != currentRule.end(); ++currentTableIterator) {
            SystemState *targetState = currentTableIterator->second;
            BOOST_CHECK(validStates.count(targetState));
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

class IsingReservoirTestFixture : \
    public RandomNumberTestFixture,
    public ConstantsTestFixture,
    public ValidStatesTestFixture
{};

BOOST_FIXTURE_TEST_SUITE(IsingReservoirTest, IsingReservoirTestFixture);

BOOST_AUTO_TEST_CASE ( testWheelStep ) {
    IsingReservoir reservoir(rng,c,6,6);
    Reservoir::InteractionResult result;
    BOOST_REQUIRE_NO_THROW(reservoir.wheelStep(result));
}

BOOST_AUTO_TEST_CASE ( testEmptyTransitionRule ) {
    TransitionRule emptyRule;
    IsingReservoir reservoir(rng,c,6,6,emptyRule);
    Reservoir::InteractionResult result;
    BOOST_REQUIRE_THROW(reservoir.wheelStep(result), EmptyTransitionRuleError);
}

BOOST_AUTO_TEST_CASE ( testDeadEndTransitionRule ) {
    TransitionRule deadEndRule;
    TransitionTable emptyTable;
    for (char k=0; k<8; k++) {
        emptyTable[k] = NULL;
    }

    Reservoir::InteractionResult result;

    for (StateSet::iterator it = validStates.begin(); it!=validStates.end(); ++it) {
        deadEndRule[*it] = emptyTable;
    }
    IsingReservoir reservoir(rng,c,6,6,deadEndRule);
    BOOST_REQUIRE_THROW(reservoir.wheelStep(result),TransitionDeadEndError);
}

BOOST_AUTO_TEST_CASE ( testTooSmallTransitionTable ) {
    TransitionRule invalidRule;
    TransitionTable tooSmallTable;

    for (char k=0; k<3; k++) {
        tooSmallTable[k] = NULL;
    }

    for (StateSet::iterator it = validStates.begin(); it!=validStates.end(); ++it) {
        invalidRule[*it] = tooSmallTable;
    }

    IsingReservoir reservoir(rng,c,6,6,invalidRule);
    Reservoir::InteractionResult result;
    BOOST_REQUIRE_THROW(reservoir.wheelStep(result),InvalidTableSizeError);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(IsingUtilityTest)

BOOST_AUTO_TEST_CASE( testNonbinaryParity ) {
    int inputState = Ising::s(0,0,157);
    BOOST_REQUIRE(inputState == 1);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(ClusterMethodTest, GridOperationTestFixture)

BOOST_AUTO_TEST_CASE( includeNoCells ) {
    Ising::Cell *startingPoint = grid[5];
    MockRandomnessDelegate delegate(false,0);
    ClusterMethodAgent agent(&delegate,1);
    agent.performMethodAtCell(startingPoint);

    std::set<Ising::Cell *> expectedCells;
    expectedCells.insert(startingPoint);

    std::set<Ising::Cell *> actualCells = changedCells();
    BOOST_REQUIRE_EQUAL_COLLECTIONS(expectedCells.begin(), expectedCells.end(), actualCells.begin(), actualCells.end());
}

BOOST_AUTO_TEST_CASE( includeAllCells ) {
    // This breaks if the default cell value is non-zero.

    Ising::Cell *startingPoint = grid[5];
    MockRandomnessDelegate delegate(true,0);
    ClusterMethodAgent agent(&delegate,1);
    agent.performMethodAtCell(startingPoint);

    std::set<Ising::Cell *> expectedCells;
    expectedCells.insert(grid.begin(),grid.end());

    std::set<Ising::Cell *> actualCells = changedCells();

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expectedCells.begin(), expectedCells.end(), actualCells.begin(), actualCells.end());
}

BOOST_AUTO_TEST_CASE( includeSomeCells ) {
    Ising::Cell *startingPoint = grid[5];
    std::set<Ising::Cell *> expectedCells;
    std::stack<Ising::Cell *> stack;
    stack.push(startingPoint);

    size_t size = 12;

    while (expectedCells.size()!=size) {
        if (stack.empty()) {
            BOOST_FAIL("Stack should not be empty.");
        }
        Ising::Cell *currentCell = stack.top();
        stack.pop();
        Ising::Cell::Neighbors neighbors = currentCell->getNeighbors();
        for (Ising::Cell::Neighbors::iterator it = neighbors.begin(); it!=neighbors.end(); ++it) {
            if (!expectedCells.count(*it)) {
                stack.push(*it);
            }
        }
        expectedCells.insert(currentCell);
    }

    BOOST_REQUIRE_EQUAL(expectedCells.size(),size);

    for (Ising::Grid::iterator it = grid.begin(); it!=grid.end(); ++it) {
        if (expectedCells.count(*it)) {
            (*it)->setValue(1);
        } else {
            (*it)->setValue(0);
        }
    }

    resetInitialValues();

    MockRandomnessDelegate delegate(true,0);
    ClusterMethodAgent agent(&delegate,1);
    agent.performMethodAtCell(startingPoint);

    std::set<Ising::Cell *> actualCells = changedCells();

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expectedCells.begin(), expectedCells.end(), actualCells.begin(), actualCells.end());
}

BOOST_AUTO_TEST_CASE( testInclusionProbability ) {
    BOOST_CHECK_THROW(ClusterMethodAgent(NULL,-0.01), InvalidProbabilityError);
    BOOST_CHECK_THROW(ClusterMethodAgent(NULL,1.01), InvalidProbabilityError);
}

BOOST_AUTO_TEST_SUITE_END()<commit_msg>Include all cells no longer fails if the default cell value is non-zero.<commit_after>//
//  IsingTest.cpp
//  jdemon
//
//  Created by Mark Larus on 3/4/13.
//  Copyright (c) 2013 Kenyon College. All rights reserved.
//

#include <boost/test/unit_test.hpp>
#include <set>
#include <algorithm>
#include <stack>

#include "Ising.h"
#include "Utilities.h"

#include "TestFixtures.h"
#include "MockObjects.h"

class TransitionRuleTestFixture : \
    public DefaultTransitionRuleTestFixture, \
    public ValidStatesTestFixture \
{};

BOOST_FIXTURE_TEST_SUITE(TransitionRuleTest, TransitionRuleTestFixture);

BOOST_AUTO_TEST_CASE ( testTableSize ) {
    for (TransitionRule::iterator it = defaultRule.begin(); it!=defaultRule.end(); ++it) {
        BOOST_REQUIRE(it->second.size() == 8 && "Transition table size should be 8");
    }
}

BOOST_AUTO_TEST_CASE ( testTableDeadEnds ) {
    typedef TransitionRule::iterator RuleIterator;
    typedef TransitionTable::iterator TableIterator;

    for (RuleIterator currentRuleIterator = defaultRule.begin(); \
         currentRuleIterator!=defaultRule.end(); ++currentRuleIterator) {

        TransitionTable currentRule = currentRuleIterator->second;

        for (TableIterator currentTableIterator = currentRule.begin();\
             currentTableIterator != currentRule.end(); ++currentTableIterator) {
            BOOST_CHECK(currentTableIterator->second);
        }
    }
}

BOOST_AUTO_TEST_CASE ( testValidTargetStates ) {
    typedef TransitionRule::iterator RuleIterator;
    typedef TransitionTable::iterator TableIterator;

    for (RuleIterator currentRuleIterator = defaultRule.begin(); \
         currentRuleIterator!=defaultRule.end(); ++currentRuleIterator) {

        TransitionTable currentRule = currentRuleIterator->second;

        for (TableIterator currentTableIterator = currentRule.begin();\
             currentTableIterator != currentRule.end(); ++currentTableIterator) {
            SystemState *targetState = currentTableIterator->second;
            BOOST_CHECK(validStates.count(targetState));
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

class IsingReservoirTestFixture : \
    public RandomNumberTestFixture,
    public ConstantsTestFixture,
    public ValidStatesTestFixture
{};

BOOST_FIXTURE_TEST_SUITE(IsingReservoirTest, IsingReservoirTestFixture);

BOOST_AUTO_TEST_CASE ( testWheelStep ) {
    IsingReservoir reservoir(rng,c,6,6);
    Reservoir::InteractionResult result;
    BOOST_REQUIRE_NO_THROW(reservoir.wheelStep(result));
}

BOOST_AUTO_TEST_CASE ( testEmptyTransitionRule ) {
    TransitionRule emptyRule;
    IsingReservoir reservoir(rng,c,6,6,emptyRule);
    Reservoir::InteractionResult result;
    BOOST_REQUIRE_THROW(reservoir.wheelStep(result), EmptyTransitionRuleError);
}

BOOST_AUTO_TEST_CASE ( testDeadEndTransitionRule ) {
    TransitionRule deadEndRule;
    TransitionTable emptyTable;
    for (char k=0; k<8; k++) {
        emptyTable[k] = NULL;
    }

    Reservoir::InteractionResult result;

    for (StateSet::iterator it = validStates.begin(); it!=validStates.end(); ++it) {
        deadEndRule[*it] = emptyTable;
    }
    IsingReservoir reservoir(rng,c,6,6,deadEndRule);
    BOOST_REQUIRE_THROW(reservoir.wheelStep(result),TransitionDeadEndError);
}

BOOST_AUTO_TEST_CASE ( testTooSmallTransitionTable ) {
    TransitionRule invalidRule;
    TransitionTable tooSmallTable;

    for (char k=0; k<3; k++) {
        tooSmallTable[k] = NULL;
    }

    for (StateSet::iterator it = validStates.begin(); it!=validStates.end(); ++it) {
        invalidRule[*it] = tooSmallTable;
    }

    IsingReservoir reservoir(rng,c,6,6,invalidRule);
    Reservoir::InteractionResult result;
    BOOST_REQUIRE_THROW(reservoir.wheelStep(result),InvalidTableSizeError);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(IsingUtilityTest)

BOOST_AUTO_TEST_CASE( testNonbinaryParity ) {
    int inputState = Ising::s(0,0,157);
    BOOST_REQUIRE(inputState == 1);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(ClusterMethodTest, GridOperationTestFixture)

BOOST_AUTO_TEST_CASE( includeNoCells ) {
    Ising::Cell *startingPoint = grid[5];
    MockRandomnessDelegate delegate(false,0);
    ClusterMethodAgent agent(&delegate,1);
    agent.performMethodAtCell(startingPoint);

    std::set<Ising::Cell *> expectedCells;
    expectedCells.insert(startingPoint);

    std::set<Ising::Cell *> actualCells = changedCells();
    BOOST_REQUIRE_EQUAL_COLLECTIONS(expectedCells.begin(), expectedCells.end(), actualCells.begin(), actualCells.end());
}

BOOST_AUTO_TEST_CASE( includeAllCells ) {
    for (Ising::Grid::iterator it = grid.begin(); it!=grid.end(); ++it) {
        (*it)->setValue(0);
    }

    Ising::Cell *startingPoint = grid[5];
    MockRandomnessDelegate delegate(true,0);
    ClusterMethodAgent agent(&delegate,1);
    agent.performMethodAtCell(startingPoint);

    std::set<Ising::Cell *> expectedCells;
    expectedCells.insert(grid.begin(),grid.end());

    std::set<Ising::Cell *> actualCells = changedCells();

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expectedCells.begin(), expectedCells.end(), actualCells.begin(), actualCells.end());
}

BOOST_AUTO_TEST_CASE( includeSomeCells ) {
    Ising::Cell *startingPoint = grid[5];
    std::set<Ising::Cell *> expectedCells;
    std::stack<Ising::Cell *> stack;
    stack.push(startingPoint);

    size_t size = 12;

    while (expectedCells.size()!=size) {
        if (stack.empty()) {
            BOOST_FAIL("Stack should not be empty.");
        }
        Ising::Cell *currentCell = stack.top();
        stack.pop();
        Ising::Cell::Neighbors neighbors = currentCell->getNeighbors();
        for (Ising::Cell::Neighbors::iterator it = neighbors.begin(); it!=neighbors.end(); ++it) {
            if (!expectedCells.count(*it)) {
                stack.push(*it);
            }
        }
        expectedCells.insert(currentCell);
    }

    BOOST_REQUIRE_EQUAL(expectedCells.size(),size);

    for (Ising::Grid::iterator it = grid.begin(); it!=grid.end(); ++it) {
        if (expectedCells.count(*it)) {
            (*it)->setValue(1);
        } else {
            (*it)->setValue(0);
        }
    }

    resetInitialValues();

    MockRandomnessDelegate delegate(true,0);
    ClusterMethodAgent agent(&delegate,1);
    agent.performMethodAtCell(startingPoint);

    std::set<Ising::Cell *> actualCells = changedCells();

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expectedCells.begin(), expectedCells.end(), actualCells.begin(), actualCells.end());
}

BOOST_AUTO_TEST_CASE( testInclusionProbability ) {
    BOOST_CHECK_THROW(ClusterMethodAgent(NULL,-0.01), InvalidProbabilityError);
    BOOST_CHECK_THROW(ClusterMethodAgent(NULL,1.01), InvalidProbabilityError);
}

BOOST_AUTO_TEST_SUITE_END()<|endoftext|>


<commit_before>#include "mssqlclrgeo.hpp"
#include <cstring>

namespace mssqlclr
{

	const std::vector<Figure> Geometry::getFiguresFromShape(int shapeIdx) const {
		std::vector<Figure> figures_list;
		if (Figures.empty()) {
			return figures_list;
		}
		int32_t offset = Shapes[shapeIdx].FigureOffset;

		if (Shapes.size() > shapeIdx + 1) {
			int32_t offsetEnd = Shapes[shapeIdx + 1].FigureOffset;

			figures_list = std::vector<Figure>(Figures.cbegin() + offset, Figures.cbegin() + offsetEnd);
		}
		else {
			figures_list = std::vector<Figure>(Figures.cbegin() + offset, Figures.cend());
		}
		return figures_list;
	}

	const std::vector<Point> Geometry::getPointsFromFigure(int figureIdx) const {
		std::vector<Point> points_list;

		if (Points.empty()) {
			return points_list;
		}

		int32_t offset = Figures[figureIdx].Offset;

		if (Figures.size() > figureIdx + 1) {
			int32_t offsetEnd = Figures[figureIdx + 1].Offset;
			points_list = std::vector<Point>(Points.cbegin() + offset, Points.cbegin() + offsetEnd);
		}
		else {
			points_list = std::vector<Point>(Points.cbegin() + offset, Points.cend());
		}
		return points_list;
	}

	sqlgeo_reader::sqlgeo_reader(const char* data, std::size_t size)
		: data_(data),
		size_(size),
		pos_(0)
	{
	}

	uint32_t sqlgeo_reader::read_uint32()
	{
		std::uint32_t val;
		if (pos_ + 4 > size_) {
			return 0;
		}
		auto data = data_ + pos_;
		std::memcpy(&val, data, 4);
		pos_ += 4;

		return val;
	}

	uint16_t sqlgeo_reader::read_uint16()
	{
		std::uint16_t val;
		if (pos_ + 2 > size_) {
			return 0;
		}
		auto data = data_ + pos_;
		std::memcpy(&val, data, 2);
		pos_ += 2;

		return val;
	}
	uint8_t sqlgeo_reader::read_uint8()
	{
		std::uint8_t val;
		if (pos_ + 1 > size_) {
			return 0;
		}
		auto data = data_ + pos_;
		std::memcpy(&val, data, 1);
		pos_ += 1;

		return val;
	}
	double sqlgeo_reader::read_double()
	{
		double val;
		if (pos_ + 8 > size_) {
			return 0;
		}
		auto data = data_ + pos_;
		std::memcpy(&val, &data[0], 8);
		pos_ += 8;

		return val;
	}

	void sqlgeo_reader::readPointsZ(std::vector<Point> points) {

		if (points.empty()) {
			return;
		}
		for (auto point : points)
		{
			point.Z = read_double();
		}

	}

	void sqlgeo_reader::readPointsM(std::vector<Point> points) {

		if (points.empty()) {
			return;
		}
		for (auto point : points)
		{
			point.M = read_double();
		}

	}
	std::vector<Point> sqlgeo_reader::readPoints(uint32_t count, bool isGeography) {
		std::vector<Point> points;

		if (count < 1) {
			return std::vector<Point>();
		}

		for (uint32_t i = 0; i < count; i++) {
			if (isGeography) {
				double y = read_double();
				double x = read_double();
				points.emplace_back(x, y);
			}
			else {
				double x = read_double();
				double y = read_double();
				points.emplace_back(x, y);
			}

		}

		return points;
	}

	std::vector<Figure> sqlgeo_reader::readFigures(uint32_t count, SerializationProperties properties) {
		std::vector<Figure> figures;
		if (count == 0) {
			return figures;
		}

		if (properties.P || properties.L) {
			figures.emplace_back(FIGURE_STROKE, 0);
		}
		else {
			for (int i = 0; i < count; i++) {

				Figure f;
				f.Attribute = (FIGURE)read_uint8();
				f.Offset = read_uint32();
				figures.push_back(f);
			}
		}
		return figures;
	}
	std::vector<Shape> sqlgeo_reader::readShapes(uint32_t count, SerializationProperties properties) {
		std::vector<Shape> shapes;
		if (count < 1) {
			return shapes;
		}

		if (properties.P) {
			shapes.emplace_back(-1, 0, SHAPE_POINT);
		}
		else if (properties.L) {
			shapes.emplace_back(-1, 0, SHAPE_LINESTRING);
		}
		else {
			for (int i = 0; i < int(count); i++) {
				uint32_t parent_offset = read_uint32();
				uint32_t figure_offset = read_uint32();
				SHAPE type = (SHAPE)read_uint8();
				shapes.emplace_back(parent_offset, figure_offset, type);
			}
		}
		return shapes;
	}
	std::vector<Segment> sqlgeo_reader::readSegments(uint32_t count) {
		std::vector<Segment> segments;
		if (count < 1) {
			return segments;
		}

		for (int i = 0; i < int(count); i++) {
			Segment s;
			s.Type = (SEGMENT)read_uint8();
			segments.push_back(s);
		}
		return segments;
	}

	Geometry sqlgeo_reader::parseGeography() {
		return parseGeometry(true);
	}

	Geometry sqlgeo_reader::parseGeometry(bool isGeography) {

		Geometry g = {};

		uint32_t numberOfPoints;
		uint32_t numberOfFigures;
		uint32_t numberOfShapes;
		uint32_t numberOfSegments;

		g.SRID = read_uint32();

		if (isGeography == true) {
			if (g.SRID == -1) {
				return g;
			}
			else if (g.SRID < 4210 || g.SRID > 4999) {
				g.Properties.V = false;
				return g;
				//("Invalid SRID for geography");
			}
		}

		//version
		g.Version = read_uint8();

		if (g.Version > 2) {
			g.Properties.V = false;
			return g;
			//("Version %d is not supported", g.Version);
		}

		//flags
		uint8_t flags = 0;
		flags = read_uint8();

		g.Properties.Z = (flags & (1 << 0)) != 0;
		g.Properties.M = (flags & (1 << 1)) != 0;
		g.Properties.V = (flags & (1 << 2)) != 0;
		g.Properties.P = (flags & (1 << 3)) != 0;
		g.Properties.L = (flags & (1 << 4)) != 0;

		if (g.Version == 2) {
			g.Properties.H = (flags & (1 << 5)) != 0;
		}
		if (g.Properties.P && g.Properties.L) {
			g.Properties.V = false;
			return g;
			//("geometry data is invalid");
		}

		//points
		if (g.Properties.P) {
			numberOfPoints = 1;
		}
		else if (g.Properties.L) {
			numberOfPoints = 2;
		}
		else {
			numberOfPoints = read_uint32();
		}
		g.Points = readPoints(numberOfPoints, isGeography);

		if (g.Properties.Z) {
			readPointsZ(g.Points);
		}
		if (g.Properties.M) {
			readPointsM(g.Points);
		}

		//figures
		if (g.Properties.P || g.Properties.L) {
			numberOfFigures = 1;
		}
		else {
			numberOfFigures = read_uint32();
		}

		g.Figures = readFigures(numberOfFigures, g.Properties);

		//shapes
		if (g.Properties.P || g.Properties.L) {
			numberOfShapes = 1;
		}
		else {
			numberOfShapes = read_uint32();
		}
		g.Shapes = readShapes(numberOfShapes, g.Properties);

		//segments
		if (g.Version == 2) {
			numberOfSegments = read_uint32();
			g.Segments = readSegments(numberOfSegments);
		}
		return g;
	}
}<commit_msg>call reserve() on vector<commit_after>#include "mssqlclrgeo.hpp"
#include <cstring>

namespace mssqlclr
{

	const std::vector<Figure> Geometry::getFiguresFromShape(int shapeIdx) const {
		std::vector<Figure> figures_list;
		if (Figures.empty()) {
			return figures_list;
		}
		int32_t offset = Shapes[shapeIdx].FigureOffset;

		if (Shapes.size() > shapeIdx + 1) {
			int32_t offsetEnd = Shapes[shapeIdx + 1].FigureOffset;

			figures_list = std::vector<Figure>(Figures.cbegin() + offset, Figures.cbegin() + offsetEnd);
		}
		else {
			figures_list = std::vector<Figure>(Figures.cbegin() + offset, Figures.cend());
		}
		return figures_list;
	}

	const std::vector<Point> Geometry::getPointsFromFigure(int figureIdx) const {
		std::vector<Point> points_list;

		if (Points.empty()) {
			return points_list;
		}

		int32_t offset = Figures[figureIdx].Offset;

		if (Figures.size() > figureIdx + 1) {
			int32_t offsetEnd = Figures[figureIdx + 1].Offset;
			points_list = std::vector<Point>(Points.cbegin() + offset, Points.cbegin() + offsetEnd);
		}
		else {
			points_list = std::vector<Point>(Points.cbegin() + offset, Points.cend());
		}
		return points_list;
	}

	sqlgeo_reader::sqlgeo_reader(const char* data, std::size_t size)
		: data_(data),
		size_(size),
		pos_(0)
	{
	}

	inline uint32_t sqlgeo_reader::read_uint32()
	{
		std::uint32_t val;
		if (pos_ + 4 > size_) {
			return 0;
		}
		auto data = data_ + pos_;
		std::memcpy(&val, data, 4);
		pos_ += 4;

		return val;
	}

	inline uint16_t sqlgeo_reader::read_uint16()
	{
		std::uint16_t val;
		if (pos_ + 2 > size_) {
			return 0;
		}
		auto data = data_ + pos_;
		std::memcpy(&val, data, 2);
		pos_ += 2;

		return val;
	}
	inline uint8_t sqlgeo_reader::read_uint8()
	{
		std::uint8_t val;
		if (pos_ + 1 > size_) {
			return 0;
		}
		auto data = data_ + pos_;
		std::memcpy(&val, data, 1);
		pos_ += 1;

		return val;
	}
	inline double sqlgeo_reader::read_double()
	{
		double val;
		if (pos_ + 8 > size_) {
			return 0;
		}
		auto data = data_ + pos_;
		std::memcpy(&val, &data[0], 8);
		pos_ += 8;

		return val;
	}

	void sqlgeo_reader::readPointsZ(std::vector<Point> points) {

		if (points.empty()) {
			return;
		}
		for (auto& point : points)
		{
			point.Z = read_double();
		}

	}

	void sqlgeo_reader::readPointsM(std::vector<Point> points) {

		if (points.empty()) {
			return;
		}
		for (auto& point : points)
		{
			point.M = read_double();
		}

	}
	std::vector<Point> sqlgeo_reader::readPoints(uint32_t count, bool isGeography) {
		std::vector<Point> points;
		points.reserve(count);

		if (count < 1) {
			return std::vector<Point>();
		}

		for (uint32_t i = 0; i < count; i++) {
			if (isGeography) {
				double y = read_double();
				double x = read_double();
				points.emplace_back(x, y);
			}
			else {
				double x = read_double();
				double y = read_double();
				points.emplace_back(x, y);
			}

		}

		return points;
	}

	std::vector<Figure> sqlgeo_reader::readFigures(uint32_t count, SerializationProperties properties) {
		std::vector<Figure> figures;
		figures.reserve(count);

		if (count == 0) {
			return figures;
		}

		if (properties.P || properties.L) {
			figures.emplace_back(FIGURE_STROKE, 0);
		}
		else {
			for (int i = 0; i < count; i++) {
				Figure f;
				f.Attribute = (FIGURE)read_uint8();
				f.Offset = read_uint32();

				figures.push_back(f);
			}
		}
		return figures;
	}
	std::vector<Shape> sqlgeo_reader::readShapes(uint32_t count, SerializationProperties properties) {
		std::vector<Shape> shapes;
		shapes.reserve(count);

		if (count < 1) {
			return shapes;
		}

		if (properties.P) {
			shapes.emplace_back(-1, 0, SHAPE_POINT);
		}
		else if (properties.L) {
			shapes.emplace_back(-1, 0, SHAPE_LINESTRING);
		}
		else {
			for (int i = 0; i < int(count); i++) {
				uint32_t parent_offset = read_uint32();
				uint32_t figure_offset = read_uint32();
				SHAPE type = (SHAPE)read_uint8();
				shapes.emplace_back(parent_offset, figure_offset, type);
			}
		}
		return shapes;
	}
	std::vector<Segment> sqlgeo_reader::readSegments(uint32_t count) {
		std::vector<Segment> segments;
		if (count < 1) {
			return segments;
		}

		for (int i = 0; i < int(count); i++) {
			Segment s;
			s.Type = (SEGMENT)read_uint8();
			segments.push_back(s);
		}
		return segments;
	}

	Geometry sqlgeo_reader::parseGeography() {
		return parseGeometry(true);
	}

	Geometry sqlgeo_reader::parseGeometry(bool isGeography) {

		Geometry g = {};

		uint32_t numberOfPoints;
		uint32_t numberOfFigures;
		uint32_t numberOfShapes;
		uint32_t numberOfSegments;

		g.SRID = read_uint32();

		if (isGeography == true) {
			if (g.SRID == -1) {
				return g;
			}
			else if (g.SRID < 4210 || g.SRID > 4999) {
				g.Properties.V = false;
				return g;
				//("Invalid SRID for geography");
			}
		}

		//version
		g.Version = read_uint8();

		if (g.Version > 2) {
			g.Properties.V = false;
			return g;
			//("Version %d is not supported", g.Version);
		}

		//flags
		uint8_t flags = 0;
		flags = read_uint8();

		g.Properties.Z = (flags & (1 << 0)) != 0;
		g.Properties.M = (flags & (1 << 1)) != 0;
		g.Properties.V = (flags & (1 << 2)) != 0;
		g.Properties.P = (flags & (1 << 3)) != 0;
		g.Properties.L = (flags & (1 << 4)) != 0;

		if (g.Version == 2) {
			g.Properties.H = (flags & (1 << 5)) != 0;
		}
		if (g.Properties.P && g.Properties.L) {
			g.Properties.V = false;
			return g;
			//("geometry data is invalid");
		}

		//points
		if (g.Properties.P) {
			numberOfPoints = 1;
		}
		else if (g.Properties.L) {
			numberOfPoints = 2;
		}
		else {
			numberOfPoints = read_uint32();
		}
		g.Points = readPoints(numberOfPoints, isGeography);

		if (g.Properties.Z) {
			readPointsZ(g.Points);
		}
		if (g.Properties.M) {
			readPointsM(g.Points);
		}

		//figures
		if (g.Properties.P || g.Properties.L) {
			numberOfFigures = 1;
		}
		else {
			numberOfFigures = read_uint32();
		}

		g.Figures = readFigures(numberOfFigures, g.Properties);

		//shapes
		if (g.Properties.P || g.Properties.L) {
			numberOfShapes = 1;
		}
		else {
			numberOfShapes = read_uint32();
		}
		g.Shapes = readShapes(numberOfShapes, g.Properties);

		//segments
		if (g.Version == 2) {
			numberOfSegments = read_uint32();
			g.Segments = readSegments(numberOfSegments);
		}
		return g;
	}
}<|endoftext|>