less sucky autopilot

diff --git a/src/ai/Autopilot.cpp b/src/ai/Autopilot.cpp
index d23d319f085f2504788383ed210721a144d8faa9..0cc281bc674c2d9fbc9734c61c84b9091290df82 100644 (file)
--- a/src/ai/Autopilot.cpp
+++ b/src/ai/Autopilot.cpp
@@ -6,8 +6,7 @@
 #include "../graphics/Canvas.h"
 #include "../graphics/Color.h"
 
-#include <iostream>
-using namespace std;
+#include <limits>
 
 
 namespace space {
@@ -20,36 +19,165 @@ Autopilot::Autopilot(Ship &ctrl, const Vector<float> &target)
 
 
 void Autopilot::Update(float deltaT) {
-       const Vector<float> deltaP(*target - ctrl->pos);
-       const float distance = Length(deltaP);
-       const Vector<float> deltaPnorm(deltaP / distance);
+       // reset
+       ctrl->linThrottle = 0;
+       ctrl->rotThrottle = 0;
 
-       const Vector<float> dir = ctrl->Dir();
+       // cache
+       dt = deltaT;
+       dp = *target - ctrl->pos;
+       dist = Length(dp);
+       normDP =  dp / dist;
+       speed = Length(ctrl->vel);
+       normVel = ctrl->vel / speed;
+       proj = ctrl->pos + (normVel * (DistanceToHalt() * 2));
+
+       if (Distance() < 0.75) {
+               if (DistanceToHalt() < Distance() + (speed * dt)) {
+                       if (StandingStill()) {
+                               Face(-normDP);
+                       } else {
+                               Face(-normVel);
+                       }
+               } else {
+                       Halt();
+               }
+               return;
+       }
+
+       if (StandingStill()) {
+               FaceTarget();
+               AccelerateAlong(normDP);
+               return;
+       }
+
+       const Vector<float> projDiff = *target - proj;
+       const float projDist = Length(projDiff);
+       const Vector<float> normProj = projDiff / projDist;
+       const float onProj = Dot(normProj, normVel);
 
-       const float faceTarget = Dot(deltaPnorm, dir);
-       const float maxAcc = faceTarget < 0
-               ? ctrl->MaxFwdAcc()
-               : ctrl->MaxRevAcc();
+       if (projDist <= 0.75 || (onProj < -0.9 && !StandingStill())) {
+               Halt();
+               return;
+       }
+
+       Face(normProj);
+       AccelerateAlong(normProj);
+       return;
+
+       if (OnTarget()) {
+               if (FacingTarget()) {
+                       if (FarAway()) {
+                               ctrl->linThrottle = 1;
+                       } else if (ReallyClose() && Speed() <= ctrl->MaxFwdAcc()) {
+                               ctrl->linThrottle = Speed() / ctrl->MaxRevAcc() * -1;
+                       } else if (InBrakingDistance()) {
+                               ctrl->linThrottle = -1;
+                       } else {
+                               ctrl->linThrottle = 1;
+                       }
+               } else if (FacingOpposite()) {
+                       if (FarAway()) {
+                               FaceTarget();
+                       } else if (ReallyClose() && Speed() <= ctrl->MaxFwdAcc()) {
+                               ctrl->linThrottle = Speed() / ctrl->MaxFwdAcc();
+                       } else if (InBrakingDistance()) {
+                               ctrl->linThrottle = 1;
+                       } else {
+                               ctrl->linThrottle = -1;
+                       }
+               } else {
+                       FaceTarget();
+               }
+               return;
+       }
 
-       const float speed = Length(ctrl->vel);
-       const Vector<float> velNorm = ctrl->vel / speed;
-       const float onTarget = Dot(deltaPnorm, velNorm);
+       const Vector<float> correction =
+               *target - ctrl->pos - (normVel * Distance());
 
-       const float linTTH = speed / maxAcc;
-       const float linDTH = (maxAcc * linTTH * linTTH) / 2;
+       const Vector<float> normCorrection = Norm(correction);
+       float faceCorrection = Dot(normCorrection, ctrl->Dir());
+
+       Face(normCorrection);
+       ctrl->linThrottle = faceCorrection * faceCorrection;
+}
+
+bool Autopilot::StandingStill() const {
+       return ctrl->vel.x * dt < std::numeric_limits<float>::epsilon()
+               && ctrl->vel.y * dt < std::numeric_limits<float>::epsilon();
+}
+
+bool Autopilot::ReallySlow() const {
+       return Speed() < ctrl->MaxFwdAcc() * dt;
+}
+
+bool Autopilot::FacingTarget() const {
+       return Facing(normDP);
+}
+
+bool Autopilot::FacingOpposite() const {
+       return Facing(-normDP);
+}
+
+bool Autopilot::Facing(Vector<float> v) const {
+       return std::abs(Dot(ctrl->Dir(), v) - 1)
+               < std::numeric_limits<float>::epsilon();
+}
+
+bool Autopilot::OnTarget() const {
+       return std::abs(Dot(Norm(ctrl->vel), Norm(*target - ctrl->pos)) - 1)
+               < std::numeric_limits<float>::epsilon();
+}
+
+
+float Autopilot::Speed() const {
+       return speed;
+}
+float Autopilot::Distance() const {
+       return dist;
+}
 
-       planFrom = Vector<float>(ctrl->pos);
-       planTo = Vector<float>(*target);
-       if (speed > 0 && onTarget < 1) {
-               planFrom += velNorm * linDTH;
-               planTo -= velNorm * linDTH;
+float Autopilot::DistanceToHalt() const {
+       if (StandingStill()) {
+               return 0;
        }
-       const Vector<float> plan(planTo - planFrom);
-       const float planLen = Length(plan);
-       const Vector<float> planNorm(plan / planLen);
+       const float timeToHalt = Speed() / ctrl->MaxFwdAcc();
+       const float angDiff = std::acos(Dot(normVel, ctrl->Dir()));
+       const float timeToTurn = std::sqrt(angDiff / ctrl->MaxRotAcc());
+       return (ctrl->MaxFwdAcc() * timeToHalt * timeToHalt) / 2
+               + timeToTurn * Speed();
+}
+
+bool Autopilot::ReallyClose() const {
+       return Distance() <= ctrl->MaxFwdAcc();
+}
+
+bool Autopilot::InBrakingDistance() const {
+       return Distance() <= DistanceToHalt();
+}
+
+bool Autopilot::FarAway() const {
+       return Distance() > 1.5 * DistanceToHalt();
+}
+
+
+float Autopilot::TargetVelAngle() const {
+       return std::atan2(normDP.y, normDP.x) - std::atan2(normVel.y, normVel.x);
+}
+
+
+void Autopilot::FaceTarget() {
+       Face(Norm(*target - ctrl->pos));
+}
+
+void Autopilot::Face(Vector<float> tgt) {
+       const Vector<float> dir = ctrl->Dir();
 
-       float facePlan = Dot(planNorm, dir);
-       ctrl->linThrottle = facePlan * facePlan;
+       float angle = std::atan2(tgt.y, tgt.x) - std::atan2(dir.y, dir.x);
+       if (angle > PI) angle -= PI2;
+       if (angle < -PI) angle += PI2;
+       const float absAngle = std::abs(angle);
+       const int sigAngle = sigma(angle);
 
        const float absRotVel = std::abs(ctrl->rotVel);
        const int sigRotVel = sigma(ctrl->rotVel);
@@ -57,29 +185,63 @@ void Autopilot::Update(float deltaT) {
        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) {
+       if (sigAngle == 0) {
+               if (absRotVel > 0) {
+                       if (absRotVel <= rotMaxAcc) {
+                               ctrl->rotThrottle = -sigRotVel * absRotVel / rotMaxAcc;
+                       } else {
+                               ctrl->rotThrottle = -sigRotVel;
+                       }
+               } else {
+                       // done
+                       ctrl->rotThrottle = 0;
+               }
+       } else if (sigAngle == sigRotVel) {
                // turning in the right direction
-               if (rotDTH >= absPlanAngle) {
+               if (rotDTH >= absAngle) {
                        // overshooting
-                       ctrl->rotThrottle = -sigPlanAngle;
+                       if (absRotVel <= rotMaxAcc) {
+                               ctrl->rotThrottle = -sigAngle * absRotVel / rotMaxAcc;
+                       } else {
+                               ctrl->rotThrottle = -sigAngle;
+                       }
                } else {
-                       ctrl->rotThrottle = sigPlanAngle;
+                       ctrl->rotThrottle = sigAngle;
                }
        } else {
                // turning in the wrong direction
-               ctrl->rotThrottle = sigPlanAngle;
+               ctrl->rotThrottle = sigAngle;
+       }
+}
+
+void Autopilot::Halt() {
+       if (StandingStill()) {
+               Face(normDP);
+               return;
+       }
+
+       Vector<float> nnVel = Norm(-normVel);
+       Face(nnVel);
+       if (ReallySlow()) {
+               if (Facing(nnVel)) {
+                       ctrl->linThrottle = Speed() / ctrl->MaxFwdAcc();
+               } else if (Facing(-nnVel)) {
+                       ctrl->linThrottle = -Speed() / ctrl->MaxFwdAcc();
+               }
+       } else {
+               AccelerateAlong(nnVel);
+       }
+}
+
+void Autopilot::AccelerateAlong(Vector<float> v) {
+       const float pointing = Dot(v, ctrl->Dir());
+       if (std::abs(pointing) > 0.7071) {
+               ctrl->linThrottle = pointing * pointing * sigma(pointing);
+       } else {
+               ctrl->linThrottle = 0;
        }
 }
 
@@ -91,20 +253,16 @@ void Autopilot::Render(Canvas &canv, const Camera &cam) const {
 
        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));
+       Vector<float> screenProj(cam.ToScreen(proj));
 
        canv.SetColor(tgtColor);
        canv.Arrow(screenPos, screenTgt);
 
        canv.SetColor(velColor);
-       canv.Arrow(screenPos, screenPlanFrom);
-       canv.Arrow(screenTgt, screenPlanTo);
+       canv.Arrow(screenPos, screenProj);
 
        canv.SetColor(planColor);
-       canv.Arrow(screenPlanFrom, screenPlanTo);
-
-       cout << "max acc: " << ctrl->MaxFwdAcc() << ", distance: " << Length(*target - ctrl->pos) << endl;
+       canv.Arrow(screenProj, screenTgt);
 }
 
 }

diff --git a/src/ai/Autopilot.h b/src/ai/Autopilot.h
index b15448ea3a117be6469cd533b38be7c96c6275e0..1a814e6c6aaf01de9283746e7db8fe82137d61f3 100644 (file)
--- a/src/ai/Autopilot.h
+++ b/src/ai/Autopilot.h
@@ -20,13 +20,54 @@ public:
 
        void Render(Canvas &, const Camera &) const;
 
+private:
+       // velocity is so small that the next integration makes no difference
+       bool StandingStill() const;
+       // can halt within the current frame
+       bool ReallySlow() const;
+       // can halt within one second
+       bool Slow() const;
+
+       // pointing exactly at target
+       bool FacingTarget() const;
+       // pointing exactly away from target
+       bool FacingOpposite() const;
+       // ship pointing in direction of given normalized vector
+       bool Facing(Vector<float>) const;
+       bool OnTarget() const;
+
+       float Speed() const;
+       float Distance() const;
+       float DistanceToHalt() const;
+       bool ReallyClose() const;
+       bool InBrakingDistance() const;
+       bool FarAway() const;
+
+       float TargetVelAngle() const;
+
+       // point the ship towards the target
+       void FaceTarget();
+       // point ship into direction of given normalized vector
+       void Face(Vector<float>);
+       // stop the ship asap
+       void Halt();
+       // accelerate if it improves moving in given normalized direction
+       void AccelerateAlong(Vector<float>);
+
 private:
        Ship *ctrl;
        const Vector<float> *target;
 
-       // cache members for debug drawing
-       Vector<float> planFrom;
-       Vector<float> planTo;
+       float dt;
+
+       Vector<float> dp;
+       float dist;
+       Vector<float> normDP;
+
+       float speed;
+       Vector<float> normVel;
+
+       Vector<float> proj;
 
 };
 

