<commit_before>//   Copyright (c) 2018 PaddlePaddle Authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*Licensed under the Apache License, Version 2.0(the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License. */

#include "mkldnn.hpp"
#include "paddle/fluid/framework/tensor.h"
#include "paddle/fluid/operators/math/selected_rows_functor.h"
#include "paddle/fluid/operators/sum_op.h"
#include "paddle/fluid/platform/device_context.h"
#include "paddle/fluid/platform/mkldnn_helper.h"

namespace paddle {
namespace operators {

using framework::DataLayout;
using mkldnn::memory;
using mkldnn::primitive;
using mkldnn::reorder;
using mkldnn::stream;
using mkldnn::sum;
using paddle::framework::Tensor;
using paddle::platform::CPUDeviceContext;
using paddle::platform::MKLDNNDeviceContext;
using platform::to_void_cast;

template <typename T>
class SumMKLDNNOpKernel : public paddle::framework::OpKernel<T> {
 public:
  void Compute(const paddle::framework::ExecutionContext& ctx) const override {
    PADDLE_ENFORCE(paddle::platform::is_cpu_place(ctx.GetPlace()),
                   "It must use CPUPlace.");
    auto& dev_ctx = ctx.template device_context<MKLDNNDeviceContext>();
    const auto& mkldnn_engine = dev_ctx.GetEngine();
    auto in_vars = ctx.MultiInputVar("X");

    const int N = in_vars.size();
    auto out_var = ctx.OutputVar("Out");
    bool in_place = out_var == in_vars[0];

    if (out_var->IsType<framework::LoDTensor>()) {
      LoDTensor* output = ctx.Output<LoDTensor>("Out");
      T* output_data = output->mutable_data<T>(ctx.GetPlace());

      std::vector<int> dst_tz = framework::vectorize2int(output->dims());
      auto src_tz = dst_tz;
      memory::format output_format{memory::format::format_undef};
      std::vector<float> scales;
      std::vector<memory::primitive_desc> srcs_mpd;
      std::vector<mkldnn::memory> srcs_mem;

      PADDLE_ENFORCE(in_vars[0]->IsType<LoDTensor>(),
                     "Input[0] must be LoDTensors");
      auto& input0 = in_vars[0]->Get<LoDTensor>();
      PADDLE_ENFORCE(input0.layout() == DataLayout::kMKLDNN &&
                         input0.format() != memory::format::format_undef,
                     "Wrong layout/format for inputs[0]");

      memory::format input_format = input0.format();

      if (src_tz.size() == 1 && (input_format == memory::format::nchw ||
                                 input_format == memory::format::nhwc)) {
        input_format = memory::format::x;
      }
      if (src_tz.size() == 2 && (input_format == memory::format::nchw ||
                                 input_format == memory::format::nhwc)) {
        input_format = memory::format::nc;
      }

      for (int i = 0; i < N; i++) {
        PADDLE_ENFORCE(in_vars[i]->IsType<LoDTensor>(),
                       "all inputs must be all LoDTensors");
        auto& input = in_vars[i]->Get<LoDTensor>();
        PADDLE_ENFORCE(input.layout() == DataLayout::kMKLDNN &&
                           input.format() != memory::format::format_undef,
                       "Wrong layout/format for inputs");

        if (input.numel() == 0) {
          continue;
        }

        const T* input_data = input.data<T>();

        auto src_md =
            memory::desc(src_tz, memory::data_type::f32, input_format);
        auto src_mpd = memory::primitive_desc(src_md, mkldnn_engine);
        auto src_mem = memory(src_mpd, to_void_cast(input_data));
        srcs_mpd.push_back(src_mpd);
        srcs_mem.push_back(src_mem);
        scales.push_back(1.0);
      }

      auto dst_md =
          memory::desc(dst_tz, memory::data_type::f32, memory::format::any);

      auto sum_pd = sum::primitive_desc(dst_md, scales, srcs_mpd);

      std::shared_ptr<memory> dst_mem;
      if (in_place) {
        dst_mem.reset(new memory(sum_pd.dst_primitive_desc()));
      } else {
        dst_mem.reset(new memory(sum_pd.dst_primitive_desc(), output_data));
      }
      std::vector<mkldnn::primitive::at> inputs;
      for (size_t i = 0; i < srcs_mem.size(); ++i) {
        inputs.push_back(srcs_mem[i]);
      }

      auto sum_prim = mkldnn::sum(sum_pd, inputs, *dst_mem);
      output_format = (memory::format)platform::GetMKLDNNFormat(sum_pd);

      primitive reorder_prim;
      std::shared_ptr<memory> target_mem;
      if (in_place) {
        output_format = input_format;
        target_mem.reset(new memory(
            {{{src_tz}, memory::data_type::f32, output_format}, mkldnn_engine},
            output_data));
        reorder_prim = reorder(*dst_mem, *target_mem);
      }

      std::vector<primitive> pipeline;
      pipeline.push_back(sum_prim);
      if (in_place) pipeline.push_back(reorder_prim);
      stream(stream::kind::eager).submit(pipeline).wait();

      output->set_layout(DataLayout::kMKLDNN);
      output->set_format(output_format);
    } else if (out_var->IsType<framework::SelectedRows>()) {
      // TODO(@mozga-intel) Add MKLDNN SelectedRows support
      std::unique_ptr<framework::SelectedRows> in0;
      if (in_place) {
        // If is in_place, we store the input[0] to in0
        auto& in_sel0 = in_vars[0]->Get<SelectedRows>();
        auto& rows = in_sel0.rows();
        in0.reset(new framework::SelectedRows(rows, in_sel0.height()));
        in0->mutable_value()->ShareDataWith(in_sel0.value());
      }

      auto get_selected_row = [&](size_t i) -> const SelectedRows& {
        if (i == 0 && in0) {
          return *in0;
        } else {
          return in_vars[i]->Get<SelectedRows>();
        }
      };
      auto* out = ctx.Output<SelectedRows>("Out");
      out->mutable_rows()->clear();
      auto* out_value = out->mutable_value();

      // Runtime InferShape
      size_t first_dim = 0;
      for (int i = 0; i < N; i++) {
        auto& sel_row = get_selected_row(i);
        first_dim += sel_row.rows().size();
      }

      std::vector<int64_t> in_dim;
      for (int i = 0; i < N; i++) {
        auto& sel_row = get_selected_row(i);
        if (sel_row.rows().size() > 0) {
          in_dim = framework::vectorize(sel_row.value().dims());
          break;
        }
      }

      if (in_dim.empty()) {
        VLOG(3) << "WARNING: all the inputs are empty";
        in_dim = framework::vectorize(get_selected_row(N - 1).value().dims());
      } else {
        in_dim[0] = static_cast<int64_t>(first_dim);
      }

      in_dim[0] = static_cast<int64_t>(first_dim);

      out_value->Resize(framework::make_ddim(in_dim));

      out_value->mutable_data<T>(ctx.GetPlace());

      // if all the input sparse vars are empty, no need to
      // merge these vars.
      if (first_dim == 0UL) {
        return;
      }

      math::SelectedRowsAddTo<CPUDeviceContext, T> functor;
      int64_t offset = 0;
      for (int i = 0; i < N; i++) {
        auto& sel_row = get_selected_row(i);
        if (sel_row.rows().size() == 0) {
          continue;
        }
        PADDLE_ENFORCE_EQ(out->height(), sel_row.height());
        functor(ctx.template device_context<CPUDeviceContext>(), sel_row,
                offset, out);
        offset += sel_row.value().numel();
      }
    } else if (out_var->IsType<framework::LoDTensorArray>()) {
      // TODO(@mozga-intel) Add MKLDNN LoDTensorArray support
      auto& out_array = *out_var->GetMutable<framework::LoDTensorArray>();
      for (size_t i = in_place ? 1 : 0; i < in_vars.size(); ++i) {
        PADDLE_ENFORCE(in_vars[i]->IsType<framework::LoDTensorArray>(),
                       "Only support all inputs are TensorArray");
        auto& in_array = in_vars[i]->Get<framework::LoDTensorArray>();

        for (size_t i = 0; i < in_array.size(); ++i) {
          if (in_array[i].numel() != 0) {
            if (i >= out_array.size()) {
              out_array.resize(i + 1);
            }
            if (out_array[i].numel() == 0) {
              framework::TensorCopy(in_array[i], in_array[i].place(),
                                    ctx.device_context(), &out_array[i]);
              out_array[i].set_lod(in_array[i].lod());
            } else {
              PADDLE_ENFORCE(out_array[i].lod() == in_array[i].lod());
              auto in = EigenVector<T>::Flatten(in_array[i]);
              auto result = EigenVector<T>::Flatten(out_array[i]);
              result.device(*ctx.template device_context<MKLDNNDeviceContext>()
                                 .eigen_device()) = result + in;
            }
          }
        }
      }
    } else {
      PADDLE_THROW("Unexpected branch, output variable type is %s",
                   framework::ToTypeName(out_var->Type()));
    }
  }
};

}  // namespace operators
}  // namespace paddle

REGISTER_OP_KERNEL(sum, MKLDNN, ::paddle::platform::CPUPlace,
                   paddle::operators::SumMKLDNNOpKernel<float>);
<commit_msg>Improve code reuse at MKL-DNN sum<commit_after>//   Copyright (c) 2018 PaddlePaddle Authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*Licensed under the Apache License, Version 2.0(the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License. */

#include "mkldnn.hpp"
#include "paddle/fluid/framework/tensor.h"
#include "paddle/fluid/operators/math/selected_rows_functor.h"
#include "paddle/fluid/operators/sum_op.h"
#include "paddle/fluid/platform/device_context.h"
#include "paddle/fluid/platform/mkldnn_helper.h"

namespace paddle {
namespace operators {

using framework::DataLayout;
using mkldnn::memory;
using mkldnn::primitive;
using mkldnn::reorder;
using mkldnn::stream;
using mkldnn::sum;
using paddle::framework::Tensor;
using paddle::platform::CPUDeviceContext;
using paddle::platform::MKLDNNDeviceContext;
using platform::to_void_cast;

template <typename T>
class SumMKLDNNOpKernel : public paddle::framework::OpKernel<T> {
 public:
  void Compute(const paddle::framework::ExecutionContext& ctx) const override {
    PADDLE_ENFORCE(paddle::platform::is_cpu_place(ctx.GetPlace()),
                   "It must use CPUPlace.");
    auto& dev_ctx = ctx.template device_context<MKLDNNDeviceContext>();
    const auto& mkldnn_engine = dev_ctx.GetEngine();
    auto in_vars = ctx.MultiInputVar("X");

    const int N = in_vars.size();
    auto out_var = ctx.OutputVar("Out");
    bool in_place = out_var == in_vars[0];

    if (out_var->IsType<framework::LoDTensor>()) {
      LoDTensor* output = ctx.Output<LoDTensor>("Out");
      T* output_data = output->mutable_data<T>(ctx.GetPlace());

      std::vector<int> dst_tz = framework::vectorize2int(output->dims());
      auto src_tz = dst_tz;
      memory::format output_format{memory::format::format_undef};
      std::vector<float> scales;
      std::vector<memory::primitive_desc> srcs_mpd;
      std::vector<mkldnn::memory> srcs_mem;

      PADDLE_ENFORCE(in_vars[0]->IsType<LoDTensor>(),
                     "Input[0] must be LoDTensors");
      auto& input0 = in_vars[0]->Get<LoDTensor>();
      PADDLE_ENFORCE(input0.layout() == DataLayout::kMKLDNN &&
                         input0.format() != memory::format::format_undef,
                     "Wrong layout/format for inputs[0]");

      memory::format input_format = input0.format();

      for (int i = 0; i < N; i++) {
        PADDLE_ENFORCE(in_vars[i]->IsType<LoDTensor>(),
                       "all inputs must be all LoDTensors");
        auto& input = in_vars[i]->Get<LoDTensor>();
        PADDLE_ENFORCE(input.layout() == DataLayout::kMKLDNN &&
                           input.format() != memory::format::format_undef,
                       "Wrong layout/format for inputs");

        if (input.numel() == 0) {
          continue;
        }

        const T* input_data = input.data<T>();

        auto src_md =
            memory::desc(src_tz, memory::data_type::f32, input_format);
        auto src_mpd = memory::primitive_desc(src_md, mkldnn_engine);
        auto src_mem = memory(src_mpd, to_void_cast(input_data));
        srcs_mpd.push_back(src_mpd);
        srcs_mem.push_back(src_mem);
        scales.push_back(1.0);
      }

      auto dst_md =
          memory::desc(dst_tz, memory::data_type::f32, memory::format::any);

      auto sum_pd = sum::primitive_desc(dst_md, scales, srcs_mpd);

      std::shared_ptr<memory> dst_mem;
      if (in_place) {
        dst_mem.reset(new memory(sum_pd.dst_primitive_desc()));
      } else {
        dst_mem.reset(new memory(sum_pd.dst_primitive_desc(), output_data));
      }
      std::vector<mkldnn::primitive::at> inputs;
      for (size_t i = 0; i < srcs_mem.size(); ++i) {
        inputs.push_back(srcs_mem[i]);
      }

      auto sum_prim = mkldnn::sum(sum_pd, inputs, *dst_mem);
      output_format = (memory::format)platform::GetMKLDNNFormat(sum_pd);

      primitive reorder_prim;
      std::shared_ptr<memory> target_mem;
      if (in_place) {
        output_format = input_format;
        target_mem.reset(new memory(
            {{{src_tz}, memory::data_type::f32, output_format}, mkldnn_engine},
            output_data));
        reorder_prim = reorder(*dst_mem, *target_mem);
      }

      std::vector<primitive> pipeline;
      pipeline.push_back(sum_prim);
      if (in_place) pipeline.push_back(reorder_prim);
      stream(stream::kind::eager).submit(pipeline).wait();

      output->set_layout(DataLayout::kMKLDNN);
      output->set_format(output_format);
    } else {  // Fallback to naive version
      // TODO(@mozga-intel) Add MKLDNN SelectedRows & LoDTensorArray support
      SumKernel<CPUDeviceContext, T> reference_kernel;
      reference_kernel.Compute(ctx);
    }
  }
};

}  // namespace operators
}  // namespace paddle

