From: Daniel Karbach <daniel.karbach@localhorst.tv>
Date: Mon, 30 Dec 2013 13:48:11 +0000 (+0100)
Subject: less sucky autopilot
X-Git-Url: https://git.localhorst.tv/?a=commitdiff_plain;h=501ffe20da16eaab69e668871001f735697c4a42;p=space.git

less sucky autopilot
---

diff --git a/src/ai/Autopilot.cpp b/src/ai/Autopilot.cpp
index d23d319..0cc281b 100644
--- 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 b15448e..1a814e6 100644
--- 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 e3c6a2b..b69cc85 100644
--- 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 4d2dd2d..17fdce8 100644
--- 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 4cf920b..97dfd4a 100644
--- 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) {