From e4732374394610bdd3deafd2138cbea267a670e0 Mon Sep 17 00:00:00 2001
From: Daniel Karbach <daniel.karbach@localhorst.tv>
Date: Sat, 18 Jan 2014 14:11:23 +0100
Subject: [PATCH] some lousy world collision, friction, and drag

---
 src/app/Application.cpp |  1 +
 src/graphics/Vector.h   | 31 +++++++++++++
 src/gworm.cpp           | 21 +++++++--
 src/world/World.cpp     | 97 ++++++++++++++++++++++++++++++++++++++++-
 src/world/World.h       |  5 +++
 5 files changed, 150 insertions(+), 5 deletions(-)

diff --git a/src/app/Application.cpp b/src/app/Application.cpp
index d798b37..b7983aa 100644
--- a/src/app/Application.cpp
+++ b/src/app/Application.cpp
@@ -220,6 +220,7 @@ void Application::RenderUI() {
 		canvas.Arrow(screenPos, screenPos + Vector<int>(e.vel * 10.0f));
 
 		cout << "entity: pos " << e.pos << ", vel " << e.vel << ", acc " << e.acc << endl;
+		cout << "        vel mag: " << Length(e.vel) << ", acc mag: " << Length(e.acc) << endl;
 	}
 }
 
diff --git a/src/graphics/Vector.h b/src/graphics/Vector.h
index d35cf33..6565cc1 100644
--- a/src/graphics/Vector.h
+++ b/src/graphics/Vector.h
@@ -3,6 +3,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <limits>
 #include <ostream>
 #include <SDL.h>
 
@@ -36,6 +37,16 @@ public:
 		y -= other.y;
 		return *this;
 	}