REGISTER_OP_KERNEL(sum, MKLDNN, ::paddle::platform::CPUPlace,
                   paddle::operators::SumMKLDNNOpKernel<float>);
<|endoftext|>


<commit_before>#include "EntityManager.hpp"

#ifndef KENGINE_MAX_SAVE_PATH_LENGTH
# define KENGINE_MAX_SAVE_PATH_LENGTH 64
#endif

#ifndef KENGINE_MAX_COMPONENT_NAME_LENGTH
# define KENGINE_MAX_COMPONENT_NAME_LENGTH 64
#endif

namespace kengine {
	Entity EntityManager::getEntity(Entity::ID id) {
		detail::ReadLock l(_entitiesMutex);
		return Entity(id, _entities[id].mask, this);
	}

	EntityView EntityManager::getEntity(Entity::ID id) const {
		detail::ReadLock l(_entitiesMutex);
		return EntityView(id, _entities[id].mask);
	}

	void EntityManager::removeEntity(EntityView e) {
		removeEntity(e.id);
	}

	void EntityManager::removeEntity(Entity::ID id) {
		detail::WriteLock l(_updatesMutex);
		if (_updatesLocked) {
			_removals.push_back({ id });
		}
		else
			doRemove(id);
	}

	void EntityManager::setEntityActive(EntityView e, bool active) {
		setEntityActive(e.id, active);
	}

	void EntityManager::setEntityActive(Entity::ID id, bool active) {
		detail::WriteLock l(_entitiesMutex);
		_entities[id].active = active;
		_entities[id].shouldActivateAfterInit = active;
	}

	struct ComponentIDSave {
		size_t id;
		putils::string<KENGINE_MAX_COMPONENT_NAME_LENGTH> name;
	};

	void EntityManager::load(const char * directory) {
		{
			detail::WriteLock l(_archetypesMutex);
			_archetypes.clear();
		}

		{
			detail::WriteLock l(_toReuseMutex);
			_toReuse.clear();
		}

		for (auto & e : getEntities())
			SystemManager::removeEntity(e);

		{
			detail::ReadLock l(_components.mutex);
			for (const auto &[_, meta] : _components.map)
				meta->load(directory);
		}

		size_t idMap[KENGINE_COMPONENT_COUNT]; // index = new, value = old
		for (auto & i : idMap)
			i = (size_t)-1;
		{
			detail::ReadLock l(_components.mutex);

			std::ifstream f(putils::string<KENGINE_MAX_SAVE_PATH_LENGTH>("%s/components.bin", directory), std::ifstream::binary);
			assert(f);
			size_t size;
			f.read((char *)&size, sizeof(size));
			for (size_t i = 0; i < size; ++i) {
				ComponentIDSave save;
				f.read((char *)&save, sizeof(save));
				assert(f.gcount() == sizeof(save));

				for (const auto & [_, meta] : _components.map)
					if (meta->funcs.name == save.name) {
						idMap[meta->id] = save.id;
						break;
					}
			}
		}

		std::ifstream f(putils::string<KENGINE_MAX_SAVE_PATH_LENGTH>("%s/entities.bin", directory), std::ifstream::binary);
		assert(f);
		size_t size;
		f.read((char *)&size, sizeof(size));

		{
			detail::WriteLock l(_entitiesMutex);
			_entities.resize(size);
			f.read((char *)_entities.data(), size * sizeof(_entities[0]));
		}

		for (auto & e : getEntities()) {
			if (e.componentMask != 0) { // Adjust for Components with new IDs since save
				Entity::Mask tmp = 0;
				for (size_t i = 0; i < KENGINE_COMPONENT_COUNT; ++i) {
					const auto oldId = idMap[i];
					tmp[i] = oldId == (size_t)-1 ? false : e.componentMask[oldId];
				}
				e.componentMask = tmp;

				if (e.componentMask != 0)
					updateMask(e.id, e.componentMask, true);
			}
			else {
				detail::WriteLock l(_toReuseMutex);
				_toReuse.emplace_back(e.id);
			}
		}

		for (auto & e : getEntities())
			SystemManager::registerEntity(e);

		SystemManager::load(directory);
	}

	void EntityManager::save(const char * directory) const {
		SystemManager::save(directory);

		putils::vector<bool, KENGINE_COMPONENT_COUNT> serializable;

		{
			detail::ReadLock l(_components.mutex);
			serializable.resize(_components.map.size());
			for (const auto &[_, meta] : _components.map)
				serializable[meta->id] = meta->save(directory);

			// Save Component IDs
			std::ofstream f(putils::string<KENGINE_MAX_SAVE_PATH_LENGTH>("%s/components.bin", directory), std::ofstream::binary);
			const size_t size = _components.map.size();
			f.write((const char *)&size, sizeof(size));
			for (const auto &[_, meta] : _components.map) {
				ComponentIDSave save;
				save.id = meta->id;
				save.name = meta->funcs.name;
				f.write((const char *)&save, sizeof(save));
			}
		}

		std::ofstream f(putils::string<KENGINE_MAX_SAVE_PATH_LENGTH>("%s/entities.bin", directory), std::ofstream::binary);
		assert(f);

		{
			detail::ReadLock l(_entitiesMutex);
			const auto size = _entities.size();
			f.write((const char *)&size, sizeof(size));
			for (EntityMetadata e : _entities) {
				for (size_t i = 0; i < serializable.size(); ++i)
					e.mask[i] = e.mask[i] && serializable[i];
				f.write((const char *)&e, sizeof(e));
			}
		}
	}

	Entity EntityManager::alloc() {
		{
			detail::ReadLock l(_toReuseMutex);
			if (_toReuse.empty()) {
				detail::WriteLock l(_entitiesMutex);
				const auto id = _entities.size();
				_entities.push_back({ false, 0 });
				return Entity(id, 0, this);
			}
		}

		Entity::ID id;
		{
			detail::WriteLock l(_toReuseMutex);
			if (!_toReuseSorted) {
				std::sort(_toReuse.begin(), _toReuse.end(), std::greater<Entity::ID>());
				_toReuseSorted = true;
			}
			id = _toReuse.back();
			_toReuse.pop_back();
		}

#ifndef NDEBUG
		{
			detail::ReadLock l(_archetypesMutex);
			for (const auto & archetype : _archetypes) {
				detail::ReadLock l(archetype.mutex);
				assert(std::find(archetype.entities.begin(), archetype.entities.end(), id) == archetype.entities.end());
			}
		}

		{
			detail::ReadLock l(_entitiesMutex);
			assert(id < _entities.size());
		}
#endif

		return Entity(id, 0, this);
	}

	void EntityManager::addComponent(Entity::ID id, size_t component) {
		updateHasComponent(id, component, true);
	}

	void EntityManager::removeComponent(Entity::ID id, size_t component) {
		updateHasComponent(id, component, false);
	}

	void EntityManager::updateHasComponent(Entity::ID id, size_t component, bool newHasComponent) {
		{
			detail::WriteLock l(_updatesMutex);
			for (auto & update : _updates)
				if (update.id == id) {
					std::unique_lock<std::mutex> l(update.mutex);
					update.newMask[component] = newHasComponent;
					return;
				}
		}

		Entity::Mask currentMask;
		{
			detail::ReadLock l(_entitiesMutex);
			currentMask = _entities[id].mask;
		}

		auto updatedMask = currentMask;
		updatedMask[component] = newHasComponent;
		updateMask(id, updatedMask);
	}

	void EntityManager::updateMask(Entity::ID id, Entity::Mask newMask, bool ignoreOldMask) {
		detail::WriteLock l(_updatesMutex);
		if (_updatesLocked == 0)
			doUpdateMask(id, newMask, ignoreOldMask);
		else {
			Update update;
			update.id = id;
			update.newMask = newMask;
			update.ignoreOldMask = ignoreOldMask;
			_updates.push_back(std::move(update));
		}
	}

	void EntityManager::doAllUpdates() {
		// _updatesMutex already locked
		for (const auto & update : _updates)
			doUpdateMask(update.id, update.newMask, update.ignoreOldMask);
		_updates.clear();

		for (const auto removal : _removals)
			doRemove(removal.id);
		_removals.clear();
	}

	void EntityManager::doUpdateMask(Entity::ID id, Entity::Mask newMask, bool ignoreOldMask) {
		// _updatesMutex already locked
		const auto oldMask = ignoreOldMask ? 0 : _entities[id].mask;
		assert(newMask != oldMask);

		bool removeDone = oldMask == 0;
		bool addDone = newMask == 0;

		{
			detail::WriteLock l(_archetypesMutex);
			for (auto & collection : _archetypes) {
				if (removeDone && addDone)
					break;

				if (collection.mask == oldMask) {
					const auto size = collection.entities.size();
					if (size > 1) {
						const auto it = std::find(collection.entities.begin(), collection.entities.end(), id);
						std::iter_swap(it, collection.entities.begin() + size - 1);
					}
					collection.entities.pop_back();
					collection.sorted = false;
					removeDone = true;
				}

				if (collection.mask == newMask) {
					collection.entities.emplace_back(id);
					collection.sorted = false;
					addDone = true;
				}
			}

			if (!addDone)
				_archetypes.emplace_back(newMask, id);
		}

		detail::WriteLock l(_entitiesMutex);
		_entities[id].mask = newMask;
	}

	void EntityManager::doRemove(Entity::ID id) {
		Entity::Mask mask;
		{
			detail::ReadLock entities(_entitiesMutex);
			mask = _entities[id].mask;
		}

		SystemManager::removeEntity(EntityView(id, mask));

		{
			detail::ReadLock entities(_entitiesMutex);
			mask = _entities[id].mask;
		}

		{
			detail::ReadLock archetypes(_archetypesMutex);
			for (auto & collection : _archetypes) {
				if (collection.mask == mask) {
					detail::WriteLock archetype(collection.mutex);
					const auto tmp = std::find(collection.entities.begin(), collection.entities.end(), id);
					if (collection.entities.size() > 1) {
						std::swap(*tmp, collection.entities.back());
						collection.sorted = false;
					}
					collection.entities.pop_back();
					break;
				}
			}
		}

		{
			detail::WriteLock entities(_entitiesMutex);
			_entities[id].mask = 0;
			_entities[id].active = false;
			_entities[id].shouldActivateAfterInit = true;
		}

		detail::WriteLock l(_toReuseMutex);
		_toReuse.emplace_back(id);
		_toReuseSorted = false;
	}

	/*
	** Collection
	*/

	EntityManager::EntityCollection EntityManager::getEntities() {
		return EntityCollection{ *this };
	}

	EntityManager::EntityCollection::EntityIterator & EntityManager::EntityCollection::EntityIterator::operator++() {
		++index;
		detail::ReadLock l(em._entitiesMutex);
		while (index < em._entities.size() && (em._entities[index].mask == 0 || !em._entities[index].active))
			++index;
		return *this;
	}

	bool EntityManager::EntityCollection::EntityIterator::operator!=(const EntityIterator & rhs) const {
		// Use `<` as it will only be compared with `end()`, and there is a risk that new entities have been added since `end()` was called
		return index < rhs.index;
	}

	Entity EntityManager::EntityCollection::EntityIterator::operator*() const {
		detail::ReadLock l(em._entitiesMutex);
		return Entity(index, em._entities[index].mask, &em);
	}

	EntityManager::EntityCollection::EntityIterator EntityManager::EntityCollection::begin() const {
		size_t i = 0;
		detail::ReadLock l(em._entitiesMutex);
		while (i < em._entities.size() && (em._entities[i].mask == 0 || !em._entities[i].active))
			++i;
		return EntityIterator{ i, em };
	}

	EntityManager::EntityCollection::EntityIterator EntityManager::EntityCollection::end() const {
		detail::ReadLock l(em._entitiesMutex);
		return EntityIterator{ em._entities.size(), em };
	}

	EntityManager::EntityCollection::EntityCollection(EntityManager & em) : em(em) {
		detail::WriteLock l(em._updatesMutex);
		++em._updatesLocked;
	}

	EntityManager::EntityCollection::~EntityCollection() {
		detail::WriteLock l(em._updatesMutex);
		assert(em._updatesLocked != 0);
		--em._updatesLocked;
		if (em._updatesLocked == 0)
			em.doAllUpdates();
	}
}<commit_msg>fix toReuse not being populated on load<commit_after>#include "EntityManager.hpp"

