X-Git-Url: http://git.localhorst.tv/?p=space.git;a=blobdiff_plain;f=src%2Fai%2FAutopilot.cpp;fp=src%2Fai%2FAutopilot.cpp;h=d23d319f085f2504788383ed210721a144d8faa9;hp=0000000000000000000000000000000000000000;hb=ffd31714f3edb64ebe16b65878750c6cc5c7e884;hpb=699437a474de8b87ccb6749d44adf740e680d620

diff --git a/src/ai/Autopilot.cpp b/src/ai/Autopilot.cpp
new file mode 100644
index 0000000..d23d319
--- /dev/null
+++ b/src/ai/Autopilot.cpp
@@ -0,0 +1,110 @@
+#include "Autopilot.h"
+
+#include "../entity/Ship.h"
+#include "../graphics/const.h"
+#include "../graphics/Camera.h"
+#include "../graphics/Canvas.h"
+#include "../graphics/Color.h"
+
+#include <iostream>
+using namespace std;
+
+
+namespace space {
+
+Autopilot::Autopilot(Ship &ctrl, const Vector<float> &target)
+: ctrl(&ctrl)
+, target(&target) {
+
+}
+
+
+void Autopilot::Update(float deltaT) {
+	const Vector<float> deltaP(*target - ctrl->pos);
+	const float distance = Length(deltaP);
+	const Vector<float> deltaPnorm(deltaP / distance);
+
+	const Vector<float> dir = ctrl->Dir();
+
+	const float faceTarget = Dot(deltaPnorm, dir);
+	const float maxAcc = faceTarget < 0
+		? ctrl->MaxFwdAcc()
+		: ctrl->MaxRevAcc();
+
+	const float speed = Length(ctrl->vel);
+	const Vector<float> velNorm = ctrl->vel / speed;
+	const float onTarget = Dot(deltaPnorm, velNorm);
+
+	const float linTTH = speed / maxAcc;
+	const float linDTH = (maxAcc * linTTH * linTTH) / 2;
+
+	planFrom = Vector<float>(ctrl->pos);
+	planTo = Vector<float>(*target);
+	if (speed > 0 && onTarget < 1) {
+		planFrom += velNorm * linDTH;
+		planTo -= velNorm * linDTH;
+	}
+	const Vector<float> plan(planTo - planFrom);
+	const float planLen = Length(plan);
+	const Vector<float> planNorm(plan / planLen);
+
+	float facePlan = Dot(planNorm, dir);
+	ctrl->linThrottle = facePlan * facePlan;
+
+	const float absRotVel = std::abs(ctrl->rotVel);
+	const int sigRotVel = sigma(ctrl->rotVel);
+	const float rotMaxAcc = ctrl->MaxRotAcc();
+	const float rotTTH = absRotVel / rotMaxAcc;
+	const float rotDTH = (rotMaxAcc * rotTTH * rotTTH) / 2;
+
+	float planAngle =
+		std::atan2(planNorm.y, planNorm.x) - std::atan2(dir.y, dir.x);
+	if (planAngle > PI) planAngle -= PI2;
+	if (planAngle < -PI) planAngle += PI2;
+	const float absPlanAngle = std::acos(facePlan);
+	const int sigPlanAngle = sigma(planAngle);
+
+	// pos rot = clockwise
+	// neg rot = counter clockwise
+
+	if (sigPlanAngle == 0) {
+		// pointing straight at the plan
+	} else if (sigPlanAngle == sigRotVel) {
+		// turning in the right direction
+		if (rotDTH >= absPlanAngle) {
+			// overshooting
+			ctrl->rotThrottle = -sigPlanAngle;
+		} else {
+			ctrl->rotThrottle = sigPlanAngle;
+		}
+	} else {
+		// turning in the wrong direction
+		ctrl->rotThrottle = sigPlanAngle;
+	}
+}
+
+
+void Autopilot::Render(Canvas &canv, const Camera &cam) const {
+	constexpr Color tgtColor(0xFF, 0x00, 0x00);
+	constexpr Color velColor(0x00, 0x00, 0xFF);
+	constexpr Color planColor(0x00, 0xFF, 0x00);
+
+	Vector<float> screenPos(cam.ToScreen(ctrl->pos));
+	Vector<float> screenTgt(cam.ToScreen(*target));
+	Vector<float> screenPlanFrom(cam.ToScreen(planFrom));
+	Vector<float> screenPlanTo(cam.ToScreen(planTo));
+
+	canv.SetColor(tgtColor);
+	canv.Arrow(screenPos, screenTgt);
+
+	canv.SetColor(velColor);
+	canv.Arrow(screenPos, screenPlanFrom);
+	canv.Arrow(screenTgt, screenPlanTo);
+
+	canv.SetColor(planColor);
+	canv.Arrow(screenPlanFrom, screenPlanTo);
+
+	cout << "max acc: " << ctrl->MaxFwdAcc() << ", distance: " << Length(*target - ctrl->pos) << endl;
+}
+
+}