From 37745b1bd20839325227f7cd5c9e432d3047024c Mon Sep 17 00:00:00 2001 From: IXtreme Date: Sun, 27 Jul 2025 03:31:46 -0400 Subject: [PATCH] gjk working git add .clang-format src/main.cpp src/phys.cpp src/phys.hpp ! --- .clang-format | 1 + src/main.cpp | 90 +++++++++++++++++++++---- src/phys.cpp | 181 ++++++++++++++++++++++++++++++++++++++++++-------- src/phys.hpp | 12 +++- 4 files changed, 242 insertions(+), 42 deletions(-) diff --git a/.clang-format b/.clang-format index 412420d..0288e02 100644 --- a/.clang-format +++ b/.clang-format @@ -30,4 +30,5 @@ AllowShortFunctionsOnASingleLine: Empty DerivePointerAlignment: false ReferenceAlignment: Left SpaceAfterCStyleCast: true +IndentCaseLabels: true diff --git a/src/main.cpp b/src/main.cpp index 9ce4fff..5cb190e 100644 --- a/src/main.cpp +++ b/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 #include #include @@ -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 left = Vector3RotateByAxisAngle(forward, cam.GetUp(), M_PI_2); + rl::Vector3 forward = Vector3RotateByQuaternion((Vector3) {0, 0, -1}, Rotation()); + rl::Vector3 up = (Vector3) {0, 1, 0}; + 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}, - -GetMouseDelta().x * 0.02 * DEG2RAD); + if(IsKeyDown(KEY_SPACE)) { + 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(); diff --git a/src/phys.cpp b/src/phys.cpp index d0ac32b..8e0260a 100644 --- a/src/phys.cpp +++ b/src/phys.cpp @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include #include #include @@ -26,20 +28,20 @@ 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); + // DrawSphere(pos, 10, GREEN); std::array 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}; - vectors[3] = (Vector3) {size.x/2, -size.y/2, size.z/2}; - vectors[4] = (Vector3) {size.x/2, size.y/2, -size.z/2}; - vectors[5] = (Vector3) {-size.x/2, -size.y/2, size.z/2}; - vectors[6] = (Vector3) {size.x/2, -size.y/2, -size.z/2}; - vectors[7] = (Vector3) {-size.x/2, size.y/2, -size.z/2}; + 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}; + vectors[3] = (Vector3) {size.x / 2, -size.y / 2, size.z / 2}; + vectors[4] = (Vector3) {size.x / 2, size.y / 2, -size.z / 2}; + vectors[5] = (Vector3) {-size.x / 2, -size.y / 2, size.z / 2}; + vectors[6] = (Vector3) {size.x / 2, -size.y / 2, -size.z / 2}; + 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); @@ -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,18 +60,17 @@ rl::Vector3 RecPrism::FindFurthestPoint(rl::Vector3 dir) const { std::array 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}; - vectors[3] = (Vector3) {size.x/2, -size.y/2, size.z/2}; - vectors[4] = (Vector3) {size.x/2, size.y/2, -size.z/2}; - vectors[5] = (Vector3) {-size.x/2, -size.y/2, size.z/2}; - vectors[6] = (Vector3) {size.x/2, -size.y/2, -size.z/2}; - vectors[7] = (Vector3) {-size.x/2, size.y/2, -size.z/2}; + 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}; + vectors[3] = (Vector3) {size.x / 2, -size.y / 2, size.z / 2}; + vectors[4] = (Vector3) {size.x / 2, size.y / 2, -size.z / 2}; + vectors[5] = (Vector3) {-size.x / 2, -size.y / 2, size.z / 2}; + vectors[6] = (Vector3) {size.x / 2, -size.y / 2, -size.z / 2}; + 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 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 list) { Simplex::Simplex() {} -void Simplex::push(rl::Vector3 point) { - points.push_back(point); +void Simplex::pushf(rl::Vector3 point) { + std::vector 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)); + 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; - while(true) { + 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; } } diff --git a/src/phys.hpp b/src/phys.hpp index bde266e..c3fb49e 100644 --- a/src/phys.hpp +++ b/src/phys.hpp @@ -51,13 +51,23 @@ struct Simplex { Simplex& operator=(std::initializer_list 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: