epa working

Makefile

@@ -9,7 +9,7 @@ BIN_DIR = bin
 LIB_DIR = lib
 
 
-CPPFLAGS = -O3 -Iinclude -Isrc/imgui -g -D ASSET_DIR=\"$(ASSET_PREFIX)\"
+CPPFLAGS = -O3 -std=c++20 -Iinclude -Isrc/imgui -g -D ASSET_DIR=\"$(ASSET_PREFIX)\"
 CFLAGS = -O3 -g -Iinclude -D ASSET_DIR=\"$(ASSET_PREFIX)\"
 LDFLAGS = -O3 -g
 #global LDLIBS

compile_commands.json

@@ -3,6 +3,7 @@
     "arguments": [
       "/sbin/g++",
       "-O3",
+      "-std=c++20",
       "-Iinclude",
       "-Isrc/imgui",
       "-g",
@@ -10,64 +11,11 @@
       "ASSET_DIR=\"assets/\"",
       "-c",
       "-o",
-      "obj/rlImGui/rlImGui.o",
+      "obj/phys.o",
-      "src/rlImGui/rlImGui.cpp"
+      "src/phys.cpp"
     ],
     "directory": "/home/inventorx/projects/shooter",
-    "file": "/home/inventorx/projects/shooter/src/rlImGui/rlImGui.cpp",
+    "file": "/home/inventorx/projects/shooter/src/phys.cpp",
-    "output": "/home/inventorx/projects/shooter/obj/rlImGui/rlImGui.o"
+    "output": "/home/inventorx/projects/shooter/obj/phys.o"
-  },
-  {
-    "arguments": [
-      "/sbin/g++",
-      "-O3",
-      "-Iinclude",
-      "-Isrc/imgui",
-      "-g",
-      "-D",
-      "ASSET_DIR=\"assets/\"",
-      "-c",
-      "-o",
-      "obj/main.o",
-      "src/main.cpp"
-    ],
-    "directory": "/home/inventorx/projects/shooter",
-    "file": "/home/inventorx/projects/shooter/src/main.cpp",
-    "output": "/home/inventorx/projects/shooter/obj/main.o"
-  },
-  {
-    "arguments": [
-      "/sbin/gcc",
-      "-O3",
-      "-g",
-      "-Iinclude",
-      "-D",
-      "ASSET_DIR=\"assets/\"",
-      "-c",
-      "-o",
-      "obj/testc.o",
-      "src/testc.c"
-    ],
-    "directory": "/home/inventorx/projects/shooter",
-    "file": "/home/inventorx/projects/shooter/src/testc.c",
-    "output": "/home/inventorx/projects/shooter/obj/testc.o"
-  },
-  {
-    "arguments": [
-      "/sbin/g++",
-      "-O3",
-      "-Iinclude",
-      "-Isrc/imgui",
-      "-g",
-      "-D",
-      "ASSET_DIR=\"assets/\"",
-      "-c",
-      "-o",
-      "obj/testcpp.o",
-      "src/testcpp.cpp"
-    ],
-    "directory": "/home/inventorx/projects/shooter",
-    "file": "/home/inventorx/projects/shooter/src/testcpp.cpp",
-    "output": "/home/inventorx/projects/shooter/obj/testcpp.o"
   }
 ]

src/main.cpp

@@ -106,6 +106,8 @@ std::ostream &operator<<(std::ostream &os, const Vector2 &obj) {
     return os;
 }
 
