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 <limits>
  10
  11
  12 namespace space {
  13
  14 Autopilot::Autopilot(Ship &ctrl, const Vector<float> &target)
  15 : ctrl(&ctrl)
  16 , target(&target) {
  17
  18 }
  19
  20
  21 void Autopilot::Update(float deltaT) {
  22         // reset
  23         ctrl->linThrottle = 0;
  24         ctrl->rotThrottle = 0;
  25
  26         // cache
  27         dt = deltaT;
  28         dp = *target - ctrl->pos;
  29         dist = Length(dp);
  30         normDP =  dp / dist;
  31         speed = Length(ctrl->vel);
  32         normVel = ctrl->vel / speed;
  33         proj = ctrl->pos + (normVel * (DistanceToHalt() * 2));
  34
  35         if (Distance() < 0.75) {
  36                 if (DistanceToHalt() < Distance() + (speed * dt)) {
  37                         if (StandingStill()) {
  38                                 Face(-normDP);
  39                         } else {
  40                                 Face(-normVel);
  41                         }
  42                 } else {
  43                         Halt();
  44                 }
  45                 return;
  46         }
  47
  48         if (StandingStill()) {
  49                 FaceTarget();
  50                 AccelerateAlong(normDP);
  51                 return;
  52         }
  53
  54         const Vector<float> projDiff = *target - proj;
  55         const float projDist = Length(projDiff);
  56         const Vector<float> normProj = projDiff / projDist;
  57         const float onProj = Dot(normProj, normVel);
  58
  59         if (projDist <= 0.75 || (onProj < -0.9 && !StandingStill())) {
  60                 Halt();
  61                 return;
  62         }
  63
  64         Face(normProj);
  65         AccelerateAlong(normProj);
  66         return;
  67
  68         if (OnTarget()) {
  69                 if (FacingTarget()) {
  70                         if (FarAway()) {
  71                                 ctrl->linThrottle = 1;
  72                         } else if (ReallyClose() && Speed() <= ctrl->MaxFwdAcc()) {
  73                                 ctrl->linThrottle = Speed() / ctrl->MaxRevAcc() * -1;
  74                         } else if (InBrakingDistance()) {
  75                                 ctrl->linThrottle = -1;
  76                         } else {
  77                                 ctrl->linThrottle = 1;
  78                         }
  79                 } else if (FacingOpposite()) {
  80                         if (FarAway()) {
  81                                 FaceTarget();
  82                         } else if (ReallyClose() && Speed() <= ctrl->MaxFwdAcc()) {
  83                                 ctrl->linThrottle = Speed() / ctrl->MaxFwdAcc();
  84                         } else if (InBrakingDistance()) {
  85                                 ctrl->linThrottle = 1;
  86                         } else {
  87                                 ctrl->linThrottle = -1;
  88                         }
  89                 } else {
  90                         FaceTarget();
  91                 }
  92                 return;
  93         }
  94
  95         const Vector<float> correction =
  96                 *target - ctrl->pos - (normVel * Distance());
  97
  98         const Vector<float> normCorrection = Norm(correction);
  99         float faceCorrection = Dot(normCorrection, ctrl->Dir());
 100
 101         Face(normCorrection);
 102         ctrl->linThrottle = faceCorrection * faceCorrection;
 103 }
 104
 105 bool Autopilot::StandingStill() const {
 106         return ctrl->vel.x * dt < std::numeric_limits<float>::epsilon()
 107                 && ctrl->vel.y * dt < std::numeric_limits<float>::epsilon();
 108 }
 109
 110 bool Autopilot::ReallySlow() const {
 111         return Speed() < ctrl->MaxFwdAcc() * dt;
 112 }
 113
 114 bool Autopilot::FacingTarget() const {
 115         return Facing(normDP);
 116 }
 117
 118 bool Autopilot::FacingOpposite() const {
 119         return Facing(-normDP);
 120 }
 121
 122 bool Autopilot::Facing(Vector<float> v) const {
 123         return std::abs(Dot(ctrl->Dir(), v) - 1)
 124                 < std::numeric_limits<float>::epsilon();
 125 }
 126
 127 bool Autopilot::OnTarget() const {
 128         return std::abs(Dot(Norm(ctrl->vel), Norm(*target - ctrl->pos)) - 1)
 129                 < std::numeric_limits<float>::epsilon();
 130 }
 131
 132
 133 float Autopilot::Speed() const {
 134         return speed;
 135 }
 136 float Autopilot::Distance() const {
 137         return dist;
 138 }
 139
 140 float Autopilot::DistanceToHalt() const {
 141         if (StandingStill()) {
 142                 return 0;
 143         }
 144         const float timeToHalt = Speed() / ctrl->MaxFwdAcc();
 145         const float angDiff = std::acos(Dot(normVel, ctrl->Dir()));
 146         const float timeToTurn = std::sqrt(angDiff / ctrl->MaxRotAcc());
 147         return (ctrl->MaxFwdAcc() * timeToHalt * timeToHalt) / 2
 148                 + timeToTurn * Speed();
 149 }
 150
 151 bool Autopilot::ReallyClose() const {
 152         return Distance() <= ctrl->MaxFwdAcc();
 153 }
 154
 155 bool Autopilot::InBrakingDistance() const {
 156         return Distance() <= DistanceToHalt();
 157 }
 158
 159 bool Autopilot::FarAway() const {
 160         return Distance() > 1.5 * DistanceToHalt();
 161 }
 162
 163
 164 float Autopilot::TargetVelAngle() const {
 165         return std::atan2(normDP.y, normDP.x) - std::atan2(normVel.y, normVel.x);
 166 }
 167
 168
 169 void Autopilot::FaceTarget() {
 170         Face(Norm(*target - ctrl->pos));
 171 }
 172
 173 void Autopilot::Face(Vector<float> tgt) {
 174         const Vector<float> dir = ctrl->Dir();
 175
 176         float angle = std::atan2(tgt.y, tgt.x) - std::atan2(dir.y, dir.x);
 177         if (angle > PI) angle -= PI2;
 178         if (angle < -PI) angle += PI2;
 179         const float absAngle = std::abs(angle);
 180         const int sigAngle = sigma(angle);
 181
 182         const float absRotVel = std::abs(ctrl->rotVel);
 183         const int sigRotVel = sigma(ctrl->rotVel);
 184         const float rotMaxAcc = ctrl->MaxRotAcc();
 185         const float rotTTH = absRotVel / rotMaxAcc;
 186         const float rotDTH = (rotMaxAcc * rotTTH * rotTTH) / 2;
 187
 188         // pos rot = clockwise
 189         // neg rot = counter clockwise
 190
 191         if (sigAngle == 0) {
 192                 if (absRotVel > 0) {
 193                         if (absRotVel <= rotMaxAcc) {
 194                                 ctrl->rotThrottle = -sigRotVel * absRotVel / rotMaxAcc;
 195                         } else {
 196                                 ctrl->rotThrottle = -sigRotVel;
 197                         }
 198                 } else {
 199                         // done
 200                         ctrl->rotThrottle = 0;
 201                 }
 202         } else if (sigAngle == sigRotVel) {
 203                 // turning in the right direction
 204                 if (rotDTH >= absAngle) {
 205                         // overshooting
 206                         if (absRotVel <= rotMaxAcc) {
 207                                 ctrl->rotThrottle = -sigAngle * absRotVel / rotMaxAcc;
 208                         } else {
 209                                 ctrl->rotThrottle = -sigAngle;
 210                         }
 211                 } else {
 212                         ctrl->rotThrottle = sigAngle;
 213                 }
 214         } else {
 215                 // turning in the wrong direction
 216                 ctrl->rotThrottle = sigAngle;
 217         }
 218 }
 219
 220 void Autopilot::Halt() {
 221         if (StandingStill()) {
 222                 Face(normDP);
 223                 return;
 224         }
 225
 226         Vector<float> nnVel = Norm(-normVel);
 227         Face(nnVel);
 228         if (ReallySlow()) {
 229                 if (Facing(nnVel)) {
 230                         ctrl->linThrottle = Speed() / ctrl->MaxFwdAcc();
 231                 } else if (Facing(-nnVel)) {
 232                         ctrl->linThrottle = -Speed() / ctrl->MaxFwdAcc();
 233                 }
 234         } else {
 235                 AccelerateAlong(nnVel);
 236         }
 237 }
 238
 239 void Autopilot::AccelerateAlong(Vector<float> v) {
 240         const float pointing = Dot(v, ctrl->Dir());
 241         if (std::abs(pointing) > 0.7071) {
 242                 ctrl->linThrottle = pointing * pointing * sigma(pointing);
 243         } else {
 244                 ctrl->linThrottle = 0;
 245         }
 246 }
 247
 248
 249 void Autopilot::Render(Canvas &canv, const Camera &cam) const {
 250         constexpr Color tgtColor(0xFF, 0x00, 0x00);
 251         constexpr Color velColor(0x00, 0x00, 0xFF);
 252         constexpr Color planColor(0x00, 0xFF, 0x00);
 253
 254         Vector<float> screenPos(cam.ToScreen(ctrl->pos));
 255         Vector<float> screenTgt(cam.ToScreen(*target));
 256         Vector<float> screenProj(cam.ToScreen(proj));
 257
 258         canv.SetColor(tgtColor);
 259         canv.Arrow(screenPos, screenTgt);
 260
 261         canv.SetColor(velColor);
 262         canv.Arrow(screenPos, screenProj);
 263
 264         canv.SetColor(planColor);
 265         canv.Arrow(screenProj, screenTgt);
 266 }
 267
 268 }