src/ai/Autopilot.cpp

   1 #include "Autopilot.h"
   2
   3 #include "../entity/Ship.h"
   4 #include "../graphics/const.h"
   5 #include "../graphics/Camera.h"
   6 #include "../graphics/Canvas.h"
   7 #include "../graphics/Color.h"
   8
   9 #include <iostream>
  10 using namespace std;
  11
  12
  13 namespace space {
  14
  15 Autopilot::Autopilot(Ship &ctrl, const Vector<float> &target)
  16 : ctrl(&ctrl)
  17 , target(&target) {
  18
  19 }
  20
  21
  22 void Autopilot::Update(float deltaT) {
  23         const Vector<float> deltaP(*target - ctrl->pos);
  24         const float distance = Length(deltaP);
  25         const Vector<float> deltaPnorm(deltaP / distance);
  26
  27         const Vector<float> dir = ctrl->Dir();
  28
  29         const float faceTarget = Dot(deltaPnorm, dir);
  30         const float maxAcc = faceTarget < 0
  31                 ? ctrl->MaxFwdAcc()
  32                 : ctrl->MaxRevAcc();
  33
  34         const float speed = Length(ctrl->vel);
  35         const Vector<float> velNorm = ctrl->vel / speed;
  36         const float onTarget = Dot(deltaPnorm, velNorm);
  37
  38         const float linTTH = speed / maxAcc;
  39         const float linDTH = (maxAcc * linTTH * linTTH) / 2;
  40
  41         planFrom = Vector<float>(ctrl->pos);
  42         planTo = Vector<float>(*target);
  43         if (speed > 0 && onTarget < 1) {
  44                 planFrom += velNorm * linDTH;
  45                 planTo -= velNorm * linDTH;
  46         }
  47         const Vector<float> plan(planTo - planFrom);
  48         const float planLen = Length(plan);
  49         const Vector<float> planNorm(plan / planLen);
  50
  51         float facePlan = Dot(planNorm, dir);
  52         ctrl->linThrottle = facePlan * facePlan;
  53
  54         const float absRotVel = std::abs(ctrl->rotVel);
  55         const int sigRotVel = sigma(ctrl->rotVel);
  56         const float rotMaxAcc = ctrl->MaxRotAcc();
  57         const float rotTTH = absRotVel / rotMaxAcc;
  58         const float rotDTH = (rotMaxAcc * rotTTH * rotTTH) / 2;
  59
  60         float planAngle =
  61                 std::atan2(planNorm.y, planNorm.x) - std::atan2(dir.y, dir.x);
  62         if (planAngle > PI) planAngle -= PI2;
  63         if (planAngle < -PI) planAngle += PI2;
  64         const float absPlanAngle = std::acos(facePlan);
  65         const int sigPlanAngle = sigma(planAngle);
  66
  67         // pos rot = clockwise
  68         // neg rot = counter clockwise
  69
  70         if (sigPlanAngle == 0) {
  71                 // pointing straight at the plan
  72         } else if (sigPlanAngle == sigRotVel) {
  73                 // turning in the right direction
  74                 if (rotDTH >= absPlanAngle) {
  75                         // overshooting
  76                         ctrl->rotThrottle = -sigPlanAngle;
  77                 } else {
  78                         ctrl->rotThrottle = sigPlanAngle;
  79                 }
  80         } else {
  81                 // turning in the wrong direction
  82                 ctrl->rotThrottle = sigPlanAngle;
  83         }
  84 }
  85
  86
  87 void Autopilot::Render(Canvas &canv, const Camera &cam) const {
  88         constexpr Color tgtColor(0xFF, 0x00, 0x00);
  89         constexpr Color velColor(0x00, 0x00, 0xFF);
  90         constexpr Color planColor(0x00, 0xFF, 0x00);
  91
  92         Vector<float> screenPos(cam.ToScreen(ctrl->pos));
  93         Vector<float> screenTgt(cam.ToScreen(*target));
  94         Vector<float> screenPlanFrom(cam.ToScreen(planFrom));
  95         Vector<float> screenPlanTo(cam.ToScreen(planTo));
  96
  97         canv.SetColor(tgtColor);
  98         canv.Arrow(screenPos, screenTgt);
  99
 100         canv.SetColor(velColor);
 101         canv.Arrow(screenPos, screenPlanFrom);
 102         canv.Arrow(screenTgt, screenPlanTo);
 103
 104         canv.SetColor(planColor);
 105         canv.Arrow(screenPlanFrom, screenPlanTo);
 106
 107         cout << "max acc: " << ctrl->MaxFwdAcc() << ", distance: " << Length(*target - ctrl->pos) << endl;
 108 }
 109
 110 }