+
+
 struct Player {
     raylib::Camera3D cam;
 
@@ -127,16 +129,20 @@ struct Player {
 		rl::Vector3 up = (Vector3) {0, 1, 0};
         rl::Vector3 left = Vector3RotateByAxisAngle(forward, up, M_PI_2);
         if (IsKeyDown(KEY_W)) {
-            accel += forward * 5;
+            //accel += forward * 5;
+            accel += rl::Vector3(forward.x, 0, forward.z).Normalize() * 5;
         }
         if (IsKeyDown(KEY_S)) {
-            accel += -forward * 5;
+            //accel += -forward * 5;
+            accel += -rl::Vector3(forward.x, 0, forward.z).Normalize() * 5;
         }
         if (IsKeyDown(KEY_A)) {
-            accel += left * 5;
+            //accel += left * 5;
+            accel += rl::Vector3(left.x, 0, left.z) * 5;
         }
         if (IsKeyDown(KEY_D)) {
-            accel += -left * 5;
+            //accel += -left * 5;
+            accel += -rl::Vector3(left.x, 0, left.z) * 5;
         }
 
 		if(IsKeyDown(KEY_SPACE)) {
@@ -221,7 +227,7 @@ int main(int argc, char *argv[]) {
 	user.pos = rl::Vector3 {0, 0, 200};
 
 	RecPrism peak(Vector3Zero(), (Vector3) {50, 50, 50} );
-	RecPrism peakduo((Vector3) {60, 0, 0} , (Vector3) {50, 50, 50} );
+	RecPrism peakduo((Vector3) {45, 0, 0} , (Vector3) {50, 50, 50} );
     while (!window.ShouldClose()) // Detect window close button or ESC key, or a quit from the menu
     {
         if (IsWindowResized()) {
@@ -259,15 +265,33 @@ int main(int argc, char *argv[]) {
 
 		}
 
+		if(IsKeyDown(KEY_L)) {
+			peak.pos.x += 0.1;
+		}
+		if(IsKeyDown(KEY_J)) {
+			peak.pos.x -= 0.1;
+		}
+		if(IsKeyDown(KEY_I)) {
+			peak.pos.z -= 0.1;
+		}
+
+		if(IsKeyDown(KEY_K)) {
+			peak.pos.z += 0.1;
+		}
 
         outputTexture.BeginMode();
         ClearBackground(BLACK);
         user.cam.BeginMode();
         DrawSphere((Vector3){0, 0, -300}, 20, RED);
 
-		if (GJK(peak, peakduo)) {
+		
+		auto opt = GJK(peak, peakduo);
+		if (opt) {
 			peakduo.Draw(RED);
 			peak.Draw(RED);
+			auto vec = EPA(opt.value(), peak, peakduo);
+			//DrawRay(Ray{peakduo.pos, vec}, BLUE);
+			peakduo.pos += vec;
 		} else {
 			peakduo.Draw();
 			peak.Draw();

src/phys.cpp

@@ -1,13 +1,16 @@
 #include "phys.hpp"
-#include "raylib-cpp/include/Camera3D.hpp"
 #include "raylib-cpp/include/Model.hpp"
 #include "raylib-cpp/include/Vector3.hpp"
 #include <array>
 #include <cassert>
+#include <cerrno>
 #include <cfloat>
+#include <compare>
 #include <initializer_list>
 #include <iostream>
+#include <optional>
 #include <ostream>
+#include <queue>
 #include <raylib.h>
 #include <raymath.h>
 #include <vector>
@@ -70,7 +73,7 @@ rl::Vector3 RecPrism::FindFurthestPoint(rl::Vector3 dir) const {
 	vectors[7] = (Vector3) {-size.x / 2, size.y / 2, -size.z / 2};
 
 	for (size_t i = 0; i < 8; i++) {
-	 	vectors[i] = Vector3RotateByQuaternion(vectors[i], rotation);
+		vectors[i] = Vector3RotateByQuaternion(vectors[i], rotation);
 	}
 
 	rl::Vector3 maxpoint;
@@ -83,7 +86,6 @@ rl::Vector3 RecPrism::FindFurthestPoint(rl::Vector3 dir) const {
 		}
 	}
 
-	
 	return pos + maxpoint;
 }
 
@@ -123,7 +125,7 @@ bool NextSimplex(Simplex& points, rl::Vector3& direction) {
 		case 2:
 			return Line(points, direction);
 		case 3:
-			return Triangle(points, direction);
+			return TriCase(points, direction);
 		case 4:
 			return Tetrahedron(points, direction);
 	}
@@ -131,7 +133,7 @@ bool NextSimplex(Simplex& points, rl::Vector3& direction) {
 	return false;
 }
 
-bool SameDirection(rl::Vector3 dir, rl::Vector3& otherdir) {
+bool SameDirection(rl::Vector3 dir, rl::Vector3 otherdir) {
 	return dir.DotProduct(otherdir) > 0;
 }
 
@@ -155,7 +157,7 @@ bool Line(Simplex& points, rl::Vector3& dir) {
 	return false;
 }
 
-bool Triangle(Simplex& points, rl::Vector3& dir) {
+bool TriCase(Simplex& points, rl::Vector3& dir) {
 	rl::Vector3 a = points[0];
 	rl::Vector3 b = points[1];
 	rl::Vector3 c = points[2];
@@ -198,33 +200,37 @@ bool Tetrahedron(Simplex& points, rl::Vector3& dir) {
 	auto ab = b - a;
 	auto ac = c - a;
 	auto ad = d - a;
-	auto ao = - a;
+	auto ao = -a;
 
 	auto abc = ab.CrossProduct(ac);
 	auto acd = ac.CrossProduct(ad);
 	auto adb = ad.CrossProduct(ab);
 
 	if (SameDirection(abc, ao)) {
-		return Triangle(points = {a, b, c}, dir);
+		return TriCase(points = {a, b, c}, dir);
 	}
 
 	if (SameDirection(acd, ao)) {
-		return Triangle(points = {a, c, d}, dir);
+		return TriCase(points = {a, c, d}, dir);
 	}
 
-	if(SameDirection(adb, ao)) {
+	if (SameDirection(adb, ao)) {
-		return Triangle(points = {a, d, b}, dir);
+		return TriCase(points = {a, d, b}, dir);
 	}
 	return true;
-	
 }
 
-bool GJK(const Collider& a, const Collider& b) {
+std::optional<Simplex> GJK(const Collider& a, const Collider& b) {
 	rl::Vector3 support = Support(a, b, rl::Vector3(1, 0, 0));
 
 	if (support == rl::Vector3(0, 0, 0)) {
 		std::cout << "h" << std::endl;
-		return true;
+		Simplex degen;
+		degen.pushf(rl::Vector3(0, 0, 0));
+		degen.pushf(rl::Vector3(0, 0, 0));
+		degen.pushf(rl::Vector3(0, 0, 0));
+		degen.pushf(rl::Vector3(0, 0, 0));
+		return std::optional(degen);
 	}
 	Simplex points;
 	points.pushf(support);
@@ -236,17 +242,192 @@ bool GJK(const Collider& a, const Collider& b) {
 		support = Support(a, b, dir);
 
 		if (support.DotProduct(dir) <= 0) {
-			std::cout << "here!!!" << its << std::endl;
+			// std::cout << "here!!!" << its << std::endl;
-			// std::cout << support.x << support.y << support.z << std::endl;
+			//  std::cout << support.x << support.y << support.z << std::endl;
-			return false; // if this point is not in the direction of the origin, then no colision
+			return std::optional<Simplex>(); // if this point is not in the direction of the origin, then no
+			                                 // colision
 		}
 
 		points.pushf(support);
 
-
 		if (NextSimplex(points, dir)) {
-			return true;
+			return points;
 		}
 		its = its + 1;
 	}
 }
+
+Triangle::Triangle(rl::Vector3 x, rl::Vector3 y, rl::Vector3 z) {
+	a = x;
+	b = y;
+	c = z;
+}
+
+rl::Vector3& Triangle::operator[](size_t i) {
+	switch (i) {
+		case 0:
+			return a;
+		case 1:
+			return b;
+		case 2:
+			return c;
+	}
+	assert(false);
+	return a;
+}
+
+rl::Vector3 Triangle::Normal() {
+	auto p1 = b - a;
+	auto p2 = c - a;
+
+	auto normal = p1.CrossProduct(p2);
+	normal = normal.Normalize();
+	if (SameDirection(normal, -a)) {
+		return -normal;
+	}
+	return normal;
+}
+
+std::array<Edge, 3> Triangle::Edges() {
+	std::array<Edge, 3> out;
+	out[0] = Edge(a,b);
+	out[1] = Edge(b,c);
+	out[2] = Edge(c,a);
+	return out;
+}
+std::ostream& operator<<(std::ostream& out, const rl::Vector3& p) {
+	out << p.x << "," << p.y << "," << p.z;
+	return out;
+}
+
+std::ostream& operator<<(std::ostream& out, const Triangle& p) {
+	out << p.a << ";" << p.b << ";" << p.c;
+	return out;
+}
+
+bool operator<(const rl::Vector3& a, const rl::Vector3& b) {
+	if (a.x != b.x) return a.x < b.x;
+	if (a.y != b.y) return a.y < b.y;
+	return a.z < b.z;
+}
+
+Edge::Edge(rl::Vector3 a, rl::Vector3 b) {
+	if (a < b) {
+		x = a;
+		y = b;
+	} else {
+		x = b;
+		y = a;
+	}
+}
+
+Edge::Edge() {
+	x = 0;
+	y = 0;
+}
+
+bool Edge::operator<(const Edge& b) const { //TODO: better
+ 	if(x != b.x) return x < b.x;
+	return y < b.y;
+}
+
+bool Triangle::operator<(const Triangle& b) const { //TODO: better
+	if(this->a != b.a) return this->a < b.a;
+	if(this->b != b.b) return this->b < b.b;
+	return this->c < b.c;
+}
+
+float Pointtoplane(Triangle t, rl::Vector3 p){
+	//std::cout << "calcing dist between" << p << " and " << t << " norm: " << t.Normal() << std::endl;
+	//return abs((p-t.a).Project(t.Normal()).Length());
+	return fabs((p-t.a).Project(t.Normal()).Length());
+}
+
+Polytope::Polytope(Simplex S) {
+	faces.insert(Triangle(S[0],S[1],S[2]));
+	faces.insert(Triangle(S[0],S[3],S[1]));
+	faces.insert(Triangle(S[0],S[2],S[3]));
+	faces.insert(Triangle(S[1],S[3],S[2]));
+}
+
+void Polytope::Extend(rl::Vector3 p) {
+	std::set<Edge> edges;
+
+
+
+	std::queue<Triangle> rem;
+	for (auto face : faces) {
+		if (SameDirection(face.Normal(), p)) {
+			rem.push(face);
+			for (auto edge : face.Edges()) {
+				if(edges.contains(edge)) {
+					edges.erase(edge);
+					
+				} else {
+					edges.insert(edge);
+				}
+			}
+		}
+	}
+
+	while (!rem.empty()) {
+		faces.erase(rem.front());
+		rem.pop();
+	}
+
+	int i = 0;
+	for (auto edge : edges) {
+		auto adding = Triangle(edge.x, edge.y, p);
+		faces.insert(adding);
+		std::cout << "adding: " << adding << "count: " << i << std::endl;
+		i++;
+	}
+}
+
+
+
+rl::Vector3 EPA(Simplex S, const Collider& a, const Collider& b) {
+	Polytope p(S);
+
+	std::cout << "BEGIN" << std::endl;
+
+	while (true) {
+		// Find closest face
+		float dist = FLT_MAX;
+		Triangle nearest(0,0,0);
+
+		for (auto face : p.faces) {
+			auto temp = Pointtoplane(face, 0);
+			std::cout << "dist: " << temp << " " << face << std::endl;
+			if ( temp < dist) {
+				dist = temp;
+				nearest = face;
+				//DrawTriangle3D(nearest.a, nearest.b, nearest.c, WHITE);
+			}
+		}
+
+
+
+		auto normal = nearest.Normal();
+		//DrawLine3D(rl::Vector3(0), normal*1000, GREEN);
+		auto supportPoint = Support(a, b, normal);
+
+		//std::cout << "dist to farthest hull" << supportPoint.Dotproduct(normal) - dist << std::endl;
+		if (abs(supportPoint.DotProduct(normal) - dist) < 0.1) { // if distance between face and point on face is same
+			//return supportPoint.Project(normal);
+			//DrawTriangle3D(nearest.a, nearest.b, nearest.c, GREEN);
+			std::cout << "END" << std::endl;
+			return normal * Pointtoplane(nearest, 0);
+		} else {
+			//extend
+			std::cout << "extending" << std::endl;
+			p.Extend(supportPoint);
+		}
+
+	}
+
+}
+
+
+
+

src/phys.hpp

@@ -5,9 +5,14 @@
 #include "raylib-cpp/include/Model.hpp"
 #include "raylib-cpp/include/Vector3.hpp"
 #include "raylib-cpp/include/Vector4.hpp"
+#include <array>
 #include <initializer_list>
+#include <optional>
+#include <ostream>
 #include <raylib-cpp/include/raylib-cpp.hpp>
 #include <raylib.h>
+#include <set>
+#include <unordered_set>
 #include <vector>
 
 namespace rl = raylib;
@@ -56,17 +61,64 @@ struct Simplex {
 	rl::Vector3& operator[](size_t i);
 };
 
+struct Edge {
+	Edge();	
+	Edge(rl::Vector3 a, rl::Vector3 b);
+	rl::Vector3 x;
+	rl::Vector3 y;
+	// bool operator==(const Edge& b);
+	bool operator<(const Edge& b) const;
+};
+
+struct Triangle {
+	Triangle(rl::Vector3 x, rl::Vector3 y, rl::Vector3 z);
+
+	
+	
+	rl::Vector3 a;
+	rl::Vector3 b;
+	rl::Vector3 c;
+
+	rl::Vector3& operator[](size_t i);
+
+	bool operator<(const Triangle& b) const;
+		
+
+	rl::Vector3 Normal(); //Normal away from origin.
+	std::array<Edge, 3> Edges();
+
+	friend std::ostream& operator<<(std::ostream& out, const Triangle& p);
+
+
+};
+
+
+struct Polytope {
+	Polytope(Simplex S);
+	std::set<Triangle> faces;
+
+	void Extend(rl::Vector3 p);
+	
+};
+
+
 
 bool Line(Simplex& points, rl::Vector3& dir);
-bool Triangle(Simplex& points, rl::Vector3& dir);
+bool TriCase(Simplex& points, rl::Vector3& dir);
 bool Tetrahedron(Simplex& points, rl::Vector3& dir);
 
 
 bool NextSimplex(Simplex& points, rl::Vector3& direction);
 
-bool GJK(const Collider& a, const Collider& b);
+std::optional<Simplex> GJK(const Collider& a, const Collider& b);
+
+bool SameDirection(rl::Vector3 dir, rl::Vector3 otherdir);
+
+float Pointtoplane(Triangle t, rl::Vector3 p);
+
+rl::Vector3 EPA(Simplex S, const Collider& a, const Collider& b);
+
 
-bool SameDirection(rl::Vector3 dir, rl::Vector3& otherdir);
 
 #endif