#ifndef KENGINE_MAX_SAVE_PATH_LENGTH
# define KENGINE_MAX_SAVE_PATH_LENGTH 64
#endif

#ifndef KENGINE_MAX_COMPONENT_NAME_LENGTH
# define KENGINE_MAX_COMPONENT_NAME_LENGTH 64
#endif

namespace kengine {
	Entity EntityManager::getEntity(Entity::ID id) {
		detail::ReadLock l(_entitiesMutex);
		return Entity(id, _entities[id].mask, this);
	}

	EntityView EntityManager::getEntity(Entity::ID id) const {
		detail::ReadLock l(_entitiesMutex);
		return EntityView(id, _entities[id].mask);
	}

	void EntityManager::removeEntity(EntityView e) {
		removeEntity(e.id);
	}

	void EntityManager::removeEntity(Entity::ID id) {
		detail::WriteLock l(_updatesMutex);
		if (_updatesLocked) {
			_removals.push_back({ id });
		}
		else
			doRemove(id);
	}

	void EntityManager::setEntityActive(EntityView e, bool active) {
		setEntityActive(e.id, active);
	}

	void EntityManager::setEntityActive(Entity::ID id, bool active) {
		detail::WriteLock l(_entitiesMutex);
		_entities[id].active = active;
		_entities[id].shouldActivateAfterInit = active;
	}

	struct ComponentIDSave {
		size_t id;
		putils::string<KENGINE_MAX_COMPONENT_NAME_LENGTH> name;
	};

	void EntityManager::load(const char * directory) {
		{
			detail::WriteLock l(_archetypesMutex);
			_archetypes.clear();
		}

		{
			detail::WriteLock l(_toReuseMutex);
			_toReuse.clear();
		}

		for (auto & e : getEntities())
			SystemManager::removeEntity(e);

		{
			detail::ReadLock l(_components.mutex);
			for (const auto &[_, meta] : _components.map)
				meta->load(directory);
		}

		size_t idMap[KENGINE_COMPONENT_COUNT]; // index = new, value = old
		for (auto & i : idMap)
			i = (size_t)-1;
		{
			detail::ReadLock l(_components.mutex);

			std::ifstream f(putils::string<KENGINE_MAX_SAVE_PATH_LENGTH>("%s/components.bin", directory), std::ifstream::binary);
			assert(f);
			size_t size;
			f.read((char *)&size, sizeof(size));
			for (size_t i = 0; i < size; ++i) {
				ComponentIDSave save;
				f.read((char *)&save, sizeof(save));
				assert(f.gcount() == sizeof(save));

				for (const auto & [_, meta] : _components.map)
					if (meta->funcs.name == save.name) {
						idMap[meta->id] = save.id;
						break;
					}
			}
		}

		std::ifstream f(putils::string<KENGINE_MAX_SAVE_PATH_LENGTH>("%s/entities.bin", directory), std::ifstream::binary);
		assert(f);
		size_t size;
		f.read((char *)&size, sizeof(size));

		{
			detail::WriteLock l(_entitiesMutex);
			_entities.resize(size);
			f.read((char *)_entities.data(), size * sizeof(_entities[0]));
		}

		for (auto & e : getEntities()) {
				Entity::Mask tmp = 0;
				for (size_t i = 0; i < KENGINE_COMPONENT_COUNT; ++i) {
					const auto oldId = idMap[i];
					tmp[i] = oldId == (size_t)-1 ? false : e.componentMask[oldId];
				}
			updateMask(e.id, tmp, true);
		}

		{
			detail::ReadLock l(_entitiesMutex);
			size_t id = 0;
			for (const auto & e : _entities) {
				if (e.mask == 0) {
					detail::WriteLock l(_toReuseMutex);
					_toReuse.emplace_back(id);
			}
				++id;
			}
		}

		for (auto & e : getEntities())
			SystemManager::registerEntity(e);

		SystemManager::load(directory);
	}

	void EntityManager::save(const char * directory) const {
		SystemManager::save(directory);

		putils::vector<bool, KENGINE_COMPONENT_COUNT> serializable;

		{
			detail::ReadLock l(_components.mutex);
			serializable.resize(_components.map.size());
			for (const auto &[_, meta] : _components.map)
				serializable[meta->id] = meta->save(directory);

			// Save Component IDs
			std::ofstream f(putils::string<KENGINE_MAX_SAVE_PATH_LENGTH>("%s/components.bin", directory), std::ofstream::binary);
			const size_t size = _components.map.size();
			f.write((const char *)&size, sizeof(size));
			for (const auto &[_, meta] : _components.map) {
				ComponentIDSave save;
				save.id = meta->id;
				save.name = meta->funcs.name;
				f.write((const char *)&save, sizeof(save));
			}
		}

		std::ofstream f(putils::string<KENGINE_MAX_SAVE_PATH_LENGTH>("%s/entities.bin", directory), std::ofstream::binary);
		assert(f);

		{
			detail::ReadLock l(_entitiesMutex);
			const auto size = _entities.size();
			f.write((const char *)&size, sizeof(size));
			for (EntityMetadata e : _entities) {
				for (size_t i = 0; i < serializable.size(); ++i)
					e.mask[i] = e.mask[i] && serializable[i];
				f.write((const char *)&e, sizeof(e));
			}
		}
	}

	Entity EntityManager::alloc() {
		{
			detail::ReadLock l(_toReuseMutex);
			if (_toReuse.empty()) {
				detail::WriteLock l(_entitiesMutex);
				const auto id = _entities.size();
				_entities.push_back({ false, 0 });
				return Entity(id, 0, this);
			}
		}

		Entity::ID id;
		{
			detail::WriteLock l(_toReuseMutex);
			if (!_toReuseSorted) {
				std::sort(_toReuse.begin(), _toReuse.end(), std::greater<Entity::ID>());
				_toReuseSorted = true;
			}
			id = _toReuse.back();
			_toReuse.pop_back();
		}

#ifndef NDEBUG
		{
			detail::ReadLock l(_archetypesMutex);
			for (const auto & archetype : _archetypes) {
				detail::ReadLock l(archetype.mutex);
				assert(std::find(archetype.entities.begin(), archetype.entities.end(), id) == archetype.entities.end());
			}
		}

		{
			detail::ReadLock l(_entitiesMutex);
			assert(id < _entities.size());
		}
#endif

		return Entity(id, 0, this);
	}

	void EntityManager::addComponent(Entity::ID id, size_t component) {
		updateHasComponent(id, component, true);
	}

	void EntityManager::removeComponent(Entity::ID id, size_t component) {
		updateHasComponent(id, component, false);
	}

	void EntityManager::updateHasComponent(Entity::ID id, size_t component, bool newHasComponent) {
		{
			detail::WriteLock l(_updatesMutex);
			for (auto & update : _updates)
				if (update.id == id) {
					std::unique_lock<std::mutex> l(update.mutex);
					update.newMask[component] = newHasComponent;
					return;
				}
		}

		Entity::Mask currentMask;
		{
			detail::ReadLock l(_entitiesMutex);
			currentMask = _entities[id].mask;
		}

		auto updatedMask = currentMask;
		updatedMask[component] = newHasComponent;
		updateMask(id, updatedMask);
	}

	void EntityManager::updateMask(Entity::ID id, Entity::Mask newMask, bool ignoreOldMask) {
		detail::WriteLock l(_updatesMutex);
		if (_updatesLocked == 0)
			doUpdateMask(id, newMask, ignoreOldMask);
		else {
			Update update;
			update.id = id;
			update.newMask = newMask;
			update.ignoreOldMask = ignoreOldMask;
			_updates.push_back(std::move(update));
		}
	}

	void EntityManager::doAllUpdates() {
		// _updatesMutex already locked
		for (const auto & update : _updates)
			doUpdateMask(update.id, update.newMask, update.ignoreOldMask);
		_updates.clear();

		for (const auto removal : _removals)
			doRemove(removal.id);
		_removals.clear();
	}

	void EntityManager::doUpdateMask(Entity::ID id, Entity::Mask newMask, bool ignoreOldMask) {
		// _updatesMutex already locked
		const auto oldMask = ignoreOldMask ? 0 : _entities[id].mask;
		assert(newMask != oldMask);

		bool removeDone = oldMask == 0;
		bool addDone = newMask == 0;

		{
			detail::WriteLock l(_archetypesMutex);
			for (auto & collection : _archetypes) {
				if (removeDone && addDone)
					break;

				if (collection.mask == oldMask) {
					const auto size = collection.entities.size();
					if (size > 1) {
						const auto it = std::find(collection.entities.begin(), collection.entities.end(), id);
						std::iter_swap(it, collection.entities.begin() + size - 1);
					}
					collection.entities.pop_back();
					collection.sorted = false;
					removeDone = true;
				}

				if (collection.mask == newMask) {
					collection.entities.emplace_back(id);
					collection.sorted = false;
					addDone = true;
				}
			}

			if (!addDone)
				_archetypes.emplace_back(newMask, id);
		}

		detail::WriteLock l(_entitiesMutex);
		_entities[id].mask = newMask;
	}

	void EntityManager::doRemove(Entity::ID id) {
		Entity::Mask mask;
		{
			detail::ReadLock entities(_entitiesMutex);
			mask = _entities[id].mask;
		}

		SystemManager::removeEntity(EntityView(id, mask));

		{
			detail::ReadLock entities(_entitiesMutex);
			mask = _entities[id].mask;
		}

		{
			detail::ReadLock archetypes(_archetypesMutex);
			for (auto & collection : _archetypes) {
				if (collection.mask == mask) {
					detail::WriteLock archetype(collection.mutex);
					const auto tmp = std::find(collection.entities.begin(), collection.entities.end(), id);
					if (collection.entities.size() > 1) {
						std::swap(*tmp, collection.entities.back());
						collection.sorted = false;
					}
					collection.entities.pop_back();
					break;
				}
			}
		}

		{
			detail::WriteLock entities(_entitiesMutex);
			_entities[id].mask = 0;
			_entities[id].active = false;
			_entities[id].shouldActivateAfterInit = true;
		}

		detail::WriteLock l(_toReuseMutex);
		_toReuse.emplace_back(id);
		_toReuseSorted = false;
	}

	/*
	** Collection
	*/

	EntityManager::EntityCollection EntityManager::getEntities() {
		return EntityCollection{ *this };
	}

	EntityManager::EntityCollection::EntityIterator & EntityManager::EntityCollection::EntityIterator::operator++() {
		++index;
		detail::ReadLock l(em._entitiesMutex);
		while (index < em._entities.size() && (em._entities[index].mask == 0 || !em._entities[index].active))
			++index;
		return *this;
	}

	bool EntityManager::EntityCollection::EntityIterator::operator!=(const EntityIterator & rhs) const {
		// Use `<` as it will only be compared with `end()`, and there is a risk that new entities have been added since `end()` was called
		return index < rhs.index;
	}

	Entity EntityManager::EntityCollection::EntityIterator::operator*() const {
		detail::ReadLock l(em._entitiesMutex);
		return Entity(index, em._entities[index].mask, &em);
	}

	EntityManager::EntityCollection::EntityIterator EntityManager::EntityCollection::begin() const {
		size_t i = 0;
		detail::ReadLock l(em._entitiesMutex);
		while (i < em._entities.size() && (em._entities[i].mask == 0 || !em._entities[i].active))
			++i;
		return EntityIterator{ i, em };
	}

	EntityManager::EntityCollection::EntityIterator EntityManager::EntityCollection::end() const {
		detail::ReadLock l(em._entitiesMutex);
		return EntityIterator{ em._entities.size(), em };
	}

	EntityManager::EntityCollection::EntityCollection(EntityManager & em) : em(em) {
		detail::WriteLock l(em._updatesMutex);
		++em._updatesLocked;
	}

	EntityManager::EntityCollection::~EntityCollection() {
		detail::WriteLock l(em._updatesMutex);
		assert(em._updatesLocked != 0);
		--em._updatesLocked;
		if (em._updatesLocked == 0)
			em.doAllUpdates();
	}
}<|endoftext|>


<commit_before>#include "common.h"
#include "watcher.h"
#include <stdlib.h>
#include <stdio.h>
#include <Psapi.h>
#pragma comment (lib, "psapi.lib")