diff --git a/src/app/Application.cpp b/src/app/Application.cpp
index e3c6a2b878bc1bb39c4294bd632192d8ebca40df..b69cc85a0cdb18578a3eee5e87472fe0196fc485 100644 (file)
--- a/src/app/Application.cpp
+++ b/src/app/Application.cpp
@@ -12,6 +12,20 @@ Application::Application(Canvas &c)
 , focus(Vector<float>(500, 500), 500)
 , cam(c.Size(), focus.Pos())
 , controlled(univ.AddShip(Ship()))
+, linGauge(
+       Vector<int>(15, 100),
+       Vector<int>(10, 10),
+       Color(0xFF, 0xFF, 0xFF),
+       Color(0x00, 0x00, 0x00),
+       Color(0x00, 0xFF, 0x00),
+       Color(0xFF, 0x00, 0x00))
+, rotGauge(
+       Vector<int>(15, 100),
+       Vector<int>(27, 10),
+       Color(0xFF, 0xFF, 0xFF),
+       Color(0x00, 0x00, 0x00),
+       Color(0x33, 0x33, 0xFF),
+       Color(0x00, 0x00, 0xFF))
 , autopilot(*controlled, focus.Pos())
 , apEnabled(false)
 , last(SDL_GetTicks())
@@ -34,6 +48,11 @@ void Application::Run() {
 
 void Application::Loop(int delta) {
        HandleEvents();
+       if (delta == 0) {
+               SDL_Delay(1);
+               return;
+       }
+
        if (!paused) {
                Update(delta);
        }
@@ -200,6 +219,9 @@ void Application::Render() {
        }
 
        autopilot.Render(canvas, cam);
+
+       linGauge.Render(canvas, controlled->linThrottle);
+       rotGauge.Render(canvas, controlled->rotThrottle);
 }
 
 }

