gjk working git add .clang-format src/main.cpp src/phys.cpp src/phys.hpp !

.clang-format

@@ -30,4 +30,5 @@ AllowShortFunctionsOnASingleLine: Empty
 DerivePointerAlignment: false
 ReferenceAlignment: Left
 SpaceAfterCStyleCast: true
+IndentCaseLabels: true
 

src/main.cpp

@@ -1,7 +1,9 @@
+#include "phys.hpp"
 #include "raylib-cpp/include/Camera3D.hpp"
 #include "raylib-cpp/include/Rectangle.hpp"
 #include "raylib-cpp/include/RenderTexture.hpp"
 #include "raylib-cpp/include/Vector3.hpp"
+#include "raylib-cpp/include/Vector4.hpp"
 #include <cmath>
 #include <cstdio>
 #include <iostream>
@@ -110,32 +112,69 @@ struct Player {
     rl::Vector3 accel;
     rl::Vector3 velocity;
     rl::Vector3 pos;
-    rl::Vector3 target;
+
+	rl::Vector3 rotEul;
 
     Player() : cam((Vector3){0, 0, 0}) {
-        target = (Vector3){0, 0, -10};
         //
     }
 
+	rl::Quaternion Rotation() {
+		return QuaternionFromEuler(rotEul.z, rotEul.y, rotEul.x);
+	}
     void KeyControl() {
-        rl::Vector3 forward = (rl::Vector3)target;
+        rl::Vector3 forward = Vector3RotateByQuaternion((Vector3) {0, 0, -1}, Rotation());
-
+		rl::Vector3 up = (Vector3) {0, 1, 0};
-        rl::Vector3 left = Vector3RotateByAxisAngle(forward, cam.GetUp(), M_PI_2);
+        rl::Vector3 left = Vector3RotateByAxisAngle(forward, up, M_PI_2);
         if (IsKeyDown(KEY_W)) {
-            accel += forward;
+            accel += forward * 5;
         }
         if (IsKeyDown(KEY_S)) {
-            accel += -forward;
+            accel += -forward * 5;
         }
         if (IsKeyDown(KEY_A)) {
-            accel += left;
+            accel += left * 5;
         }
         if (IsKeyDown(KEY_D)) {
-            accel += -left;
+            accel += -left * 5;
         }
 
-        target = Vector3RotateByAxisAngle(target, (Vector3){0, 1, 0},
+		if(IsKeyDown(KEY_SPACE)) {
-                                          -GetMouseDelta().x * 0.02 * DEG2RAD);
+			accel += up * 5.0f;
+		}
+
+		if(IsKeyDown(KEY_LEFT_SHIFT)) {
+			accel -= up * 5;
+		}
+
+		if(IsKeyDown(KEY_H)) {
+			velocity = 0;
+		}
+
+
+        //target = Vector3RotateByAxisAngle(target, (Vector3){0, 1, 0}, -GetMouseDelta().x * 0.02 * DEG2RAD);
+			//target = Vector3RotateByAxisAngle(target, (Vector3){1, 0, 0}, GetMouseDelta().y * 0.1 * DEG2RAD);
+
+
+		rotEul.z -= GetMouseDelta().y * 0.02 * DEG2RAD;
+		rotEul.y -= GetMouseDelta().x * 0.02 * DEG2RAD;
+
+
+		// rotation = QuaternionMultiply(rotation, QuaternionFromAxisAngle((Vector3) {1,0,0} , -GetMouseDelta().y * 0.02 * DEG2RAD));
+		// rotation = QuaternionNormalize(rotation);
+        // cam.SetTarget(pos + Vector3RotateByQuaternion((Vector3) {0, 0, -1}, rotation));
+		
+		// forward = Vector3RotateByQuaternion((Vector3) {0, 0, -1}, rotation);
+		// up = Vector3RotateByQuaternion((Vector3) {0, 1, 0}, rotation);
+        // left = Vector3RotateByAxisAngle(forward, cam.GetUp(), M_PI_2);
+
+		// rotation = QuaternionMultiply(rotation, QuaternionFromAxisAngle((Vector3) {0,1,0}, -GetMouseDelta().x * 0.02 * DEG2RAD));
+		// rotation = QuaternionNormalize(rotation);
+        // cam.SetTarget(pos + Vector3RotateByQuaternion((Vector3) {0, 0, -1}, rotation));
+
+		//zero roll
+		
+
         // std::cout << GetMouseDelta() << std::endl;
     }
 
@@ -145,7 +184,7 @@ struct Player {
         velocity += accel * GetFrameTime() * 0.5f;
         accel = Vector3Zero();
         cam.SetPosition(pos);
-        cam.SetTarget(pos + target);
+        cam.SetTarget(pos + Vector3RotateByQuaternion((Vector3) {0, 0, -1}, Rotation()));
     }
 };
 