namespace NodeJudger {

#define EXCEPTION_WX86_BREAKPOINT 0x4000001F
#define MAX_TIME_LIMIT_DELAY 200
const bool __WATCHER_PRINT_DEBUG = (GetEnvironmentVar("JUDGE_DEBUG") == "true");
#define MAX(a, b) ((a) > (b) ? (a) : (b)

inline __int64 GetRunTime_(HANDLE process)
{
    _FILETIME create_time, exit_time, kernel_time, user_time;
    __int64* ut;

    if(GetProcessTimes(process, &create_time, &exit_time, &kernel_time, &user_time))
    {
        ut = reinterpret_cast<__int64*>(&user_time);
        return (__int64)((*ut) / 10000);
    }

    return -1;
}

__int64 GetRunMemo_(HANDLE process)
{
    PROCESS_MEMORY_COUNTERS memo_counter;
    memo_counter.cb = sizeof(PROCESS_MEMORY_COUNTERS);

    if(GetProcessMemoryInfo(process, &memo_counter, sizeof(PROCESS_MEMORY_COUNTERS)))
    {
        return memo_counter.PagefileUsage / 1024;
    }

    return -1;
}

void ExitAndSetError(HANDLE process, CodeState& code_state, StateEnum state_enum, const char* code = NULL)
{
    DWORD process_id = GetProcessId(process);
    if(process_id) DebugActiveProcessStop(process_id);

    // refer to https://msdn.microsoft.com/en-us/library/windows/desktop/ms686714(v=vs.85).aspx
    //
    // TerminateProcess is asynchronous; it initiates termination and returns immediately. If you
    // need to be sure the process has terminated, call the WaitForSingleObject function with a
    // handle to the process.
    TerminateProcess(process, 4);
    WaitForSingleObject(process, 2000);

    code_state.state = state_enum;
    if(code && strlen(code) != 0)
    {
        strcpy(code_state.error_code, code);
    }
}

void SetRuntimeErrorCode_(CodeState& code_state, DWORD code)
{
    switch(code)
    {
    case EXCEPTION_ACCESS_VIOLATION:
    {
        strcpy(code_state.error_code, "ACCESS_VIOLATION");
        break;
    }

    case EXCEPTION_ARRAY_BOUNDS_EXCEEDED:
    {
        strcpy(code_state.error_code, "ARRAY_BOUNDS_EXCEEDED");
        break;
    }

    case EXCEPTION_FLT_DENORMAL_OPERAND:
    {
        strcpy(code_state.error_code, "FLOAT_DENORMAL_OPERAND");
        break;
    }

    case EXCEPTION_FLT_DIVIDE_BY_ZERO:
    {
        strcpy(code_state.error_code, "FLOAT_DIVIDE_BY_ZERO");
        break;
    }

    case EXCEPTION_FLT_OVERFLOW:
    {
        strcpy(code_state.error_code, "FLOAT_OVERFLOW");
        break;
    }

    case EXCEPTION_FLT_UNDERFLOW:
    {
        strcpy(code_state.error_code, "FLOAT_UNDERFLOW");
        break;
    }

    case EXCEPTION_INT_DIVIDE_BY_ZERO:
    {
        strcpy(code_state.error_code, "INTEGER_DIVIDE_BY_ZERO");
        break;
    }

    case EXCEPTION_INT_OVERFLOW:
    {
        strcpy(code_state.error_code, "INTEGER_OVERFLOW");
        break;
    }

    case EXCEPTION_STACK_OVERFLOW:
    {
        strcpy(code_state.error_code, "STACK_OVERFLOW");
        break;
    }

    default:
    {
        char temp[32];
        sprintf(temp, "OTHER_ERRORS_0x%.8X", code);
        strcpy(code_state.error_code, temp);
        break;
    }
    }
}

bool WatchProcess(const HANDLE process,
        const __int64 time_limit,
        const __size memo_limit,
        CodeState& code_state)
{
    __int64 run_time = 0;
    __size run_memo = 0;

    // refer to https://msdn.microsoft.com/en-us/library/windows/desktop/ms679308(v=vs.85).aspx
    //
    // Describes a debugging event.
    //
    // typedef struct _DEBUG_EVENT {
    //     DWORD dwDebugEventCode;
    //     DWORD dwProcessId;
    //     DWORD dwThreadId;
    //     union {
    //         EXCEPTION_DEBUG_INFO      Exception;
    //         CREATE_THREAD_DEBUG_INFO  CreateThread;
    //         CREATE_PROCESS_DEBUG_INFO CreateProcessInfo;
    //         EXIT_THREAD_DEBUG_INFO    ExitThread;
    //         EXIT_PROCESS_DEBUG_INFO   ExitProcess;
    //         LOAD_DLL_DEBUG_INFO       LoadDll;
    //         UNLOAD_DLL_DEBUG_INFO     UnloadDll;
    //         OUTPUT_DEBUG_STRING_INFO  DebugString;
    //         RIP_INFO                  RipInfo;
    //     } u;
    // } DEBUG_EVENT, *LPDEBUG_EVENT;
    DEBUG_EVENT dbe;

    while(true)
    {
        // refer to https://msdn.microsoft.com/en-us/library/windows/desktop/ms681423(v=vs.85).aspx
        //
        // Waits for a debugging event to occur in a process being debugged.
        //
        // If the function succeeds, the return value is nonzero.
        // If the function fails, the return value is zero.To get extended error information, call GetLastError.
        BOOL flag = WaitForDebugEvent(&dbe, (DWORD)time_limit + MAX_TIME_LIMIT_DELAY - run_time);
        if(!flag)
        {
            std::string error = "Cannot wait for debug event: " + GetLastErrorAsString();
            code_state.exe_time = time_limit + MAX_TIME_LIMIT_DELAY;
            code_state.exe_memory = MAX(GetRunMemo_(process), code_state.exe_memory);
            ExitAndSetError(process, code_state, TIME_LIMIT_EXCEEDED_2, error.c_str());
            return false;
        }

        // refer to http://www.debuginfo.com/examples/src/DebugEvents.cpp
        //
        // close some handles inside dbe
        switch(dbe.dwDebugEventCode)
        {
        case CREATE_PROCESS_DEBUG_EVENT:
            if(__WATCHER_PRINT_DEBUG)
            {
                printf("Event: Process creation\n");
                printf("  CREATE_PROCESS_DEBUG_INFO members:\n");
                printf("    hFile:                  %08p\n", dbe.u.CreateProcessInfo.hFile);
                printf("    hProcess:               %08p\n", dbe.u.CreateProcessInfo.hProcess);
                printf("    hThread                 %08p\n", dbe.u.CreateProcessInfo.hThread);
            }

            // With this event, the debugger receives the following handles:
            //   CREATE_PROCESS_DEBUG_INFO.hProcess - debuggee process handle
            //   CREATE_PROCESS_DEBUG_INFO.hThread  - handle to the initial thread of the debuggee process
            //   CREATE_PROCESS_DEBUG_INFO.hFile    - handle to the executable file that was
            //                                        used to create the debuggee process (.EXE file)
            //
            // hProcess and hThread handles will be closed by the operating system
            // when the debugger calls ContinueDebugEvent after receiving
            // EXIT_PROCESS_DEBUG_EVENT for the given process
            //
            // hFile handle should be closed by the debugger, when the handle
            // is no longer needed
            SAFE_CLOSE_HANDLE(dbe.u.CreateProcessInfo.hFile);
            break;

        case CREATE_THREAD_DEBUG_EVENT:
            // With this event, the debugger receives the following handle:
            //   CREATE_THREAD_DEBUG_INFO.hThread  - handle to the thread that has been created
            //
            // This handle will be closed by the operating system
            // when the debugger calls ContinueDebugEvent after receiving
            // EXIT_THREAD_DEBUG_EVENT for the given thread
            break;

        case LOAD_DLL_DEBUG_EVENT:
        {
            std::string dll_name = GetDLLNameFromDebug(dbe.u.LoadDll);
            if(__WATCHER_PRINT_DEBUG)
            {
                printf("Event: DLL loaded\n");
                printf("  LOAD_DLL_DEBUG_INFO members:\n");
                printf("    hFile:                  %08p\n", dbe.u.LoadDll.hFile);
                printf("    lpBaseOfDll:            %08p\n", dbe.u.LoadDll.lpBaseOfDll);
                printf("    DLLName:                %s\n", dll_name.c_str());
            }

            // With this event, the debugger receives the following handle:
            //   LOAD_DLL_DEBUG_INFO.hFile    - handle to the DLL file
            //
            // This handle should be closed by the debugger, when the handle
            // is no longer needed
            SAFE_CLOSE_HANDLE(dbe.u.LoadDll.hFile);

            // And what's more
            // users are not allowed to load DLL
            if(dll_name.find("\\ntdll.dll") == std::string::npos &&
                    dll_name.find("\\kernel32.dll") == std::string::npos &&
                    dll_name.find("\\KernelBase.dll") == std::string::npos &&
                    dll_name.find("\\msvcrt.dll") == std::string::npos &&
                    dll_name.find("\\wow64.dll") == std::string::npos &&
                    dll_name.find("\\wow64win.dll") == std::string::npos &&
                    dll_name.find("\\wow64cpu.dll") == std::string::npos &&
                    dll_name.find("\\user32.dll") == std::string::npos)
            {
                std::string error = "Code is up to load DLL.";
                code_state.exe_time = 0;
                code_state.exe_memory = 0;
                ExitAndSetError(process, code_state, DANGEROUS_CODE, error.c_str());
                return false;
            }
        }

        default: break;
        }

        // get the run time
        run_time = GetRunTime_(process);
        if(-1 == run_time)
        {
            std::string error = "Cannot get running time: " + GetLastErrorAsString();
            code_state.exe_time = 0;
            code_state.exe_memory = MAX(GetRunMemo_(process), code_state.exe_memory);
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            ExitAndSetError(process, code_state, SYSTEM_ERROR, error.c_str());
            return false;
        }

        // get the run memory
        run_memo = GetRunMemo_(process);
        if(-1 == run_memo)
        {
            std::string error = "Cannot get occupied memory: " + GetLastErrorAsString();
            code_state.exe_time = run_time;
            code_state.exe_memory = 0;
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            ExitAndSetError(process, code_state, SYSTEM_ERROR, error.c_str());
            return false;
        }

        code_state.exe_time = run_time;
        code_state.exe_memory = MAX(code_state.exe_memory, run_memo);

        if(run_time > time_limit)
        {
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            ExitAndSetError(process, code_state, TIME_LIMIT_EXCEEDED_1);
            return false;
        }

        if(run_memo > memo_limit)
        {
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            ExitAndSetError(process, code_state, MEMORY_LIMIT_EXCEEDED);
            return false;
        }

        // do some reaction via each DEBUG_EVENT
        switch(dbe.dwDebugEventCode)
        {
        case EXIT_PROCESS_DEBUG_EVENT:
            code_state.state = FINISHED;
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            return true;

        case EXCEPTION_DEBUG_EVENT:
            if(dbe.u.Exception.ExceptionRecord.ExceptionCode != EXCEPTION_BREAKPOINT &&
                    dbe.u.Exception.ExceptionRecord.ExceptionCode != EXCEPTION_WX86_BREAKPOINT)
            {
                SetRuntimeErrorCode_(code_state, dbe.u.Exception.ExceptionRecord.ExceptionCode);
                ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_EXCEPTION_NOT_HANDLED);
                ExitAndSetError(process, code_state, RUNTIME_ERROR);
                return false;
            }
            break;

        default: break;
        }

        ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
        code_state.state = CONTINUE;
    }
}

};<commit_msg>fix: a missing bracket<commit_after>#include "common.h"
#include "watcher.h"
#include <stdlib.h>
#include <stdio.h>
#include <Psapi.h>
#pragma comment (lib, "psapi.lib")

namespace NodeJudger {

#define EXCEPTION_WX86_BREAKPOINT 0x4000001F
#define MAX_TIME_LIMIT_DELAY 200
const bool __WATCHER_PRINT_DEBUG = (GetEnvironmentVar("JUDGE_DEBUG") == "true");
#define MAX(a, b) ((a) > (b) ? (a) : (b))

inline __int64 GetRunTime_(HANDLE process)
{
    _FILETIME create_time, exit_time, kernel_time, user_time;
    __int64* ut;

    if(GetProcessTimes(process, &create_time, &exit_time, &kernel_time, &user_time))
    {
        ut = reinterpret_cast<__int64*>(&user_time);
        return (__int64)((*ut) / 10000);
    }

    return -1;
}

__int64 GetRunMemo_(HANDLE process)
{
    PROCESS_MEMORY_COUNTERS memo_counter;
    memo_counter.cb = sizeof(PROCESS_MEMORY_COUNTERS);

    if(GetProcessMemoryInfo(process, &memo_counter, sizeof(PROCESS_MEMORY_COUNTERS)))
    {
        return memo_counter.PagefileUsage / 1024;
    }

    return -1;
}

void ExitAndSetError(HANDLE process, CodeState& code_state, StateEnum state_enum, const char* code = NULL)
{
    DWORD process_id = GetProcessId(process);
    if(process_id) DebugActiveProcessStop(process_id);

    // refer to https://msdn.microsoft.com/en-us/library/windows/desktop/ms686714(v=vs.85).aspx
    //
    // TerminateProcess is asynchronous; it initiates termination and returns immediately. If you
    // need to be sure the process has terminated, call the WaitForSingleObject function with a
    // handle to the process.
    TerminateProcess(process, 4);
    WaitForSingleObject(process, 2000);

    code_state.state = state_enum;
    if(code && strlen(code) != 0)
    {
        strcpy(code_state.error_code, code);
    }
}

void SetRuntimeErrorCode_(CodeState& code_state, DWORD code)
{
    switch(code)
    {
    case EXCEPTION_ACCESS_VIOLATION:
    {
        strcpy(code_state.error_code, "ACCESS_VIOLATION");
        break;
    }

    case EXCEPTION_ARRAY_BOUNDS_EXCEEDED:
    {
        strcpy(code_state.error_code, "ARRAY_BOUNDS_EXCEEDED");
        break;
    }

    case EXCEPTION_FLT_DENORMAL_OPERAND:
    {
        strcpy(code_state.error_code, "FLOAT_DENORMAL_OPERAND");
        break;
    }

    case EXCEPTION_FLT_DIVIDE_BY_ZERO:
    {
        strcpy(code_state.error_code, "FLOAT_DIVIDE_BY_ZERO");
        break;
    }

    case EXCEPTION_FLT_OVERFLOW:
    {
        strcpy(code_state.error_code, "FLOAT_OVERFLOW");
        break;
    }

    case EXCEPTION_FLT_UNDERFLOW:
    {
        strcpy(code_state.error_code, "FLOAT_UNDERFLOW");
        break;
    }

    case EXCEPTION_INT_DIVIDE_BY_ZERO:
    {
        strcpy(code_state.error_code, "INTEGER_DIVIDE_BY_ZERO");
        break;
    }

    case EXCEPTION_INT_OVERFLOW:
    {
        strcpy(code_state.error_code, "INTEGER_OVERFLOW");
        break;
    }

    case EXCEPTION_STACK_OVERFLOW:
    {
        strcpy(code_state.error_code, "STACK_OVERFLOW");
        break;
    }

    default:
    {
        char temp[32];
        sprintf(temp, "OTHER_ERRORS_0x%.8X", code);
        strcpy(code_state.error_code, temp);
        break;
    }
    }
}

bool WatchProcess(const HANDLE process,
        const __int64 time_limit,
        const __size memo_limit,
        CodeState& code_state)
{
    __int64 run_time = 0;
    __size run_memo = 0;

    // refer to https://msdn.microsoft.com/en-us/library/windows/desktop/ms679308(v=vs.85).aspx
    //
    // Describes a debugging event.
    //
    // typedef struct _DEBUG_EVENT {
    //     DWORD dwDebugEventCode;
    //     DWORD dwProcessId;
    //     DWORD dwThreadId;
    //     union {
    //         EXCEPTION_DEBUG_INFO      Exception;
    //         CREATE_THREAD_DEBUG_INFO  CreateThread;
    //         CREATE_PROCESS_DEBUG_INFO CreateProcessInfo;
    //         EXIT_THREAD_DEBUG_INFO    ExitThread;
    //         EXIT_PROCESS_DEBUG_INFO   ExitProcess;
    //         LOAD_DLL_DEBUG_INFO       LoadDll;
    //         UNLOAD_DLL_DEBUG_INFO     UnloadDll;
    //         OUTPUT_DEBUG_STRING_INFO  DebugString;
    //         RIP_INFO                  RipInfo;
    //     } u;
    // } DEBUG_EVENT, *LPDEBUG_EVENT;
    DEBUG_EVENT dbe;

    while(true)
    {
        // refer to https://msdn.microsoft.com/en-us/library/windows/desktop/ms681423(v=vs.85).aspx
        //
        // Waits for a debugging event to occur in a process being debugged.
        //
        // If the function succeeds, the return value is nonzero.
        // If the function fails, the return value is zero.To get extended error information, call GetLastError.
        BOOL flag = WaitForDebugEvent(&dbe, (DWORD)time_limit + MAX_TIME_LIMIT_DELAY - run_time);
        if(!flag)
        {
            std::string error = "Cannot wait for debug event: " + GetLastErrorAsString();
            code_state.exe_time = time_limit + MAX_TIME_LIMIT_DELAY;
            code_state.exe_memory = MAX(GetRunMemo_(process), code_state.exe_memory);
            ExitAndSetError(process, code_state, TIME_LIMIT_EXCEEDED_2, error.c_str());
            return false;
        }

        // refer to http://www.debuginfo.com/examples/src/DebugEvents.cpp
        //
        // close some handles inside dbe
        switch(dbe.dwDebugEventCode)
        {
        case CREATE_PROCESS_DEBUG_EVENT:
            if(__WATCHER_PRINT_DEBUG)
            {
                printf("Event: Process creation\n");
                printf("  CREATE_PROCESS_DEBUG_INFO members:\n");
                printf("    hFile:                  %08p\n", dbe.u.CreateProcessInfo.hFile);
                printf("    hProcess:               %08p\n", dbe.u.CreateProcessInfo.hProcess);
                printf("    hThread                 %08p\n", dbe.u.CreateProcessInfo.hThread);
            }

            // With this event, the debugger receives the following handles:
            //   CREATE_PROCESS_DEBUG_INFO.hProcess - debuggee process handle
            //   CREATE_PROCESS_DEBUG_INFO.hThread  - handle to the initial thread of the debuggee process
            //   CREATE_PROCESS_DEBUG_INFO.hFile    - handle to the executable file that was
            //                                        used to create the debuggee process (.EXE file)
            //
            // hProcess and hThread handles will be closed by the operating system
            // when the debugger calls ContinueDebugEvent after receiving
            // EXIT_PROCESS_DEBUG_EVENT for the given process
            //
            // hFile handle should be closed by the debugger, when the handle
            // is no longer needed
            SAFE_CLOSE_HANDLE(dbe.u.CreateProcessInfo.hFile);
            break;

        case CREATE_THREAD_DEBUG_EVENT:
            // With this event, the debugger receives the following handle:
            //   CREATE_THREAD_DEBUG_INFO.hThread  - handle to the thread that has been created
            //
            // This handle will be closed by the operating system
            // when the debugger calls ContinueDebugEvent after receiving
            // EXIT_THREAD_DEBUG_EVENT for the given thread
            break;

        case LOAD_DLL_DEBUG_EVENT:
        {
            std::string dll_name = GetDLLNameFromDebug(dbe.u.LoadDll);
            if(__WATCHER_PRINT_DEBUG)
            {
                printf("Event: DLL loaded\n");
                printf("  LOAD_DLL_DEBUG_INFO members:\n");
                printf("    hFile:                  %08p\n", dbe.u.LoadDll.hFile);
                printf("    lpBaseOfDll:            %08p\n", dbe.u.LoadDll.lpBaseOfDll);
                printf("    DLLName:                %s\n", dll_name.c_str());
            }

            // With this event, the debugger receives the following handle:
            //   LOAD_DLL_DEBUG_INFO.hFile    - handle to the DLL file
            //
            // This handle should be closed by the debugger, when the handle
            // is no longer needed
            SAFE_CLOSE_HANDLE(dbe.u.LoadDll.hFile);

            // And what's more
            // users are not allowed to load DLL
            if(dll_name.find("\\ntdll.dll") == std::string::npos &&
                    dll_name.find("\\kernel32.dll") == std::string::npos &&
                    dll_name.find("\\KernelBase.dll") == std::string::npos &&
                    dll_name.find("\\msvcrt.dll") == std::string::npos &&
                    dll_name.find("\\wow64.dll") == std::string::npos &&
                    dll_name.find("\\wow64win.dll") == std::string::npos &&
                    dll_name.find("\\wow64cpu.dll") == std::string::npos &&
                    dll_name.find("\\user32.dll") == std::string::npos)
            {
                std::string error = "Code is up to load DLL.";
                code_state.exe_time = 0;
                code_state.exe_memory = 0;
                ExitAndSetError(process, code_state, DANGEROUS_CODE, error.c_str());
                return false;
            }
        }

        default: break;
        }

        // get the run time
        run_time = GetRunTime_(process);
        if(-1 == run_time)
        {
            std::string error = "Cannot get running time: " + GetLastErrorAsString();
            code_state.exe_time = 0;
            code_state.exe_memory = MAX(GetRunMemo_(process), code_state.exe_memory);
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            ExitAndSetError(process, code_state, SYSTEM_ERROR, error.c_str());
            return false;
        }

        // get the run memory
        run_memo = GetRunMemo_(process);
        if(-1 == run_memo)
        {
            std::string error = "Cannot get occupied memory: " + GetLastErrorAsString();
            code_state.exe_time = run_time;
            code_state.exe_memory = 0;
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            ExitAndSetError(process, code_state, SYSTEM_ERROR, error.c_str());
            return false;
        }

        code_state.exe_time = run_time;
        code_state.exe_memory = MAX(code_state.exe_memory, run_memo);

        if(run_time > time_limit)
        {
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            ExitAndSetError(process, code_state, TIME_LIMIT_EXCEEDED_1);
            return false;
        }

        if(run_memo > memo_limit)
        {
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            ExitAndSetError(process, code_state, MEMORY_LIMIT_EXCEEDED);
            return false;
        }

        // do some reaction via each DEBUG_EVENT
        switch(dbe.dwDebugEventCode)
        {
        case EXIT_PROCESS_DEBUG_EVENT:
            code_state.state = FINISHED;
            ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
            return true;

        case EXCEPTION_DEBUG_EVENT:
            if(dbe.u.Exception.ExceptionRecord.ExceptionCode != EXCEPTION_BREAKPOINT &&
                    dbe.u.Exception.ExceptionRecord.ExceptionCode != EXCEPTION_WX86_BREAKPOINT)
            {
                SetRuntimeErrorCode_(code_state, dbe.u.Exception.ExceptionRecord.ExceptionCode);
                ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_EXCEPTION_NOT_HANDLED);
                ExitAndSetError(process, code_state, RUNTIME_ERROR);
                return false;
            }
            break;

        default: break;
        }

        ContinueDebugEvent(dbe.dwProcessId, dbe.dwThreadId, DBG_CONTINUE);
        code_state.state = CONTINUE;
    }
}

};<|endoftext|>


<commit_before>/* Generated automatically by ppremake 1.11 from Sources.pp. */
/* ################################# DO NOT EDIT ########################### */

#include "fmod_audio_composite1.cxx"

<commit_msg>remove accidentally committed file<commit_after><|endoftext|>


<commit_before>#include <boost/function.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assert.hpp>
#include <math.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit_cartesian_plan_plugin/generate_cartesian_path.h>

#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>

#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>

GenerateCartesianPath::GenerateCartesianPath(QObject *parent)
{
    //to add initializations
    init();

}
GenerateCartesianPath::~GenerateCartesianPath()
{
  /*! The destructor resets the moveit_group_ and the kinematic_state of the robot.
  */
  moveit_group_.reset();
  kinematic_state.reset();
  robot_model_loader.reset();
  kmodel.reset();
}

void GenerateCartesianPath::init()
{
  /*! Initialize the MoveIt parameters:
        - MoveIt group
        - Kinematic State is the current kinematic congiguration of the Robot
        - Robot model which handles getting the Robot Model
        - Joint Model group which are necessary for checking if Way-Point is outside the IK Solution
        .
  */
  selected_plan_group = 0;
  robot_model_loader = RobotModelLoaderPtr(new robot_model_loader::RobotModelLoader("robot_description"));
  kmodel = robot_model_loader->getModel();
  //kmodel = robot_model::RobotModelConstPtr(new moveit::core::RobotModel(robot_model_loader->getModel()));

  //group_names = kmodel->getJointModelGroupNames();

  end_eff_joint_groups = kmodel->getEndEffectors();

  ROS_INFO_STREAM("size of the end effectors is: "<<end_eff_joint_groups.size());

  if (end_eff_joint_groups.empty())
  {
    group_names = kmodel->getJointModelGroupNames();
  }
  else
  {
  for(int i=0;i<end_eff_joint_groups.size();i++)
  {
    // const std::pair< std::string, std::string > & parent_group_name = end_eff_joint_groups.at(i)->getEndEffectorParentGroup();
    if(end_eff_joint_groups.at(i)->isChain())
    {
    const std::string& parent_group_name = end_eff_joint_groups.at(i)->getName();
    group_names.push_back(parent_group_name);

    ROS_INFO_STREAM("Group name:"<< group_names.at(i));
    }
    else
    {
        ROS_INFO_STREAM("This group is not a chain. Find the parent of the group");
        const std::pair< std::string, std::string > & parent_group_name = end_eff_joint_groups.at(i)->getEndEffectorParentGroup();
        group_names.push_back(parent_group_name.first);
    }
  }
}

  ROS_INFO_STREAM("Group name:"<< group_names[selected_plan_group]);

  moveit_group_ = MoveGroupPtr(new move_group_interface::MoveGroup(group_names[selected_plan_group]));
  kinematic_state = moveit::core::RobotStatePtr(new robot_state::RobotState(kmodel));
  kinematic_state->setToDefaultValues();

  joint_model_group = kmodel->getJointModelGroup(group_names[selected_plan_group]);

}
void GenerateCartesianPath::setCartParams(double plan_time_,double cart_step_size_, double cart_jump_thresh_, bool moveit_replan_,bool avoid_collisions_)
{
  /*! Set the necessary parameters for the MoveIt and the Cartesian Path Planning.
      These parameters correspond to the ones that the user has entered or the default ones before the execution of the Cartesian Path Planner.
  */
  ROS_INFO_STREAM("MoveIt and Cartesian Path parameters from UI:\n MoveIt Plan Time:"<<plan_time_
                  <<"\n Cartesian Path Step Size:"<<cart_step_size_
                  <<"\n Jump Threshold:"<<cart_jump_thresh_
                  <<"\n Replanning:"<<moveit_replan_
                  <<"\n Avoid Collisions:"<<avoid_collisions_);

  PLAN_TIME_        = plan_time_;
  MOVEIT_REPLAN_    = moveit_replan_;
  CART_STEP_SIZE_   = cart_step_size_;
  CART_JUMP_THRESH_ = cart_jump_thresh_;
  AVOID_COLLISIONS_ = avoid_collisions_;
}

