added autopilot that sucks

[space.git] / src / ai / Autopilot.cpp

diff --git a/src/ai/Autopilot.cpp b/src/ai/Autopilot.cpp
new file mode 100644 (file)
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;
+}
+
+}