SimsWorld/ComponentBasic/SurfaceMotionPlugin.cpp

#include "SurfaceMotionPlugin.h"

SurfaceMotionControlPlugin::SurfaceMotionControlPlugin()
{

}

void SurfaceMotionControlPlugin::recoveryFrom(const QJsonObject& obj)
{
}

void SurfaceMotionControlPlugin::saveTo(QJsonObject& obj) const
{
}

void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
	std::shared_ptr<const HorizontalArcMotion> in, QList<std::shared_ptr<RespondDefault>>& out)
{
	auto exec_result = std::make_shared<RespondDefault>();
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC><CEBB>
	if (_current_cmd) {
		auto exec = std::make_shared<MotionDeduceRequest>();
		exec->reset(in->targetEntity(), in->sourceEntity());
		exec->_target_time = _current_cmd->startTimepoint();
		map->execute(exec);
	}

	auto req = std::make_shared<Get3DBox>();
	req->reset(this->_bind_entity.lock()->entityID(), this->_bind_entity.lock()->entityID());
	auto rst = map->execute<Get3DBox, Box3DDesc>(req);
	if (rst.size()) {
		// <20><><EFBFBD>浱ǰ<E6B5B1><C7B0><EFBFBD><EFBFBD>
		this->_current_cmd = in;
		// <20><><EFBFBD>»<EFBFBD>׼<EFBFBD><D7BC><EFBFBD><EFBFBD>
		auto item_result = rst.first();
		this->_start_lla = item_result->_lla_pos;
		this->_start_posture = item_result->_posture_d3;

		exec_result->_success_mark = true;
	}
	else {
		exec_result->_reason_text = "<EFBFBD>޷<EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ʵ<EFBFBD>壨id=%1<><31><EFBFBD><EFBFBD>ȡ%1<><31>Ϣ<EFBFBD><CFA2><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>";
	}
	out << exec_result;
}

void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
	std::shared_ptr<const StrightLineMotion> in, QList<std::shared_ptr<RespondDefault>>& out)
{
	auto exec_result = std::make_shared<RespondDefault>();
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC><CEBB>
	if (_current_cmd) {
		auto exec = std::make_shared<MotionDeduceRequest>();
		exec->reset(in->targetEntity(), in->sourceEntity());
		exec->_target_time = _current_cmd->startTimepoint();
		map->execute(exec);
	}

	// <20><>ȡ<EFBFBD><C8A1>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>״̬
	auto req = std::make_shared<Get3DBox>();
	req->reset(this->_bind_entity.lock()->entityID(), this->_bind_entity.lock()->entityID());
	auto rst = map->execute<Get3DBox, Box3DDesc>(req);
	if (rst.size()) {
		// <20><><EFBFBD>浱ǰ<E6B5B1><C7B0><EFBFBD><EFBFBD>
		this->_current_cmd = in;
		// <20><><EFBFBD>»<EFBFBD>׼<EFBFBD><D7BC><EFBFBD><EFBFBD>
		auto item_result = rst.first();
		this->_start_lla = item_result->_lla_pos;
		this->_start_posture = item_result->_posture_d3;

		exec_result->_success_mark = true;
	}
	else {
		exec_result->_reason_text = "<EFBFBD>޷<EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ʵ<EFBFBD>壨id=%1<><31><EFBFBD><EFBFBD>ȡ%1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>";
	}
	out << exec_result;
}

#include <QMatrix4x4>
void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
	std::shared_ptr<const SyncRequest> in, QList<std::shared_ptr<RespondDefault>>& out)
{
	auto reqs = std::make_shared<MotionDeduceRequest>();
	reqs->reset(in->targetEntity(), in->sourceEntity());
	reqs->_target_time = in->_time_current;

	for (auto rst : map->execute(reqs)) {
		auto conv = std::dynamic_pointer_cast<RespondDefault>(rst);
		if (conv) out << conv;
	}
}

#include <cmath>
void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
	std::shared_ptr<const MotionDeduceRequest> in, QList<std::shared_ptr<RespondDefault>>& out)
{
	auto rst = std::make_shared<RespondDefault>();
	rst->reset(in->targetEntity(), in->sourceEntity());
	if (_current_cmd) {
		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>
		auto pos_d3 = _current_cmd->posAtTimespan(in->_target_time - _current_cmd->startTimepoint());
		auto pos_p3 = _current_cmd->speedAtTimespan(in->_target_time - _current_cmd->startTimepoint());

		QMatrix4x4 t;
		t.rotate(-_start_posture._azimuth_deg, QVector3D(0, 0, 1));

		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>
		auto conv_pos = pos_d3 * t;
		auto conv_psr = pos_p3 * t;

		// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
		auto azimuth_target_cos = QVector3D::dotProduct(conv_pos.normalized(), QVector3D(0, 1, 0));
		auto azimuth_target = rad2d(std::acos(azimuth_target_cos));
		if (QVector3D::crossProduct(conv_pos.normalized(), QVector3D(0, 1, 0)).z() < 0)
			azimuth_target = 360 - azimuth_target;
		LonLatAlt deduce_lla;
		StandardGlobe::getTargetLLAWithDistance(_start_lla, conv_pos.length(), azimuth_target, deduce_lla);

		// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1>λ<EFBFBD><CEBB>
		auto azimuth_self_cos = QVector3D::dotProduct(conv_psr.normalized(), QVector3D(0, 1, 0));
		auto azimuth_self = rad2d(std::acos(azimuth_self_cos));
		if (QVector3D::crossProduct(conv_psr.normalized(), QVector3D(0, 1, 0)).z() < 0)
			azimuth_self = 360 - azimuth_self;

		auto setpos = std::make_shared<Set3DBoxLLAPos>();
		setpos->_lla_pos = deduce_lla;
		map->execute(setpos);

		auto setpsr = std::make_shared<Set3DBoxPosture>();
		setpsr->_posture_d3._azimuth_deg = azimuth_self;
		map->execute(setpsr);

		rst->_success_mark = true;
	}
	else {
		rst->_reason_text = QString("ʵ<EFBFBD><EFBFBD>%1δ<31><CEB4><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD>Ӧ<EFBFBD><D3A6>%2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>")
			.arg(this->_bind_entity.lock()->entityID()).arg(in->topicString());
	}

	out << rst;
}

std::shared_ptr<WsComponent> SurfaceMotionControlPlugin::defaultNew() const
{
	return std::make_shared<SurfaceMotionControlPlugin>();
}

void SurfaceMotionControlPlugin::bindEntity(std::weak_ptr<WsEntity> host)
{
	this->_bind_entity = host;
}

QString SurfaceMotionControlPlugin::name() const
{
	return NAME(SurfaceMotionControlPlugin);
}