@@ -179,6 +218,10 @@ int main(int argc, char *argv[]) {
     bool showDemoWindow = true;
 
     Player user;
+	user.pos = rl::Vector3 {0, 0, 200};
+
+	RecPrism peak(Vector3Zero(), (Vector3) {50, 50, 50} );
+	RecPrism peakduo((Vector3) {60, 0, 0} , (Vector3) {50, 50, 50} );
     while (!window.ShouldClose()) // Detect window close button or ESC key, or a quit from the menu
     {
         if (IsWindowResized()) {
@@ -202,10 +245,33 @@ int main(int argc, char *argv[]) {
         user.KeyControl();
         user.UpdatePhysics();
 
+		if (IsKeyDown(KEY_Q)) {
+
+			
+
+
+			//std::cout << "PrevRot: " << peak.rotation.ToString() << std::endl;
+
+		    peak.rotation =  QuaternionMultiply(peak.rotation, QuaternionFromAxisAngle((Vector3) {0, 1, 0}, 0.01));
+			
+			//std::cout << "NewRot: " << n.ToString() << std::endl;
+			
+
+		}
+
+
         outputTexture.BeginMode();
         ClearBackground(BLACK);
         user.cam.BeginMode();
         DrawSphere((Vector3){0, 0, -300}, 20, RED);
+
+		if (GJK(peak, peakduo)) {
+			peakduo.Draw(RED);
+			peak.Draw(RED);
+		} else {
+			peakduo.Draw();
+			peak.Draw();
+		}
         user.cam.EndMode();
 
         outputTexture.EndMode();

src/phys.cpp

@@ -6,6 +6,8 @@
 #include <cassert>
 #include <cfloat>
 #include <initializer_list>
+#include <iostream>
+#include <ostream>
 #include <raylib.h>
 #include <raymath.h>
 #include <vector>
@@ -26,7 +28,7 @@ RecPrism::RecPrism(rl::Vector3 v1, rl::Vector3 v2) : model(GenMeshCube(v2.x, v2.
 
 void RecPrism::Draw(rl::Color color) {
 	model.SetTransform(QuaternionToMatrix(rotation));
-	DrawModel(model, pos, 1.0, BLUE);
+	DrawModel(model, pos, 1.0, color);
 
 	// DrawSphere(pos, 10, GREEN);
 
@@ -48,7 +50,6 @@ void RecPrism::Draw(rl::Color color) {
 	for (auto v : vectors) {
 		DrawSphere(v, 10, RED);
 	}
-	
 }
 
 rl::Vector3 RecPrism::FindFurthestPoint(rl::Vector3 dir) const {
@@ -59,7 +60,6 @@ rl::Vector3 RecPrism::FindFurthestPoint(rl::Vector3 dir) const {
 
 	std::array<rl::Vector3, 8> vectors;
 
-	
 	vectors[0] = (Vector3) {size.x / 2, size.y / 2, size.z / 2};
 	vectors[1] = (Vector3) {-size.x / 2, -size.y / 2, -size.z / 2};
 	vectors[2] = (Vector3) {-size.x / 2, size.y / 2, size.z / 2};
@@ -70,7 +70,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] = pos + Vector3RotateByQuaternion(vectors[i], rotation);
+	 	vectors[i] = Vector3RotateByQuaternion(vectors[i], rotation);
 	}
 
 	rl::Vector3 maxpoint;
@@ -83,7 +83,8 @@ rl::Vector3 RecPrism::FindFurthestPoint(rl::Vector3 dir) const {
 		}
 	}
 
-	return maxpoint;
+	
+	return pos + maxpoint;
 }
 
 rl::Vector3 Support(const Collider& a, const Collider& b, rl::Vector3 dir) {
@@ -92,6 +93,7 @@ rl::Vector3 Support(const Collider& a, const Collider& b, rl::Vector3 dir) {
 
 Simplex& Simplex::operator=(std::initializer_list<rl::Vector3> list) {
 	assert(list.size() <= 4);
+	points.clear();
 	for (auto p : list) {
 		points.push_back(p);
 	}
@@ -100,8 +102,15 @@ Simplex& Simplex::operator=(std::initializer_list<rl::Vector3> list) {
 
 Simplex::Simplex() {}
 
-void Simplex::push(rl::Vector3 point) {
+void Simplex::pushf(rl::Vector3 point) {
-	points.push_back(point);
+	std::vector<rl::Vector3> new_points;
+	new_points.push_back(point);
+
+	for (auto oldp : points) {
+		new_points.push_back(oldp);
+	}
+
+	points = new_points;
 }
 
 rl::Vector3& Simplex::operator[](size_t i) {
@@ -109,21 +118,135 @@ rl::Vector3& Simplex::operator[](size_t i) {
 	return points[i];
 }
 
+bool NextSimplex(Simplex& points, rl::Vector3& direction) {
+	switch (points.points.size()) {
+		case 2:
+			return Line(points, direction);
+		case 3:
+			return Triangle(points, direction);
+		case 4:
+			return Tetrahedron(points, direction);
+	}
+	assert(false);
+	return false;
+}
+
+bool SameDirection(rl::Vector3 dir, rl::Vector3& otherdir) {
+	return dir.DotProduct(otherdir) > 0;
+}
+
+bool Line(Simplex& points, rl::Vector3& dir) {
+	rl::Vector3 a = points[0];
+	rl::Vector3 b = points[1];
+
+	auto ab = b - a;
+	auto ao = -a;
+
+	if (SameDirection(ab, ao)) {
+		dir = ab.CrossProduct(ao).CrossProduct(ab); // set the direction perpandicular from the line in points
+		// and towards the origin. this sets us up for the triangle case.
+	}
+
+	else {
+		points = {a};
+		dir = ao;
+	}
+
+	return false;
+}
+
+bool Triangle(Simplex& points, rl::Vector3& dir) {
+	rl::Vector3 a = points[0];
+	rl::Vector3 b = points[1];
+	rl::Vector3 c = points[2];
+
+	auto ab = b - a;
+	auto ac = c - a;
+	auto ao = -a;
+
+	auto abc = ab.CrossProduct(ac); // vector pointing out of a
+
+	if (SameDirection(abc.CrossProduct(ac), ao)) {
+		if (SameDirection(ac, ao)) {
+			points = {a, c};
+			dir = ac.CrossProduct(ao).CrossProduct(ac);
+		} else {
+			return Line(points = {a, b}, dir);
+		}
+	} else {
+		if (SameDirection(ab.CrossProduct(abc), ao)) {
+			return Line(points = {a, b}, dir);
+		} else { // above or below tri
+			if (SameDirection(abc, ao)) {
+				dir = abc;
+			} else {
+				points = {a, c, b};
+				dir = -abc;
+			}
+		}
+	}
+
+	return false;
+}
+
+bool Tetrahedron(Simplex& points, rl::Vector3& dir) {
+	rl::Vector3 a = points[0];
+	rl::Vector3 b = points[1];
+	rl::Vector3 c = points[2];
+	rl::Vector3 d = points[3];
+
+	auto ab = b - a;
+	auto ac = c - a;
+	auto ad = d - 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);
+	}
+
+	if (SameDirection(acd, ao)) {
+		return Triangle(points = {a, c, d}, dir);
+	}
+
+	if(SameDirection(adb, ao)) {
+		return Triangle(points = {a, d, b}, dir);
+	}
+	return true;
+	
+}
 
 bool 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 points;
-	points.push(support);
+	points.pushf(support);
 
 	rl::Vector3 dir = -support;
 
+	size_t its = 0;
 	while (true) {
 		support = Support(a, b, dir);
 
 		if (support.DotProduct(dir) <= 0) {
-			return false;
+			std::cout << "here!!!" << its << 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
 		}
 
+		points.pushf(support);
+
+
+		if (NextSimplex(points, dir)) {
+			return true;
+		}
+		its = its + 1;
 	}
 }

src/phys.hpp

@@ -51,13 +51,23 @@ struct Simplex {
 
 	Simplex& operator=(std::initializer_list<rl::Vector3> list);
 
-	void push(rl::Vector3 point);
+	void pushf(rl::Vector3 point);
 
 	rl::Vector3& operator[](size_t i);
 };
 
+
+bool Line(Simplex& points, rl::Vector3& dir);
+bool Triangle(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);
 
+bool SameDirection(rl::Vector3 dir, rl::Vector3& otherdir);
+
 #endif
 
 /* TODO: