From: Daniel Karbach <daniel.karbach@localhorst.tv>
Date: Thu, 26 Dec 2013 13:20:46 +0000 (+0100)
Subject: added autopilot that sucks
X-Git-Url: http://git.localhorst.tv/?p=space.git;a=commitdiff_plain;h=ffd31714f3edb64ebe16b65878750c6cc5c7e884

added autopilot that sucks
---

diff --git a/src/ai/Autopilot.cpp b/src/ai/Autopilot.cpp
new file mode 100644
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;
+}
+
+}
diff --git a/src/ai/Autopilot.h b/src/ai/Autopilot.h
new file mode 100644
index 0000000..b15448e
--- /dev/null
+++ b/src/ai/Autopilot.h
@@ -0,0 +1,35 @@
+#ifndef SPACE_AUTOPILOT_H_
+#define SPACE_AUTOPILOT_H_
+
+#include "../graphics/Vector.h"
+
+
+namespace space {
+
+class Camera;
+class Canvas;
+class Ship;
+
+class Autopilot {
+
+public:
+	Autopilot(Ship &ctrl, const Vector<float> &target);
+
+public:
+	void Update(float deltaT);
+
+	void Render(Canvas &, const Camera &) const;
+
+private:
+	Ship *ctrl;
+	const Vector<float> *target;
+
+	// cache members for debug drawing
+	Vector<float> planFrom;
+	Vector<float> planTo;
+
+};
+
+}
+
+#endif
diff --git a/src/app/Application.cpp b/src/app/Application.cpp
index 5820c57..e3c6a2b 100644
--- a/src/app/Application.cpp
+++ b/src/app/Application.cpp
@@ -11,9 +11,13 @@ Application::Application(Canvas &c)
 , univ(Vector<int>(10, 10), Vector<int>(10, 10), Vector<int>(10, 10))
 , focus(Vector<float>(500, 500), 500)
 , cam(c.Size(), focus.Pos())
+, controlled(univ.AddShip(Ship()))
+, autopilot(*controlled, focus.Pos())
+, apEnabled(false)
 , last(SDL_GetTicks())