void GenerateCartesianPath::moveToPose(std::vector<geometry_msgs::Pose> waypoints)
{
    /*!

    */
    Q_EMIT cartesianPathExecuteStarted();

    moveit_group_->setPlanningTime(PLAN_TIME_);
    moveit_group_->allowReplanning (MOVEIT_REPLAN_);

    move_group_interface::MoveGroup::Plan plan;

    moveit_msgs::RobotTrajectory trajectory_;
    double fraction = moveit_group_->computeCartesianPath(waypoints,CART_STEP_SIZE_,CART_JUMP_THRESH_,trajectory_,AVOID_COLLISIONS_);
    robot_trajectory::RobotTrajectory rt(kmodel, group_names[selected_plan_group]);

    rt.setRobotTrajectoryMsg(*kinematic_state, trajectory_);

    ROS_INFO_STREAM("Pose reference frame: " << moveit_group_->getPoseReferenceFrame ());

    // Thrid create a IterativeParabolicTimeParameterization object
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    bool success = iptp.computeTimeStamps(rt);
    ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

    // Get RobotTrajectory_msg from RobotTrajectory
    rt.getRobotTrajectoryMsg(trajectory_);
    // Finally plan and execute the trajectory
    plan.trajectory_ = trajectory_;
    ROS_INFO("Visualizing plan (cartesian path) (%.2f%% acheived)",fraction * 100.0);
    Q_EMIT cartesianPathCompleted(fraction);

    moveit_group_->execute(plan);

    kinematic_state = moveit_group_->getCurrentState();

    Q_EMIT cartesianPathExecuteFinished();

}

void GenerateCartesianPath::cartesianPathHandler(std::vector<geometry_msgs::Pose> waypoints)
{
  /*! Since the execution of the Cartesian path is time consuming and can lead to locking up of the Plugin and the RViz enviroment the function for executing the Cartesian Path Plan has been placed in a separtate thread.
      This prevents the RViz and the Plugin to lock.
  */
  ROS_INFO("Starting concurrent process for Cartesian Path");
  QFuture<void> future = QtConcurrent::run(this, &GenerateCartesianPath::moveToPose, waypoints);
}

void GenerateCartesianPath::checkWayPointValidity(const geometry_msgs::Pose& waypoint,const int point_number)
{
      /*! This function is called every time the user updates the pose of the Way-Point and checks if the Way-Point is within the valid IK solution for the Robot.
          In the case when a point is outside the valid IK solution this function send a signal to the RViz enviroment to update the color of the Way-Point.
      */
       bool found_ik = kinematic_state->setFromIK(joint_model_group, waypoint, 3, 0.006);

         if(found_ik)
        {

           Q_EMIT wayPointOutOfIK(point_number,0);
        }
        else
        {
          // ROS_INFO("Did not find IK solution for waypoint %d",point_number);
          Q_EMIT wayPointOutOfIK(point_number,1);
        }
}

void GenerateCartesianPath::initRviz_done()
{
  /*! Once the initialization of the RViz is has finished, this function sends the pose of the robot end-effector and the name of the base frame to the RViz enviroment.
      The RViz enviroment sets the User Interactive Marker pose and Add New Way-Point RQT Layout default values based on the end-effector starting position.
      The transformation frame of the InteractiveMarker is set based on the robot PoseReferenceFrame.
  */
  ROS_INFO("RViz is done now we need to emit the signal");

  if(moveit_group_->getEndEffectorLink().empty())
  {
    ROS_INFO("End effector link is empty");
    const std::vector< std::string > &  joint_names = joint_model_group->getLinkModelNames();
    for(int i=0;i<joint_names.size();i++)
    {
      ROS_INFO_STREAM("Link " << i << " name: "<< joint_names.at(i));
    }
    const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform(joint_names.at(0));
    //tf::Transform end_effector;
    tf::transformEigenToTF(end_effector_state, end_effector);
    Q_EMIT getRobotModelFrame_signal(moveit_group_->getPoseReferenceFrame(),end_effector);
  }
  else
  {

    ROS_INFO("End effector link is not empty");
    const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform(moveit_group_->getEndEffectorLink());
    //tf::Transform end_effector;
    tf::transformEigenToTF(end_effector_state, end_effector);

    Q_EMIT getRobotModelFrame_signal(moveit_group_->getPoseReferenceFrame(),end_effector);
  }

    Q_EMIT sendCartPlanGroup(group_names);

}
void GenerateCartesianPath::moveToHome()
{

  geometry_msgs::Pose home_pose;
  tf::poseTFToMsg(end_effector,home_pose);

  std::vector<geometry_msgs::Pose> waypoints;
  waypoints.push_back(home_pose);

  cartesianPathHandler(waypoints);

}

void GenerateCartesianPath::getSelectedGroupIndex(int index)
{
  selected_plan_group = index;

  ROS_INFO_STREAM("selected name is:"<<group_names[selected_plan_group]);
  moveit_group_.reset();
  kinematic_state.reset();
  moveit_group_ = MoveGroupPtr(new move_group_interface::MoveGroup(group_names[selected_plan_group]));

  kinematic_state = moveit::core::RobotStatePtr(new robot_state::RobotState(kmodel));
  kinematic_state->setToDefaultValues();

  joint_model_group = kmodel->getJointModelGroup(group_names[selected_plan_group]);

  if(moveit_group_->getEndEffectorLink().empty())
  {
    ROS_INFO("End effector link is empty");
    const std::vector< std::string > &  joint_names = joint_model_group->getLinkModelNames();
    for(int i=0;i<joint_names.size();i++)
    {
      ROS_INFO_STREAM("Link " << i << " name: "<< joint_names.at(i));
    }
    const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform(joint_names.at(0));
    //tf::Transform end_effector;
    tf::transformEigenToTF(end_effector_state, end_effector);
    Q_EMIT getRobotModelFrame_signal(moveit_group_->getPoseReferenceFrame(),end_effector);
  }
  else
  {

    ROS_INFO("End effector link is not empty");
    const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform(moveit_group_->getEndEffectorLink());
    //tf::Transform end_effector;
    tf::transformEigenToTF(end_effector_state, end_effector);

    Q_EMIT getRobotModelFrame_signal(moveit_group_->getPoseReferenceFrame(),end_effector);
  }

}<commit_msg>Further imporvement of setting up starting group for different moveit configuration robots. Checking for end effectors and setting planning group only if the end-effector is a kinematic chain.<commit_after>#include <boost/function.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assert.hpp>
#include <math.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit_cartesian_plan_plugin/generate_cartesian_path.h>

#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>

#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>

GenerateCartesianPath::GenerateCartesianPath(QObject *parent)
{
    //to add initializations
    init();

}
GenerateCartesianPath::~GenerateCartesianPath()
{
  /*! The destructor resets the moveit_group_ and the kinematic_state of the robot.
  */
  moveit_group_.reset();
  kinematic_state.reset();
  robot_model_loader.reset();
  kmodel.reset();
}

void GenerateCartesianPath::init()
{
  /*! Initialize the MoveIt parameters:
        - MoveIt group
        - Kinematic State is the current kinematic congiguration of the Robot
        - Robot model which handles getting the Robot Model
        - Joint Model group which are necessary for checking if Way-Point is outside the IK Solution
        .
  */
  selected_plan_group = 0;
  robot_model_loader = RobotModelLoaderPtr(new robot_model_loader::RobotModelLoader("robot_description"));
  kmodel = robot_model_loader->getModel();
  //kmodel = robot_model::RobotModelConstPtr(new moveit::core::RobotModel(robot_model_loader->getModel()));

  //group_names = kmodel->getJointModelGroupNames();

  end_eff_joint_groups = kmodel->getEndEffectors();

  ROS_INFO_STREAM("size of the end effectors is: "<<end_eff_joint_groups.size());

  if (end_eff_joint_groups.empty())
  {
    std::vector< std::string > group_names_tmp_;
    const moveit::core::JointModelGroup *  end_eff_joint_groups_tmp_;
    group_names_tmp_ = kmodel->getJointModelGroupNames();

    for(int i=0;i<group_names_tmp_.size();i++)
    {

    end_eff_joint_groups_tmp_ = kmodel->getJointModelGroup(group_names_tmp_.at(i));
    if(end_eff_joint_groups_tmp_->isChain())
    {
      group_names.push_back(group_names_tmp_.at(i));
    }
    else
    {
      ROS_INFO_STREAM("The group:" << end_eff_joint_groups_tmp_->getName() <<" is not a Chain. Depreciate it!!");
    }
    }
  }
  else
  {
  for(int i=0;i<end_eff_joint_groups.size();i++)
  {
    // const std::pair< std::string, std::string > & parent_group_name = end_eff_joint_groups.at(i)->getEndEffectorParentGroup();
    if(end_eff_joint_groups.at(i)->isChain())
    {
    const std::string& parent_group_name = end_eff_joint_groups.at(i)->getName();
    group_names.push_back(parent_group_name);

    ROS_INFO_STREAM("Group name:"<< group_names.at(i));
    }
    else
    {
        ROS_INFO_STREAM("This group is not a chain. Find the parent of the group");
        const std::pair< std::string, std::string > & parent_group_name = end_eff_joint_groups.at(i)->getEndEffectorParentGroup();
        group_names.push_back(parent_group_name.first);
    }
  }
}

  ROS_INFO_STREAM("Group name:"<< group_names[selected_plan_group]);

  moveit_group_ = MoveGroupPtr(new move_group_interface::MoveGroup(group_names[selected_plan_group]));
  kinematic_state = moveit::core::RobotStatePtr(new robot_state::RobotState(kmodel));
  kinematic_state->setToDefaultValues();

  joint_model_group = kmodel->getJointModelGroup(group_names[selected_plan_group]);

}
void GenerateCartesianPath::setCartParams(double plan_time_,double cart_step_size_, double cart_jump_thresh_, bool moveit_replan_,bool avoid_collisions_)
{
  /*! Set the necessary parameters for the MoveIt and the Cartesian Path Planning.
      These parameters correspond to the ones that the user has entered or the default ones before the execution of the Cartesian Path Planner.
  */
  ROS_INFO_STREAM("MoveIt and Cartesian Path parameters from UI:\n MoveIt Plan Time:"<<plan_time_
                  <<"\n Cartesian Path Step Size:"<<cart_step_size_
                  <<"\n Jump Threshold:"<<cart_jump_thresh_
                  <<"\n Replanning:"<<moveit_replan_
                  <<"\n Avoid Collisions:"<<avoid_collisions_);

  PLAN_TIME_        = plan_time_;
  MOVEIT_REPLAN_    = moveit_replan_;
  CART_STEP_SIZE_   = cart_step_size_;
  CART_JUMP_THRESH_ = cart_jump_thresh_;
  AVOID_COLLISIONS_ = avoid_collisions_;
}

void GenerateCartesianPath::moveToPose(std::vector<geometry_msgs::Pose> waypoints)
{
    /*!

    */
    Q_EMIT cartesianPathExecuteStarted();

    moveit_group_->setPlanningTime(PLAN_TIME_);
    moveit_group_->allowReplanning (MOVEIT_REPLAN_);

    move_group_interface::MoveGroup::Plan plan;

    moveit_msgs::RobotTrajectory trajectory_;
    double fraction = moveit_group_->computeCartesianPath(waypoints,CART_STEP_SIZE_,CART_JUMP_THRESH_,trajectory_,AVOID_COLLISIONS_);
    robot_trajectory::RobotTrajectory rt(kmodel, group_names[selected_plan_group]);

    rt.setRobotTrajectoryMsg(*kinematic_state, trajectory_);

    ROS_INFO_STREAM("Pose reference frame: " << moveit_group_->getPoseReferenceFrame ());

    // Thrid create a IterativeParabolicTimeParameterization object
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    bool success = iptp.computeTimeStamps(rt);
    ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

    // Get RobotTrajectory_msg from RobotTrajectory
    rt.getRobotTrajectoryMsg(trajectory_);
    // Finally plan and execute the trajectory
    plan.trajectory_ = trajectory_;
    ROS_INFO("Visualizing plan (cartesian path) (%.2f%% acheived)",fraction * 100.0);
    Q_EMIT cartesianPathCompleted(fraction);

    moveit_group_->execute(plan);

    kinematic_state = moveit_group_->getCurrentState();

    Q_EMIT cartesianPathExecuteFinished();

}

void GenerateCartesianPath::cartesianPathHandler(std::vector<geometry_msgs::Pose> waypoints)
{
  /*! Since the execution of the Cartesian path is time consuming and can lead to locking up of the Plugin and the RViz enviroment the function for executing the Cartesian Path Plan has been placed in a separtate thread.
      This prevents the RViz and the Plugin to lock.
  */
  ROS_INFO("Starting concurrent process for Cartesian Path");
  QFuture<void> future = QtConcurrent::run(this, &GenerateCartesianPath::moveToPose, waypoints);
}

void GenerateCartesianPath::checkWayPointValidity(const geometry_msgs::Pose& waypoint,const int point_number)
{
      /*! This function is called every time the user updates the pose of the Way-Point and checks if the Way-Point is within the valid IK solution for the Robot.
          In the case when a point is outside the valid IK solution this function send a signal to the RViz enviroment to update the color of the Way-Point.
      */
       bool found_ik = kinematic_state->setFromIK(joint_model_group, waypoint, 3, 0.006);

         if(found_ik)
        {

           Q_EMIT wayPointOutOfIK(point_number,0);
        }
        else
        {
          // ROS_INFO("Did not find IK solution for waypoint %d",point_number);
          Q_EMIT wayPointOutOfIK(point_number,1);
        }
}