diff --git a/src/app/Application.h b/src/app/Application.h
index 4d2dd2dbf38f6545320d7e92cf7c85e4d40306f0..17fdce8c84b77f7074477563dd67bf9eef203e8d 100644 (file)
--- a/src/app/Application.h
+++ b/src/app/Application.h
@@ -5,6 +5,7 @@
 #include "../graphics/Camera.h"
 #include "../graphics/Moveable.h"
 #include "../graphics/Vector.h"
+#include "../ui/DualGauge.h"
 #include "../world/Universe.h"
 
 #include <SDL.h>
@@ -45,6 +46,9 @@ private:
        Ship *controlled;
        Vector<int> control;
 
+       DualGauge linGauge;
+       DualGauge rotGauge;
+
        Autopilot autopilot;
        bool apEnabled;;
 

diff --git a/src/graphics/Vector.h b/src/graphics/Vector.h
index 4cf920bd70b8a6eaeafb8f9f75ab04ce1570ace7..97dfd4ac70662c4a22eada8982bb116fbc8f89b3 100644 (file)
--- a/src/graphics/Vector.h
+++ b/src/graphics/Vector.h
@@ -154,6 +154,10 @@ template<class Scalar>
 constexpr Scalar Length(Vector<Scalar> v) {
        return std::sqrt(Dot(v, v));
 }
+template<class Scalar>
+constexpr Vector<Scalar> Norm(Vector<Scalar> v) {
+       return v / Length(v);
+}
 
 template<class Scalar>
 constexpr Vector<Scalar> Rotate90(Vector<Scalar> v) {