+	Vector<Scalar> &operator *=(Scalar factor) {
+		x *= factor;
+		y *= factor;
+		return *this;
+	}
+	Vector<Scalar> &operator /=(Scalar factor) {
+		x /= factor;
+		y /= factor;
+		return *this;
+	}
 
 	SDL_Point ToPoint() const {
 		SDL_Point p;
@@ -142,6 +153,13 @@ constexpr bool operator !=(Vector<Scalar> lhs, Vector<Scalar> rhs) {
 }
 
 
+template<class Scalar>
+constexpr bool IsZero(Vector<Scalar> v) {
+	return std::abs(v.x) < std::numeric_limits<Scalar>::epsilon()
+		&& std::abs(v.y) < std::numeric_limits<Scalar>::epsilon();
+}
+
+
 template<class Scalar>
 constexpr Scalar Cross2D(Vector<Scalar> lhs, Vector<Scalar> rhs) {
 	return (lhs.x * rhs.y) - (lhs.y * rhs.x);
@@ -171,6 +189,19 @@ template<class Scalar>
 constexpr Vector<Scalar> Rotate270(Vector<Scalar> v) {
 	return Vector<Scalar>(v.y, -v.x);
 }
+template<class Scalar, class Float>
+inline Vector<Scalar> Rotate(Vector<Scalar> v, Float by) {
+	Float sine(std::sin(by));
+	Float cosine(std::cos(by));
+	return Vector<Scalar>(v.x * cosine - v.y * sine, v.x * sine + v.y * cosine);
+}
+
+/// reflect v along normalized n
+template<class Scalar>
+inline Vector<Scalar> Reflect(Vector<Scalar> v, Vector<Scalar> n) {
+	const Scalar dd = Scalar(2) * Dot(v, n);
+	return Vector<Scalar>(v.x - (dd * n.x), v.y - (dd * n.y));
+}
 
 
 template<class Scalar>
diff --git a/src/gworm.cpp b/src/gworm.cpp
index efd5757..3f22ade 100644
--- a/src/gworm.cpp
+++ b/src/gworm.cpp
@@ -1,11 +1,15 @@
 #include "app/Application.h"
 #include "app/SDL.h"
+#include "graphics/const.h"
 #include "graphics/Canvas.h"
 #include "graphics/Window.h"
 #include "world/Entity.h"
 #include "world/World.h"
 
+#include <iostream>
+
 using namespace gworm;
+using namespace std;
 
 
 namespace {
@@ -48,10 +52,19 @@ int main(int argc, const char *argv[]) {
 	World world(Vector<int>(500, 500));
 	make_planet(world, Vector<int>(250, 250), 220);
 
-	Entity e;
-	e.vel = Vector<float>(-19, 19);
-	e.mass = 1;
-	world.AddEntity(e);
+	Entity orb;
+	orb.vel = Vector<float>(-19, 19);
+	orb.mass = 1;
+	world.AddEntity(orb);
+
+	Entity coll;
+	coll.pos = Vector<float>(250, -25);
+	coll.vel = Vector<float>(-10, 0);
+	coll.mass = 2;
+	world.AddEntity(coll);
+
+	cout << "normal at top: " << world.NormalAt(Vector<float>(250, 30)) << endl;
+	cout << "normal at left: " << world.NormalAt(Vector<float>(30, 250)) << endl;
 
 	Application app(canv, world);
 	app.Run();
diff --git a/src/world/World.cpp b/src/world/World.cpp
index 53115dc..dcac95e 100644
--- a/src/world/World.cpp
+++ b/src/world/World.cpp
@@ -15,12 +15,107 @@ World::World(Vector<int> size)
 
 
 void World::Update(float dt) {
+	constexpr float fricDyn = 0.75f;
+	constexpr float fricStat = 2.5f;
+	constexpr float dragFact = 0.05f;
+
 	for (Entity &e : entities) {
-		e.acc = ForceAt(e.pos, e.mass) / e.mass;
+		const Vector<float> gravity = ForceAt(e.pos, e.mass);
+		const Vector<float> drag = Rotate180(e.vel) * dragFact;
+		// TODO: drag should be inverse square. and not onmipresent
+
+		e.acc = (gravity + drag) / e.mass;
 		e.Update(dt);
+
+		Vector<float> at;
+		if (WorldCollision(e, at)) {
+			float speed = Length(e.vel);
+			Vector<float> normVel = e.vel / speed;
+			float steps = 0;
+			while (MassAt(e.pos) > 0) {
+				e.pos -= normVel;
+				steps += 1.0f;
+			}
+			const Vector<float> normal = NormalAt(e.pos + normVel);
+			e.vel = Reflect(e.vel, normal);
+			speed = Length(e.vel);
+			normVel = e.vel / speed;
+			// some kind of friction, depends on angle to normal
+			speed *= (fricDyn * (std::abs(Dot(normVel, normal))));
+			if (speed > fricStat) {
+				speed -= fricStat;
+				e.vel = normVel * speed;
+				e.pos += normVel * steps;
+			} else {
+				e.vel = Vector<float>(0, 0);
+			}
+		}
+	}
+}
+
+
+bool World::WorldCollision(const Entity &e, Vector<float> &at) const {
+	if (InBounds(e.pos) && MassAt(e.pos) > 0) {
+		at = e.pos;
+		return true;
+	} else {
+		return false;
 	}
 }
 
+Vector<float> World::NormalAt(Vector<float> pos) const {
+	constexpr Vector<float> check[] = {
+		{  0,        -2        },
+		{  0.765367, -1.84776  },
+		{  1.41421,  -1.41421  },
+		{  1.84776,  -0.765367 },
+		{  2,         0        },
+		{  1.84776,   0.765367 },
+		{  1.41421,   1.41421  },
+		{  0.765367,  1.84776  },
+		{  0,         2        },
+		{ -0.765367,  1.84776  },
+		{ -1.41421,   1.41421  },
+		{ -1.84776,   0.765367 },
+		{ -2,         0        },
+		{ -1.84776,  -0.765367 },
+		{ -1.41421,  -1.41421  },
+		{ -0.765367, -1.84776  },
+	};
+	Vector<float> normal;
+	for (auto v : check) {
+		if (MassAt(pos + v) == 0) {
+			normal += v;
+		}
+	}
+	return Norm(normal);
+}
+
+//Vector<float> World::NormalAt(Vector<float> pos) const {
+//	Vector<int> begin(pos - Vector<float>(3, 3));
+//	Vector<int> end(pos + Vector<float>(3, 3));
+//
+//	Vector<float> normal;
+//	for (Vector<int> cur(begin); cur.y < end.y; ++cur.y) {
+//		for (cur.x = begin.x; cur.x < end.x; ++cur.x) {
+//			if (IsSurface(cur) > 0) {
+//				const Vector<float> dir = pos - Vector<float>(cur);
+//				normal += Rotate90(dir);
+//			}
+//		}
+//	}
+//
+//	return Norm(normal);
+//}
+
+bool World::IsSurface(Vector<int> pos) const {
+	return MassAt(pos) > 0 && (
+			MassAt(pos - Vector<int>(1, 0)) == 0 ||
+			MassAt(pos + Vector<int>(1, 0)) == 0 ||
+			MassAt(pos - Vector<int>(0, 1)) == 0 ||
+			MassAt(pos + Vector<int>(0, 1)) == 0);
+}
+
 
 Entity &World::AddEntity(const Entity &e) {
 	entities.emplace_back(e);
diff --git a/src/world/World.h b/src/world/World.h
index f15d43b..8a00f60 100644
--- a/src/world/World.h
+++ b/src/world/World.h
@@ -22,6 +22,8 @@ public:
 public:
 	void Update(float dt);
 
+	bool InBounds(Vector<int> pos) const
+		{ return pos.x > 0 && pos.y > 0 && pos.x < size.x && pos.y < size.y; }
 	int Index(Vector<int> pos) const { return pos.x * size.y + pos.y; }
 
 	float MassAt(Vector<int> pos) const { return masses[Index(pos)]; }
@@ -33,6 +35,9 @@ public:
 	Entity &AddEntity(const Entity &);
 
 	Vector<float> ForceAt(Vector<float>, float m) const;
+	bool WorldCollision(const Entity &, Vector<float> &) const;
+	Vector<float> NormalAt(Vector<float>) const;
+	bool IsSurface(Vector<int>) const;
 
 private:
 	Vector<int> size;
-- 
2.39.2