void GenerateCartesianPath::initRviz_done()
{
  /*! Once the initialization of the RViz is has finished, this function sends the pose of the robot end-effector and the name of the base frame to the RViz enviroment.
      The RViz enviroment sets the User Interactive Marker pose and Add New Way-Point RQT Layout default values based on the end-effector starting position.
      The transformation frame of the InteractiveMarker is set based on the robot PoseReferenceFrame.
  */
  ROS_INFO("RViz is done now we need to emit the signal");

  if(moveit_group_->getEndEffectorLink().empty())
  {
    ROS_INFO("End effector link is empty");
    const std::vector< std::string > &  joint_names = joint_model_group->getLinkModelNames();
    for(int i=0;i<joint_names.size();i++)
    {
      ROS_INFO_STREAM("Link " << i << " name: "<< joint_names.at(i));
    }
    const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform(joint_names.at(0));
    //tf::Transform end_effector;
    tf::transformEigenToTF(end_effector_state, end_effector);
    Q_EMIT getRobotModelFrame_signal(moveit_group_->getPoseReferenceFrame(),end_effector);
  }
  else
  {

    ROS_INFO("End effector link is not empty");
    const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform(moveit_group_->getEndEffectorLink());
    //tf::Transform end_effector;
    tf::transformEigenToTF(end_effector_state, end_effector);

    Q_EMIT getRobotModelFrame_signal(moveit_group_->getPoseReferenceFrame(),end_effector);
  }

    Q_EMIT sendCartPlanGroup(group_names);

}
void GenerateCartesianPath::moveToHome()
{

  geometry_msgs::Pose home_pose;
  tf::poseTFToMsg(end_effector,home_pose);

  std::vector<geometry_msgs::Pose> waypoints;
  waypoints.push_back(home_pose);

  cartesianPathHandler(waypoints);

}

void GenerateCartesianPath::getSelectedGroupIndex(int index)
{
  selected_plan_group = index;

  ROS_INFO_STREAM("selected name is:"<<group_names[selected_plan_group]);
  moveit_group_.reset();
  kinematic_state.reset();
  moveit_group_ = MoveGroupPtr(new move_group_interface::MoveGroup(group_names[selected_plan_group]));

  kinematic_state = moveit::core::RobotStatePtr(new robot_state::RobotState(kmodel));
  kinematic_state->setToDefaultValues();

  joint_model_group = kmodel->getJointModelGroup(group_names[selected_plan_group]);

   //for debugging only. This will be left out after some testing
  // if(moveit_group_->getEndEffectorLink().empty())
  // {
  //   ROS_INFO("End effector link is empty");
  //   const std::vector< std::string > &  joint_names = joint_model_group->getLinkModelNames();
  //   for(int i=0;i<joint_names.size();i++)
  //   {
  //     ROS_INFO_STREAM("Link " << i << " name: "<< joint_names.at(i));
  //   }
  //   const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform(joint_names.at(0));
  //   //tf::Transform end_effector;
  //   tf::transformEigenToTF(end_effector_state, end_effector);
  //   Q_EMIT getRobotModelFrame_signal(moveit_group_->getPoseReferenceFrame(),end_effector);
  // }
  // else
  // {

    ROS_INFO("End effector link is not empty");
    const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform(moveit_group_->getEndEffectorLink());
    //tf::Transform end_effector;
    tf::transformEigenToTF(end_effector_state, end_effector);

    Q_EMIT getRobotModelFrame_signal(moveit_group_->getPoseReferenceFrame(),end_effector);
  //}

}<|endoftext|>


