+#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;
+}
+
+}