Idk what to do with EPA.

To salve the problem with infinite cycle in EPA, I added iteration counter.

Player.cpp

@@ -110,10 +110,12 @@ void Player::previousWeapon() {
 }
 
 void Player::fire() {
-    auto damagedPlayers = _weapons[_selectedWeapon]->fire(_rayCastFunction, attached("camera")->position(), attached("camera")->lookAt());
+    if(attached("camera") != nullptr) {
-    for(auto& damagedPlayer : damagedPlayers) {
+        auto damagedPlayers = _weapons[_selectedWeapon]->fire(_rayCastFunction, attached("camera")->position(), attached("camera")->lookAt());
-        sf::Uint16 targetId = std::stoi(damagedPlayer.first.substr(7));
+        for(auto& damagedPlayer : damagedPlayers) {
-        _damagePlayerCallBack(targetId, damagedPlayer.second);
+            sf::Uint16 targetId = std::stoi(damagedPlayer.first.substr(6));
+            _damagePlayerCallBack(targetId, damagedPlayer.second);
+        }
     }
 }
 

Player.h

@@ -54,7 +54,7 @@ public:
         loadObj("obj/cube.obj", "", Vec3D{0.5, 1.9, 0.5});
         setAcceleration(Vec3D{0, -_g, 0});
         setCollision(true);
-        setVisible(false);
+        //setVisible(false);
         setColor({240, 168, 168});
 
         _changeWeaponSound.setBuffer(*ResourceManager::loadSoundBuffer("sound/weapons/change_weapon.ogg"));

PlayerController.cpp

@@ -45,7 +45,7 @@ void PlayerController::update() {
                     Keyboard::isKeyPressed(sf::Keyboard::S));
 
     std::shared_ptr<Object> camera = _player->attached("camera");
-    if(_inRunning) {
+    if(camera != nullptr && _inRunning) {
         if (!Timeline::isInAnimList("camera_hor_oscil")) {
             Timeline::animate("camera_hor_oscil", new ATranslate(camera, -camera->left() / 6, 0.3,Animation::LoopOut::None, Animation::InterpolationType::cos));
             Timeline::animate("camera_hor_oscil", new AWait(0));
@@ -61,7 +61,7 @@ void PlayerController::update() {
 
             Timeline::animate("camera_init", new ATranslateToPoint( camera, _player->position() + Vec3D{0, 1.8, 0}, 0.3, Animation::LoopOut::None, Animation::InterpolationType::cos));
         }
-    } else if(inRunning_old && !_inRunning) {
+    } else if(camera != nullptr && inRunning_old && !_inRunning) {
         Timeline::deleteAnimationList("camera_hor_oscil");
         Timeline::deleteAnimationList("camera_vert_oscil");
         Timeline::deleteAnimationList("camera_init");
@@ -141,6 +141,9 @@ void PlayerController::update() {
     _player->setHeadAngle(_player->headAngle() + rotationLeft);
     _player->rotateWeaponsRelativePoint(_player->position() + Vec3D{0, 1.8, 0}, _player->left(), rotationLeft);
 
+    if(camera != nullptr)
+        camera->rotateLeft(_player->headAngle() - camera->angleLeftUpLookAt().x());
+
     if (_keyboard->isKeyTapped(sf::Keyboard::Right) || _keyboard->isKeyTapped(sf::Keyboard::E)) {
         _player->nextWeapon();
     }

Shooter.cpp

@@ -136,8 +136,6 @@ void Shooter::update() {
         screen->setTitle("Shooter");
         playerController->update();
         mouse->setMouseInCenter();
-        camera->rotateLeft(player->headAngle() - camera->angleLeftUpLookAt().x());
-
     } else {
         mainMenu.update();
     }
@@ -229,7 +227,7 @@ void Shooter::spawnPlayer(sf::Uint16 id) {
 }
 
 void Shooter::removePlayer(sf::Uint16 id) {
-    std::string name = "Enemy_" + std::to_string(id);
+    std::string name = std::to_string(id) + "_Enemy";
     world->removeBody(name);
     world->removeBody(name + "_head");
     world->removeBody(name + "_eye1");

Source.cpp

@@ -9,10 +9,10 @@ using namespace std;
 int main() {
     Shooter game;
 
-    //game.create(1280, 720, "Shooter");
+    game.create(1280, 720, "Shooter");
     //game.create(1920, 1080, "Shooter", true, {255, 255, 255}, sf::Style::Fullscreen);
 
-    game.create(2048, 1152, "Shooter");
+    //game.create(2048, 1152, "Shooter");
     //game.create(3072, 1920, "Shooter", true, {255, 255, 255}, sf::Style::Fullscreen);
 
     return 0;

engine/Mesh.h

@@ -41,6 +41,7 @@ public:
     // Rotate body around normalised vector 'v' by 'r' radians relative val 'point4D'
     void rotateRelativePoint(const Vec3D& point4D, const Vec3D& v, double r) override;
     void scale(const Vec3D& s) override;
+    [[nodiscard]] int size() const { return _tris.size()*3; }
 
     [[nodiscard]] sf::Color color() const { return _color; }
     void setColor(const sf::Color& c);

engine/Object.cpp

@@ -122,7 +122,7 @@ void Object::rotateToAngle(const Vec3D &v) {
 
 std::shared_ptr<Object> Object::attached(const std::string &name) {
     if(_attachedObjects.count(name) == 0)
-        Log::log("Object::attached: object '" + name + "' does not exist.");
+        return nullptr;
     return _attachedObjects.find(name)->second;
 }
 

engine/Point4D.cpp

@@ -66,5 +66,5 @@ Point4D Point4D::normalized() const {
     if(vecAbs > Consts::EPS)
         return Point4D(*this)/abs();
     else
-        return Point4D(0);
+        return Point4D(1);
 }

engine/Triangle.h

@@ -30,6 +30,7 @@ public:
 
     [[nodiscard]] sf::Color color() const { return _color; }
 
+    [[nodiscard]] double distance(const Vec3D& vec) const { return norm().dot(Vec3D(_points[0]) - vec); }
 };
 
 

engine/Vec3D.cpp

@@ -68,7 +68,7 @@ Vec3D Vec3D::normalized() const {
     if(vecAbs > Consts::EPS)
         return Vec3D(*this)/abs();
     else
-        return Vec3D(0);
+        return Vec3D(1);
 }
 
 double Vec3D::dot(const Vec3D& vec) const {

engine/World.cpp

@@ -101,6 +101,6 @@ void World::projectObjectsInCamera(std::shared_ptr<Camera> camera) {
 
 std::shared_ptr<RigidBody> World::body(const string &name) {
     if(_objects.count(name) == 0)
-        Log::log("World::body: mesh '" + name + "' does not exist.");
+        return nullptr;
     return _objects.find(name)->second;
 }

engine/physics/RigidBody.cpp

@@ -8,27 +8,31 @@
 #include "../utils/Time.h"
 #include <iostream>
 #include <cmath>
+#include <fstream>
 
 Vec3D RigidBody::_findFurthestPoint(const Vec3D& direction) {
-    std::unique_ptr<Vec3D> maxPoint = std::make_unique<Vec3D>(Vec3D{0, 0, 0});
+    std::shared_ptr<Vec3D> maxPoint = std::make_shared<Vec3D>(Vec3D{0, 0, 0});
-    auto maxDistance = -std::numeric_limits<double>::max();
+    double maxDistance = -std::numeric_limits<double>::max();
+
     for(auto& tri : triangles()){
         for(int i = 0; i < 3; i++){
-            Vec3D point = Vec3D(tri[i] + position().makePoint4D());
+            Vec3D point = Vec3D(tri[i]) + position();
 
-            double distance = point.dot(direction);
+            double distance = point.dot(direction.normalized());
             if(distance > maxDistance) {
                 maxDistance = distance;
-                maxPoint = std::make_unique<Vec3D>(point);
+                maxPoint = std::make_shared<Vec3D>(point);
             }
         }
     }
+
     return *maxPoint;
 }
 
 Vec3D RigidBody::_support(std::shared_ptr<RigidBody> obj, const Vec3D& direction) {
     Vec3D p1 = _findFurthestPoint(direction);
     Vec3D p2 = obj->_findFurthestPoint(-direction);
+    Vec3D res = p1 - p2;
 
     return p1 - p2;
 }
@@ -44,8 +48,8 @@ NextSimplex RigidBody::_nextSimplex(const Simplex &points) {
 }
 
 NextSimplex RigidBody::_lineCase(const Simplex& points) {
-    std::unique_ptr<Simplex> newPoints = std::make_unique<Simplex>(points);
+    std::shared_ptr<Simplex> newPoints = std::make_shared<Simplex>(points);
-    std::unique_ptr<Vec3D> newDirection;
+    std::shared_ptr<Vec3D> newDirection;
 
     Vec3D a = points[0];
     Vec3D b = points[1];
@@ -54,18 +58,18 @@ NextSimplex RigidBody::_lineCase(const Simplex& points) {
     Vec3D ao =   - a;
 
     if (ab.dot(ao) > 0) {
-        newDirection = std::make_unique<Vec3D>(ab.cross(ao).cross(ab));
+        newDirection = std::make_shared<Vec3D>(ab.cross(ao).cross(ab));
     } else {
-        newPoints = std::make_unique<Simplex>(Simplex{a});
+        newPoints = std::make_shared<Simplex>(Simplex{a});
-        newDirection = std::make_unique<Vec3D>(ao);
+        newDirection = std::make_shared<Vec3D>(ao);
     }
 
     return NextSimplex{*newPoints, *newDirection, false};
 }
 
 NextSimplex RigidBody::_triangleCase(const Simplex &points) {
-    std::unique_ptr<Simplex> newPoints = std::make_unique<Simplex>(points);
+    std::shared_ptr<Simplex> newPoints = std::make_shared<Simplex>(points);
-    std::unique_ptr<Vec3D> newDirection;
+    std::shared_ptr<Vec3D> newDirection;
 
     Vec3D a = points[0];
     Vec3D b = points[1];
@@ -79,8 +83,8 @@ NextSimplex RigidBody::_triangleCase(const Simplex &points) {
 
     if (abc.cross(ac).dot(ao) > 0) {
         if (ac.dot(ao) > 0) {
-            newPoints = std::make_unique<Simplex>(Simplex{ a, c });
+            newPoints = std::make_shared<Simplex>(Simplex{ a, c });
-            newDirection = std::make_unique<Vec3D>(ac.cross(ao).cross(ac));
+            newDirection = std::make_shared<Vec3D>(ac.cross(ao).cross(ac));
         }
         else {
             return _lineCase(Simplex { a, b });
@@ -91,10 +95,10 @@ NextSimplex RigidBody::_triangleCase(const Simplex &points) {
         }
         else {
             if (abc.dot(ao) > 0) {
-                newDirection = std::make_unique<Vec3D>(abc);
+                newDirection = std::make_shared<Vec3D>(abc);
             } else {
-                newPoints = std::make_unique<Simplex>(Simplex{ a, c, b });
+                newPoints = std::make_shared<Simplex>(Simplex{ a, c, b });
-                newDirection = std::make_unique<Vec3D>(-abc);
+                newDirection = std::make_shared<Vec3D>(-abc);
             }
         }
     }
@@ -135,17 +139,17 @@ NextSimplex RigidBody::_tetrahedronCase(const Simplex &points) {
 std::pair<bool, Simplex> RigidBody::checkGJKCollision(std::shared_ptr<RigidBody> obj) {
 
     // Get initial support point in any direction
-    std::unique_ptr<Vec3D> support = std::make_unique<Vec3D>(_support(obj, Vec3D{1, 0, 0}));
+    std::shared_ptr<Vec3D> support = std::make_shared<Vec3D>(_support(obj, Vec3D{1, 0, 0}));
 
     // Simplex is an array of points, max count is 4
-    std::unique_ptr<Simplex> points = std::make_unique<Simplex>();
+    std::shared_ptr<Simplex> points = std::make_shared<Simplex>();
     points->push_front(*support);
 
     // New direction is towards the origin
-    std::unique_ptr<Vec3D> direction = std::make_unique<Vec3D>(-*support);
+    std::shared_ptr<Vec3D> direction = std::make_shared<Vec3D>(-*support);
 
     while (true) {
-        support = std::make_unique<Vec3D>(_support(obj, *direction));
+        support = std::make_shared<Vec3D>(_support(obj, *direction));
 
         if (support->dot(*direction) <= 0)
             return std::make_pair(false, *points); // no collision
@@ -154,8 +158,8 @@ std::pair<bool, Simplex> RigidBody::checkGJKCollision(std::shared_ptr<RigidBody>
 
         NextSimplex nextSimplex = _nextSimplex(*points);
 
-        direction = std::make_unique<Vec3D>(nextSimplex.newDirection);
+        direction = std::make_shared<Vec3D>(nextSimplex.newDirection);
-        points = std::make_unique<Simplex>(nextSimplex.newSimplex);
+        points = std::make_shared<Simplex>(nextSimplex.newSimplex);
 
         if (nextSimplex.finishSearching) {
             if(obj->isCollider())
@@ -175,13 +179,15 @@ CollisionPoint RigidBody::EPA(const Simplex& simplex, std::shared_ptr<RigidBody>
             1, 3, 2
     };
 
-    // list: vector4(normal, distance), index: min distance
+    auto faceNormals = _getFaceNormals(polytope, faces);
-    auto [normals, minFace] = std::move(_getFaceNormals(polytope, faces));
+    std::vector<std::shared_ptr<FaceNormal>> normals = faceNormals.first;
+    size_t minFace = faceNormals.second;
 
-    std::shared_ptr<Vec3D> minNormal{};
+    std::shared_ptr<Vec3D> minNormal = std::make_shared<Vec3D>(normals[minFace]->normal);
     double minDistance = std::numeric_limits<double>::max();
 
-    while (minDistance == std::numeric_limits<double>::max()) {
+    int iters = 0;
+    while (minDistance == std::numeric_limits<double>::max() && iters++ < size() + obj->size()) {
         minNormal = std::make_shared<Vec3D>(normals[minFace]->normal);
         minDistance = normals[minFace]->distance;
 
@@ -213,39 +219,27 @@ CollisionPoint RigidBody::EPA(const Simplex& simplex, std::shared_ptr<RigidBody>
                 newFaces.push_back(edgeIndex2);
                 newFaces.push_back(polytope.size());
             }
-
             polytope.push_back(support);
 
-            auto [newNormals, newMinFace] = _getFaceNormals(polytope, newFaces);
+            faces.insert(faces.end(), newFaces.begin(), newFaces.end());
 
-            if(newNormals.empty())
+            auto newFaceNormals = _getFaceNormals(polytope, faces);
-                break;
 
-            double oldMinDistance = std::numeric_limits<double>::max();
+            normals = newFaceNormals.first;
-            for (size_t i = 0; i < normals.size(); i++) {
+            minFace = newFaceNormals.second;
-                if (normals[i]->distance < oldMinDistance) {
-                    oldMinDistance = normals[i]->distance;
-                    minFace = i;
-                }
-            }
-
-            if (newNormals[newMinFace]->distance < oldMinDistance) {
-                minFace = newMinFace + normals.size();
-            }
-
-            faces  .insert(faces  .end(), newFaces  .begin(), newFaces  .end());
-            normals.insert(normals.end(), newNormals.begin(), newNormals.end());
         }
     }
 
     _collisionNormal = minNormal;
-    return CollisionPoint{*minNormal, minDistance + Consts::EPA_EPS, minDistance < std::numeric_limits<double>::max()};
+    if(std::abs(minDistance - std::numeric_limits<double>::max()) < Consts::EPS)
+        return CollisionPoint{*minNormal, 0};
+
+    return CollisionPoint{*minNormal, minDistance + Consts::EPA_EPS};
 }
 
 std::pair<std::vector<std::shared_ptr<FaceNormal>>, size_t> RigidBody::_getFaceNormals(const std::vector<Vec3D>& polytope, const std::vector<size_t>& faces) {
     std::vector<std::shared_ptr<FaceNormal>> normals;
     size_t nearestFaceIndex = 0;
-
     double minDistance = std::numeric_limits<double>::max();
 
     for (size_t i = 0; i < faces.size(); i += 3) {
@@ -254,17 +248,14 @@ std::pair<std::vector<std::shared_ptr<FaceNormal>>, size_t> RigidBody::_getFaceN
         Vec3D c = polytope[faces[i + 2]];
 
         std::shared_ptr<Vec3D> normal = std::make_shared<Vec3D>((b - a).cross(c - a).normalized());
-        if(normal->sqrAbs() < Consts::EPS)
-            continue;
 
         double distance = normal->dot(a);
 
-        if (distance < 0) {
+        if (distance < -Consts::EPS) {
             normal = std::make_unique<Vec3D>(-*normal);
             distance *= -1;
         }
 
-        normal = std::make_shared<Vec3D>(Vec3D{normal->x(), normal->y(), normal->z()});
         normals.emplace_back(std::make_shared<FaceNormal>(FaceNormal{*normal, distance}));
 
         if (distance < minDistance) {
@@ -316,3 +307,15 @@ void RigidBody::setAcceleration(const Vec3D& acceleration) {
 RigidBody::RigidBody(const Mesh &mesh) : Mesh(mesh) {
 
 }
+
+void RigidBody::makeLogObjPolytope(const std::vector<Vec3D> &polytope, const std::vector<size_t> &faces) {
+    std::fstream file("polytope_log.obj", std::ios::out);
+
+    for(auto &p : polytope)
+        file << "v " << p.x() << " " << p.y()  << " " << p.z() << std::endl;
+
+    for(int i = 0; i < faces.size(); i += 3)
+        file << "f " << faces[i + 0]+1 << " " << faces[i + 1]+1  << " " << faces[i + 2]+1 << std::endl;
+
+    file.close();
+}

engine/physics/RigidBody.h

@@ -15,7 +15,6 @@
 struct CollisionPoint {
     const Vec3D normal;
     const double depth;
-    const bool hasCollision;
 };
 
 struct FaceNormal {
@@ -41,9 +40,11 @@ private:
     static NextSimplex _triangleCase(const Simplex& points);
     static NextSimplex _tetrahedronCase(const Simplex& points);
 
-    static std::pair<std::vector<std::shared_ptr<FaceNormal>>, size_t> _getFaceNormals(const std::vector<Vec3D>& polytope, const std::vector<size_t>&  faces);
+    static std::pair<std::vector<std::shared_ptr<FaceNormal>>, size_t> _getFaceNormals(const std::vector<Vec3D>& polytope, const std::vector<size_t>& faces);
     static std::vector<std::pair<size_t, size_t>> _addIfUniqueEdge(const std::vector<std::pair<size_t, size_t>>& edges, const std::vector<size_t>& faces, size_t a, size_t b);
 
+    static void makeLogObjPolytope(const std::vector<Vec3D>& polytope, const std::vector<size_t>& faces);
+
 protected:
     std::unique_ptr<Vec3D> _velocity = std::make_unique<Vec3D>(Vec3D{0, 0, 0});;
     std::unique_ptr<Vec3D> _acceleration = std::make_unique<Vec3D>(Vec3D{0, 0, 0});;

engine/physics/Simplex.h

@@ -6,7 +6,7 @@
 #define ENGINE_SIMPLEX_H
 
 #include "../Vec3D.h"
-#include <deque>
+#include <list>
 
 enum class SimplexType {
     Zero,
@@ -18,7 +18,7 @@ enum class SimplexType {
 
 struct Simplex final {
 private:
-    std::deque<Vec3D> _points{};
+    std::list<Vec3D> _points{};
 
 public:
     Simplex() = default;
@@ -37,7 +37,13 @@ public:
             _points.pop_back();
     }
 
-    Vec3D operator[](unsigned i) const { return _points[i]; }
+    Vec3D operator[](unsigned i) const {
+        auto it = _points.begin();
+        for(int k=0; k<i; k++)
+            ++it;
+        return *it;
+
+    }
     [[nodiscard]] unsigned size() const { return _points.size(); }
 
     [[nodiscard]] auto begin() const { return _points.begin(); }

engine/physics/Solver.cpp

@@ -6,8 +6,6 @@
 #include "../utils/Log.h"
 
 void Solver::solveCollision(std::shared_ptr<RigidBody> obj1, std::shared_ptr<RigidBody> obj2, const CollisionPoint& collision) {
-    if(!collision.hasCollision)
-        return;
 
     Vec3D obj1_velocity_parallel = collision.normal * obj1->velocity().dot(collision.normal);
     Vec3D obj1_velocity_perpendicular = obj1->velocity() - obj1_velocity_parallel;
@@ -27,8 +25,9 @@ void Solver::solveCollision(std::shared_ptr<RigidBody> obj1, std::shared_ptr<Rig
     if(obj1->isCollision() && obj2->isCollision()) {
         obj1->translate(-collision.normal * collision.depth/2.0);
         obj2->translate(collision.normal * collision.depth/2.0);
-    } else if(obj1->isCollision())
+    } else if(obj1->isCollision()) {
         obj1->translate(-collision.normal * collision.depth);
+    }
     else
         obj2->translate(collision.normal * collision.depth);
 }

weapon/Shotgun.cpp

@@ -34,7 +34,7 @@ Shotgun::processFire(std::function<std::pair<Vec3D, std::string>(const Vec3D&, c
 
         // damage player
         auto rayCast = rayCastFunction(pos, pos + direction * 1000 + randV);
-        if (rayCast.second.find("Player") != std::string::npos) {
+        if (rayCast.second.find("Enemy") != std::string::npos) {
             damagedPlayers[rayCast.second] += _damage / (1.0 + (pos - rayCast.first).abs());
         }
 

weapon/Weapon.cpp

@@ -66,7 +66,7 @@ std::map<std::string, double> Weapon::processFire(std::function<std::pair<Vec3D,
 
     // damage player
     auto rayCast = rayCastFunction(pos, pos + direction * 1000 + randV);
-    if(rayCast.second.find("Player") != std::string::npos) {
+    if(rayCast.second.find("Enemy") != std::string::npos) {
         damagedPlayers[rayCast.second] += _damage/(1.0 + (pos - rayCast.first).abs());
     }