<commit_before>/* Copyright 2007-2015 QReal Research Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include "kitBase/blocksBase/common/waitForEncoderBlock.h"

#include "kitBase/robotModel/robotParts/encoderSensor.h"
#include "kitBase/robotModel/robotModelUtils.h"

using namespace kitBase;
using namespace blocksBase::common;
using namespace robotModel;

WaitForEncoderBlock::WaitForEncoderBlock(RobotModelInterface &robotModel)
	: WaitForSensorBlock(robotModel)
{
}

void WaitForEncoderBlock::responseSlot(int reading)
{
	const int result = eval<int>("TachoLimit");
	if (!errorsOccured()) {
		processResponce(reading, result);
	}
}

DeviceInfo WaitForEncoderBlock::device() const
{
	return DeviceInfo::create<robotParts::EncoderSensor>();
}
<commit_msg>Reduced active waiting timeout for encoder block<commit_after>/* Copyright 2007-2015 QReal Research Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include "kitBase/blocksBase/common/waitForEncoderBlock.h"

#include "kitBase/robotModel/robotParts/encoderSensor.h"
#include "kitBase/robotModel/robotModelUtils.h"

using namespace kitBase;
using namespace blocksBase::common;
using namespace robotModel;

WaitForEncoderBlock::WaitForEncoderBlock(RobotModelInterface &robotModel)
	: WaitForSensorBlock(robotModel)
{
	mActiveWaitingTimer.setInterval(1);
}

void WaitForEncoderBlock::responseSlot(int reading)
{
	const int result = eval<int>("TachoLimit");
	if (!errorsOccured()) {
		processResponce(reading, result);
	}
}

DeviceInfo WaitForEncoderBlock::device() const
{
	return DeviceInfo::create<robotParts::EncoderSensor>();
}
<|endoftext|>


<commit_before>

#include "Globals.h"

#include "IncrementalRedstoneSimulator.h"
#include "../../Chunk.h"

#include "CommandBlockHandler.h"
#include "DoorHandler.h"
#include "RedstoneTorchHandler.h"
#include "RedstoneWireHandler.h"
#include "RedstoneRepeaterHandler.h"
#include "RedstoneToggleHandler.h"
#include "SolidBlockHandler.h"
#include "RedstoneLampHandler.h"
#include "RedstoneBlockHandler.h"
#include "PistonHandler.h"
#include "SmallGateHandler.h"
#include "NoteBlockHandler.h"
#include "ObserverHandler.h"
#include "TNTHandler.h"
#include "PoweredRailHandler.h"
#include "PressurePlateHandler.h"
#include "TripwireHookHandler.h"
#include "DropSpenserHandler.h"
#include "RedstoneComparatorHandler.h"
#include "TrappedChestHandler.h"
#include "HopperHandler.h"

const cRedstoneHandler * cIncrementalRedstoneSimulator::GetComponentHandler(BLOCKTYPE a_BlockType)
{
	struct sComponents :
		public std::array<std::unique_ptr<cRedstoneHandler>, 256>
	{
		sComponents()
		{
			for (size_t i = 0; i != 256; ++i)
			{
				(*this)[i] = cIncrementalRedstoneSimulator::CreateComponent(static_cast<BLOCKTYPE>(i));
			}
		}
	};

	static sComponents Components;
	return Components[a_BlockType].get();
}

std::unique_ptr<cRedstoneHandler> cIncrementalRedstoneSimulator::CreateComponent(BLOCKTYPE a_BlockType)
{
	switch (a_BlockType)
	{
		case E_BLOCK_ACTIVATOR_RAIL:
		case E_BLOCK_DETECTOR_RAIL:
		case E_BLOCK_POWERED_RAIL: return cpp14::make_unique<cPoweredRailHandler>();

		case E_BLOCK_ACTIVE_COMPARATOR:
		case E_BLOCK_INACTIVE_COMPARATOR: return cpp14::make_unique<cRedstoneComparatorHandler>();

		case E_BLOCK_DISPENSER:
		case E_BLOCK_DROPPER: return cpp14::make_unique<cDropSpenserHandler>();

		case E_BLOCK_HEAVY_WEIGHTED_PRESSURE_PLATE:
		case E_BLOCK_LIGHT_WEIGHTED_PRESSURE_PLATE:
		case E_BLOCK_STONE_PRESSURE_PLATE:
		case E_BLOCK_WOODEN_PRESSURE_PLATE: return cpp14::make_unique<cPressurePlateHandler>();

		case E_BLOCK_ACACIA_FENCE_GATE:
		case E_BLOCK_BIRCH_FENCE_GATE:
		case E_BLOCK_DARK_OAK_FENCE_GATE:
		case E_BLOCK_FENCE_GATE:
		case E_BLOCK_IRON_TRAPDOOR:
		case E_BLOCK_JUNGLE_FENCE_GATE:
		case E_BLOCK_SPRUCE_FENCE_GATE:
		case E_BLOCK_TRAPDOOR: return cpp14::make_unique<cSmallGateHandler>();

		case E_BLOCK_REDSTONE_LAMP_OFF:
		case E_BLOCK_REDSTONE_LAMP_ON: return cpp14::make_unique<cRedstoneLampHandler>();

		case E_BLOCK_REDSTONE_REPEATER_OFF:
		case E_BLOCK_REDSTONE_REPEATER_ON: return cpp14::make_unique<cRedstoneRepeaterHandler>();

		case E_BLOCK_REDSTONE_TORCH_OFF:
		case E_BLOCK_REDSTONE_TORCH_ON: return cpp14::make_unique<cRedstoneTorchHandler>();

		case E_BLOCK_OBSERVER: return cpp14::make_unique<cObserverHandler>();

		case E_BLOCK_PISTON:
		case E_BLOCK_STICKY_PISTON: return cpp14::make_unique<cPistonHandler>();

		case E_BLOCK_LEVER:
		case E_BLOCK_STONE_BUTTON:
		case E_BLOCK_WOODEN_BUTTON: return cpp14::make_unique<cRedstoneToggleHandler>();

		case E_BLOCK_BLOCK_OF_REDSTONE: return cpp14::make_unique<cRedstoneBlockHandler>();
		case E_BLOCK_COMMAND_BLOCK: return cpp14::make_unique<cCommandBlockHandler>();
		case E_BLOCK_HOPPER: return cpp14::make_unique<cHopperHandler>();
		case E_BLOCK_NOTE_BLOCK: return cpp14::make_unique<cNoteBlockHandler>();
		case E_BLOCK_REDSTONE_WIRE: return cpp14::make_unique<cRedstoneWireHandler>();
		case E_BLOCK_TNT: return cpp14::make_unique<cTNTHandler>();
		case E_BLOCK_TRAPPED_CHEST: return cpp14::make_unique<cTrappedChestHandler>();
		case E_BLOCK_TRIPWIRE_HOOK: return cpp14::make_unique<cTripwireHookHandler>();
		default:
		{
			if (cBlockDoorHandler::IsDoorBlockType(a_BlockType))
			{
				return cpp14::make_unique<cDoorHandler>();
			}

			if (cBlockInfo::FullyOccupiesVoxel(a_BlockType))
			{
				return cpp14::make_unique<cSolidBlockHandler>();
			}
			return nullptr;
		}
	}
}

void cIncrementalRedstoneSimulator::SimulateChunk(std::chrono::milliseconds a_Dt, int a_ChunkX, int a_ChunkZ, cChunk * a_Chunk)
{
	auto & ChunkData = *static_cast<cIncrementalRedstoneSimulatorChunkData *>(a_Chunk->GetRedstoneSimulatorData());
	for (auto & DelayInfo : ChunkData.m_MechanismDelays)
	{
		if ((--DelayInfo.second.first) == 0)
		{
			ChunkData.WakeUp(DelayInfo.first);
		}
	}

	// Build our work queue
	auto & WorkQueue = ChunkData.GetActiveBlocks();

	// Process the work queue
	while (!WorkQueue.empty())
	{
		// Grab the first element and remove it from the list
		Vector3i CurrentLocation = WorkQueue.top();
		WorkQueue.pop();

		const auto NeighbourChunk = a_Chunk->GetRelNeighborChunkAdjustCoords(CurrentLocation);
		if ((NeighbourChunk == nullptr) || !NeighbourChunk->IsValid())
		{
			return;
		}

		ProcessWorkItem(*NeighbourChunk, *a_Chunk, CurrentLocation);
	}

	for (const auto Position : ChunkData.AlwaysTickedPositions)
	{
		ChunkData.WakeUp(Position);
	}
}

void cIncrementalRedstoneSimulator::ProcessWorkItem(cChunk & Chunk, cChunk & TickingSource, const Vector3i Position)
{
	auto & ChunkData = *static_cast<cIncrementalRedstoneSimulatorChunkData *>(Chunk.GetRedstoneSimulatorData());

	BLOCKTYPE CurrentBlock;
	NIBBLETYPE CurrentMeta;
	Chunk.GetBlockTypeMeta(Position, CurrentBlock, CurrentMeta);

	auto CurrentHandler = GetComponentHandler(CurrentBlock);
	if (CurrentHandler == nullptr)  // Block at CurrentPosition doesn't have a corresponding redstone handler
	{
		// Clean up cached PowerData for CurrentPosition
		ChunkData.ErasePowerData(Position);
		return;
	}

	PoweringData Power;
	CurrentHandler->ForValidSourcePositions(Chunk, Position, CurrentBlock, CurrentMeta, [&Chunk, Position, CurrentBlock, &Power](Vector3i Location)
	{
		if (!cChunk::IsValidHeight(Location.y))
		{
			return;
		}

		const auto NeighbourChunk = Chunk.GetRelNeighborChunkAdjustCoords(Location);
		if ((NeighbourChunk == nullptr) || !NeighbourChunk->IsValid())
		{
			return;
		}

		BLOCKTYPE PotentialBlock;
		NIBBLETYPE PotentialMeta;
		NeighbourChunk->GetBlockTypeMeta(Location, PotentialBlock, PotentialMeta);

		auto PotentialSourceHandler = GetComponentHandler(PotentialBlock);
		if (PotentialSourceHandler == nullptr)
		{
			return;
		}

		const PoweringData PotentialPower(
			PotentialBlock,
			PotentialSourceHandler->GetPowerDeliveredToPosition(
				*NeighbourChunk, Location, PotentialBlock, PotentialMeta,
				cIncrementalRedstoneSimulatorChunkData::RebaseRelativePosition(Chunk, *NeighbourChunk, Position), CurrentBlock
			)
		);
		Power = std::max(Power, PotentialPower);
	});

	// Inform the handler to update
	CurrentHandler->Update(Chunk, TickingSource, Position, CurrentBlock, CurrentMeta, Power);
}

void cIncrementalRedstoneSimulator::AddBlock(Vector3i a_Block, cChunk * a_Chunk)
{
	// Can't inspect block, ignore:
	if ((a_Chunk == nullptr) || (!a_Chunk->IsValid()))
	{
		return;
	}

	auto & ChunkData = *static_cast<cIncrementalRedstoneSimulatorChunkData *>(a_Chunk->GetRedstoneSimulatorData());
	const auto Relative = cChunkDef::AbsoluteToRelative(a_Block, a_Chunk->GetPos());
	const auto CurrentBlock = a_Chunk->GetBlock(Relative);

	// Always update redstone devices
	if (IsRedstone(CurrentBlock))
	{
		if (IsAlwaysTicked(CurrentBlock))
		{
			ChunkData.AlwaysTickedPositions.emplace(Relative);
		}
		ChunkData.WakeUp(Relative);
		return;
	}

	// Never update blocks without a handler
	if (GetComponentHandler(CurrentBlock) == nullptr)
	{
		ChunkData.ErasePowerData(Relative);
		return;
	}

	// Only update others if there is a redstone device nearby
	for (int x = -1; x < 2; ++x)
	{
		for (int y = -1; y < 2; ++y)
		{
			if (!cChunkDef::IsValidHeight(Relative.y + y))
			{
				continue;
			}

			for (int z = -1; z < 2; ++z)
			{
				auto CheckPos = Relative + Vector3i{x, y, z};
				BLOCKTYPE Block;
				NIBBLETYPE Meta;

				// If we can't read the block, assume it is a mechanism
				if (
					!a_Chunk->UnboundedRelGetBlock(CheckPos, Block, Meta) ||
					IsRedstone(Block)
				)
				{
					ChunkData.WakeUp(Relative);
					return;
				}
			}
		}
	}
}
<commit_msg>Remove redundant ErasePowerData call<commit_after>

#include "Globals.h"

#include "IncrementalRedstoneSimulator.h"
#include "../../Chunk.h"

#include "CommandBlockHandler.h"
#include "DoorHandler.h"
#include "RedstoneTorchHandler.h"
#include "RedstoneWireHandler.h"
#include "RedstoneRepeaterHandler.h"
#include "RedstoneToggleHandler.h"
#include "SolidBlockHandler.h"
#include "RedstoneLampHandler.h"
#include "RedstoneBlockHandler.h"
#include "PistonHandler.h"
#include "SmallGateHandler.h"
#include "NoteBlockHandler.h"
#include "ObserverHandler.h"
#include "TNTHandler.h"
#include "PoweredRailHandler.h"
#include "PressurePlateHandler.h"
#include "TripwireHookHandler.h"
#include "DropSpenserHandler.h"
#include "RedstoneComparatorHandler.h"
#include "TrappedChestHandler.h"
#include "HopperHandler.h"

const cRedstoneHandler * cIncrementalRedstoneSimulator::GetComponentHandler(BLOCKTYPE a_BlockType)
{
	struct sComponents :
		public std::array<std::unique_ptr<cRedstoneHandler>, 256>
	{
		sComponents()
		{
			for (size_t i = 0; i != 256; ++i)
			{
				(*this)[i] = cIncrementalRedstoneSimulator::CreateComponent(static_cast<BLOCKTYPE>(i));
			}
		}
	};

	static sComponents Components;
	return Components[a_BlockType].get();
}

std::unique_ptr<cRedstoneHandler> cIncrementalRedstoneSimulator::CreateComponent(BLOCKTYPE a_BlockType)
{
	switch (a_BlockType)
	{
		case E_BLOCK_ACTIVATOR_RAIL:
		case E_BLOCK_DETECTOR_RAIL:
		case E_BLOCK_POWERED_RAIL: return cpp14::make_unique<cPoweredRailHandler>();

		case E_BLOCK_ACTIVE_COMPARATOR:
		case E_BLOCK_INACTIVE_COMPARATOR: return cpp14::make_unique<cRedstoneComparatorHandler>();

		case E_BLOCK_DISPENSER:
		case E_BLOCK_DROPPER: return cpp14::make_unique<cDropSpenserHandler>();

		case E_BLOCK_HEAVY_WEIGHTED_PRESSURE_PLATE:
		case E_BLOCK_LIGHT_WEIGHTED_PRESSURE_PLATE:
		case E_BLOCK_STONE_PRESSURE_PLATE:
		case E_BLOCK_WOODEN_PRESSURE_PLATE: return cpp14::make_unique<cPressurePlateHandler>();

		case E_BLOCK_ACACIA_FENCE_GATE:
		case E_BLOCK_BIRCH_FENCE_GATE:
		case E_BLOCK_DARK_OAK_FENCE_GATE:
		case E_BLOCK_FENCE_GATE:
		case E_BLOCK_IRON_TRAPDOOR:
		case E_BLOCK_JUNGLE_FENCE_GATE:
		case E_BLOCK_SPRUCE_FENCE_GATE:
		case E_BLOCK_TRAPDOOR: return cpp14::make_unique<cSmallGateHandler>();

		case E_BLOCK_REDSTONE_LAMP_OFF:
		case E_BLOCK_REDSTONE_LAMP_ON: return cpp14::make_unique<cRedstoneLampHandler>();

		case E_BLOCK_REDSTONE_REPEATER_OFF:
		case E_BLOCK_REDSTONE_REPEATER_ON: return cpp14::make_unique<cRedstoneRepeaterHandler>();

		case E_BLOCK_REDSTONE_TORCH_OFF:
		case E_BLOCK_REDSTONE_TORCH_ON: return cpp14::make_unique<cRedstoneTorchHandler>();

		case E_BLOCK_OBSERVER: return cpp14::make_unique<cObserverHandler>();

		case E_BLOCK_PISTON:
		case E_BLOCK_STICKY_PISTON: return cpp14::make_unique<cPistonHandler>();

		case E_BLOCK_LEVER:
		case E_BLOCK_STONE_BUTTON:
		case E_BLOCK_WOODEN_BUTTON: return cpp14::make_unique<cRedstoneToggleHandler>();

		case E_BLOCK_BLOCK_OF_REDSTONE: return cpp14::make_unique<cRedstoneBlockHandler>();
		case E_BLOCK_COMMAND_BLOCK: return cpp14::make_unique<cCommandBlockHandler>();
		case E_BLOCK_HOPPER: return cpp14::make_unique<cHopperHandler>();
		case E_BLOCK_NOTE_BLOCK: return cpp14::make_unique<cNoteBlockHandler>();
		case E_BLOCK_REDSTONE_WIRE: return cpp14::make_unique<cRedstoneWireHandler>();
		case E_BLOCK_TNT: return cpp14::make_unique<cTNTHandler>();
		case E_BLOCK_TRAPPED_CHEST: return cpp14::make_unique<cTrappedChestHandler>();
		case E_BLOCK_TRIPWIRE_HOOK: return cpp14::make_unique<cTripwireHookHandler>();
		default:
		{
			if (cBlockDoorHandler::IsDoorBlockType(a_BlockType))
			{
				return cpp14::make_unique<cDoorHandler>();
			}

			if (cBlockInfo::FullyOccupiesVoxel(a_BlockType))
			{
				return cpp14::make_unique<cSolidBlockHandler>();
			}
			return nullptr;
		}
	}
}

void cIncrementalRedstoneSimulator::SimulateChunk(std::chrono::milliseconds a_Dt, int a_ChunkX, int a_ChunkZ, cChunk * a_Chunk)
{
	auto & ChunkData = *static_cast<cIncrementalRedstoneSimulatorChunkData *>(a_Chunk->GetRedstoneSimulatorData());
	for (auto & DelayInfo : ChunkData.m_MechanismDelays)
	{
		if ((--DelayInfo.second.first) == 0)
		{
			ChunkData.WakeUp(DelayInfo.first);
		}
	}

	// Build our work queue
	auto & WorkQueue = ChunkData.GetActiveBlocks();

	// Process the work queue
	while (!WorkQueue.empty())
	{
		// Grab the first element and remove it from the list
		Vector3i CurrentLocation = WorkQueue.top();
		WorkQueue.pop();

		const auto NeighbourChunk = a_Chunk->GetRelNeighborChunkAdjustCoords(CurrentLocation);
		if ((NeighbourChunk == nullptr) || !NeighbourChunk->IsValid())
		{
			continue;
		}

		ProcessWorkItem(*NeighbourChunk, *a_Chunk, CurrentLocation);
	}

	for (const auto Position : ChunkData.AlwaysTickedPositions)
	{
		ChunkData.WakeUp(Position);
	}
}

void cIncrementalRedstoneSimulator::ProcessWorkItem(cChunk & Chunk, cChunk & TickingSource, const Vector3i Position)
{
	BLOCKTYPE CurrentBlock;
	NIBBLETYPE CurrentMeta;
	Chunk.GetBlockTypeMeta(Position, CurrentBlock, CurrentMeta);

	auto CurrentHandler = GetComponentHandler(CurrentBlock);
	if (CurrentHandler == nullptr)
	{
		// Block at Position doesn't have a corresponding redstone handler
		// ErasePowerData will have been called in AddBlock
		return;
	}

	PoweringData Power;
	CurrentHandler->ForValidSourcePositions(Chunk, Position, CurrentBlock, CurrentMeta, [&Chunk, Position, CurrentBlock, &Power](Vector3i Location)
	{
		if (!cChunk::IsValidHeight(Location.y))
		{
			return;
		}

		const auto NeighbourChunk = Chunk.GetRelNeighborChunkAdjustCoords(Location);
		if ((NeighbourChunk == nullptr) || !NeighbourChunk->IsValid())
		{
			return;
		}

		BLOCKTYPE PotentialBlock;
		NIBBLETYPE PotentialMeta;
		NeighbourChunk->GetBlockTypeMeta(Location, PotentialBlock, PotentialMeta);

		auto PotentialSourceHandler = GetComponentHandler(PotentialBlock);
		if (PotentialSourceHandler == nullptr)
		{
			return;
		}

		const PoweringData PotentialPower(
			PotentialBlock,
			PotentialSourceHandler->GetPowerDeliveredToPosition(
				*NeighbourChunk, Location, PotentialBlock, PotentialMeta,
				cIncrementalRedstoneSimulatorChunkData::RebaseRelativePosition(Chunk, *NeighbourChunk, Position), CurrentBlock
			)
		);
		Power = std::max(Power, PotentialPower);
	});

	// Inform the handler to update
	CurrentHandler->Update(Chunk, TickingSource, Position, CurrentBlock, CurrentMeta, Power);
}

void cIncrementalRedstoneSimulator::AddBlock(Vector3i a_Block, cChunk * a_Chunk)
{
	// Can't inspect block, ignore:
	if ((a_Chunk == nullptr) || !a_Chunk->IsValid())
	{
		return;
	}

	auto & ChunkData = *static_cast<cIncrementalRedstoneSimulatorChunkData *>(a_Chunk->GetRedstoneSimulatorData());
	const auto Relative = cChunkDef::AbsoluteToRelative(a_Block, a_Chunk->GetPos());
	const auto CurrentBlock = a_Chunk->GetBlock(Relative);

	// Always update redstone devices
	if (IsRedstone(CurrentBlock))
	{
		if (IsAlwaysTicked(CurrentBlock))
		{
			ChunkData.AlwaysTickedPositions.emplace(Relative);
		}
		ChunkData.WakeUp(Relative);
		return;
	}

	// Never update blocks without a handler
	if (GetComponentHandler(CurrentBlock) == nullptr)
	{
		ChunkData.ErasePowerData(Relative);
		return;
	}

	// Only update others if there is a redstone device nearby
	for (int x = -1; x < 2; ++x)
	{
		for (int y = -1; y < 2; ++y)
		{
			if (!cChunkDef::IsValidHeight(Relative.y + y))
			{
				continue;
			}

			for (int z = -1; z < 2; ++z)
			{
				auto CheckPos = Relative + Vector3i{x, y, z};
				BLOCKTYPE Block;
				NIBBLETYPE Meta;

				// If we can't read the block, assume it is a mechanism
				if (
					!a_Chunk->UnboundedRelGetBlock(CheckPos, Block, Meta) ||
					IsRedstone(Block)
				)
				{
					ChunkData.WakeUp(Relative);
					return;
				}
			}
		}
	}
}
<|endoftext|>