-, running(false) {
-	controlled = univ.AddShip(Ship());
+, running(false)
+, paused(false) {
+
 }
 
 
@@ -30,7 +34,9 @@ void Application::Run() {
 
 void Application::Loop(int delta) {
 	HandleEvents();
-	Update(delta);
+	if (!paused) {
+		Update(delta);
+	}
 	Render();
 	canvas.Present();
 }
@@ -97,6 +103,12 @@ void Application::OnKeyDown(const SDL_KeyboardEvent &e) {
 		case SDLK_x:
 			cam.StartShrink();
 			break;
+		case SDLK_c:
+			apEnabled = !apEnabled;
+			break;
+		case SDLK_p:
+			paused = !paused;
+			break;
 		default:
 			break;
 	}
@@ -142,8 +154,12 @@ void Application::OnKeyUp(const SDL_KeyboardEvent &e) {
 
 void Application::Update(int dt) {
 	const float delta = dt / 1e3;
-	controlled->rotThrottle = control.x;
-	controlled->linThrottle = -control.y;
+	if (apEnabled) {
+		autopilot.Update(delta);
+	} else {
+		controlled->rotThrottle = control.x;
+		controlled->linThrottle = -control.y;
+	}
 	cam.Update(delta);
 	univ.Update(delta);
 	focus.SetSpeed(500 / cam.Zoom());
@@ -175,13 +191,15 @@ void Application::Render() {
 	canvas.SetColor(entityColor);
 	for (const Ship &s : univ.Ships()) {
 		const Vector<float> direction = s.Dir();
-		const Vector<int> position = cam.ToScreen(Vector<float>(s.area * univ.areaSize) + s.pos);
+		const Vector<int> position = cam.ToScreen(s.pos);
 		const Vector<int> nose = position + Vector<int>(direction * 15.0f);
 		const Vector<int> left = position + Vector<int>((Rotate90(direction) * 8.0f) - (direction * 4.0f));
 		const Vector<int> right = position + Vector<int>((Rotate270(direction) * 8.0f) - (direction * 4.0f));
 		canvas.Line(position, nose);
 		canvas.Quad(nose, left, position, right);
 	}
+
+	autopilot.Render(canvas, cam);
 }
 
 }
diff --git a/src/app/Application.h b/src/app/Application.h
index 7e3d91b..4d2dd2d 100644
--- a/src/app/Application.h
+++ b/src/app/Application.h
@@ -1,6 +1,7 @@
 #ifndef SPACE_APPLICATION_H_
 #define SPACE_APPLICATION_H_
 
+#include "../ai/Autopilot.h"
 #include "../graphics/Camera.h"
 #include "../graphics/Moveable.h"
 #include "../graphics/Vector.h"
@@ -44,8 +45,12 @@ private:
 	Ship *controlled;
 	Vector<int> control;
 
+	Autopilot autopilot;
+	bool apEnabled;;
+
 	Uint32 last;
 	bool running;
+	bool paused;
 
 };
 
diff --git a/src/entity/Ship.h b/src/entity/Ship.h
index a21f4b1..a373302 100644
--- a/src/entity/Ship.h
+++ b/src/entity/Ship.h
@@ -1,6 +1,7 @@
 #ifndef SPACE_SHIP_H_
 #define SPACE_SHIP_H_
 
+#include "../graphics/const.h"
 #include "../graphics/Vector.h"
 
 #include <cmath>
@@ -19,22 +20,31 @@ public:
 	float revForce = 1;
 	float rotForce = 1;
 
-	Vector<int> area;
-
 	Vector<float> pos;
 	Vector<float> vel;
 	Vector<float> Dir() const {
 		return Vector<float>::FromPolar(1, orient);
 	}
+	float MaxFwdAcc() const {
+		return linForce / mass;
+	}
+	float MaxRevAcc() const {
+		return revForce / mass;
+	}
+	float MaxLinAcc() const {
+		return (linThrottle < 0 ? MaxRevAcc() : MaxFwdAcc());
+	}
 	Vector<float> Acc() const {
-		float force = (linThrottle < 0 ? revForce : linForce);
-		return Dir() * force / mass * linThrottle;
+		return Dir() * MaxLinAcc() * linThrottle;
 	}
 
 	float orient = 0;
 	float rotVel = 0;
+	float MaxRotAcc() const {
+		return rotForce / mass;
+	}
 	float RotAcc() const {
-		return rotForce / mass * rotThrottle;
+		return MaxRotAcc() * rotThrottle;
 	}
 
 	float linThrottle = 0;
@@ -44,6 +54,8 @@ public:
 	void Update(float delta) {
 		rotVel += RotAcc() * delta;
 		orient += rotVel * delta;
+		while (orient < 0) orient += PI2;
+		while (orient > PI2) orient -= PI2;
 		vel += Acc() * delta;
 		pos += vel * delta;
 	}
diff --git a/src/graphics/Canvas.cpp b/src/graphics/Canvas.cpp
index 93e3f21..6241ab3 100644
--- a/src/graphics/Canvas.cpp
+++ b/src/graphics/Canvas.cpp
@@ -95,6 +95,15 @@ void Canvas::Cross(Vector<int> pos, int extent) {
 		Vector<int>(pos.x, pos.y + extent));
 }
 
+void Canvas::Arrow(Vector<int> from, Vector<int> to) {
+	Line(from, to);
+	Vector<float> delta(to - from);
+	delta = delta / Length(delta);
+
+	Line(to, to + Vector<int>(Rotate90(delta) * 5.0f - (delta * 5.0f)));
+	Line(to, to + Vector<int>(Rotate270(delta) * 5.0f - (delta * 5.0f)));
+}
+
 void Canvas::Triangle(Vector<int> v1, Vector<int> v2, Vector<int> v3) {
 	SDL_Point points[4] = { v1, v2, v3, v1 };
 	SDL_RenderDrawLines(canv, points, 4);
diff --git a/src/graphics/Canvas.h b/src/graphics/Canvas.h
index c9a6488..042ac43 100644
--- a/src/graphics/Canvas.h
+++ b/src/graphics/Canvas.h
@@ -38,6 +38,7 @@ public:
 
 	void Dot(Vector<int> pos);
 	void Cross(Vector<int> pos, int extent);
+	void Arrow(Vector<int> from, Vector<int> to);
 	void Triangle(Vector<int>, Vector<int>, Vector<int>);
 	void Quad(Vector<int>, Vector<int>, Vector<int>, Vector<int>);
 
diff --git a/src/graphics/Vector.h b/src/graphics/Vector.h
index 992ba1e..4cf920b 100644
--- a/src/graphics/Vector.h
+++ b/src/graphics/Vector.h
@@ -142,6 +142,10 @@ constexpr bool operator !=(Vector<Scalar> lhs, Vector<Scalar> rhs) {
 }
 
 
+template<class Scalar>
+constexpr Scalar Cross2D(Vector<Scalar> lhs, Vector<Scalar> rhs) {
+	return (lhs.x * rhs.y) - (lhs.y * rhs.x);
+}
 template<class Scalar>
 constexpr Scalar Dot(Vector<Scalar> lhs, Vector<Scalar> rhs) {
 	return (lhs.x * rhs.x) + (lhs.y * rhs.y);
diff --git a/src/graphics/const.h b/src/graphics/const.h
new file mode 100644
index 0000000..64175da
--- /dev/null
+++ b/src/graphics/const.h
@@ -0,0 +1,17 @@
+#ifndef SPACE_CONST_H_
+#define SPACE_CONST_H_
+
+namespace space {
+
+// this is so very darn long so it can easily be changed to double
+constexpr float PI = 3.141592653589793238462643383279502884;
+constexpr float PI2 = 2 * PI;
+
+template<class T>
+constexpr int sigma(T v) {
+	return v > 0 ? 1 : (v < 0 ? -1 : 0);
+}
+
+}
+
+#endif
diff --git a/src/world/Universe.cpp b/src/world/Universe.cpp
index 6f2e5c1..c5bb915 100644
--- a/src/world/Universe.cpp
+++ b/src/world/Universe.cpp
@@ -27,22 +27,6 @@ Ship *Universe::AddShip(const Ship &s) {
 void Universe::Update(float delta) {
 	for (Ship &s : ships) {
 		s.Update(delta);
-		while (s.pos.x > areaSize.x) {
-			s.pos.x -= areaSize.x;
-			++s.area.x;
-		}
-		while (s.pos.x < 0) {
-			s.pos.x += areaSize.x;
-			--s.area.x;
-		}
-		while (s.pos.y > areaSize.y) {
-			s.pos.y -= areaSize.y;
-			++s.area.y;
-		}
-		while (s.pos.y < 0) {
-			s.pos.y += areaSize.y;
-			--s.area.y;
-		}
 	}
 }