From 06d0a63a7552d9ba6a0d0e2218a32204043e7a8b Mon Sep 17 00:00:00 2001
From: Vectozavr <60608292+vectozavr@users.noreply.github.com>
Date: Sun, 17 Oct 2021 12:32:23 +0700
Subject: [PATCH] Move Solver to RigidBody.

---
 engine/World.cpp             |  2 +-
 engine/physics/RigidBody.cpp | 11 +++++++++++
 engine/physics/RigidBody.h   |  2 +-
 engine/physics/Solver.cpp    | 34 ----------------------------------
 engine/physics/Solver.h      | 16 ----------------
 5 files changed, 13 insertions(+), 52 deletions(-)
 delete mode 100644 engine/physics/Solver.cpp
 delete mode 100644 engine/physics/Solver.h

diff --git a/engine/World.cpp b/engine/World.cpp
index 69cc81e..7207bc8 100644
--- a/engine/World.cpp
+++ b/engine/World.cpp
@@ -77,7 +77,7 @@ void World::checkCollision(const std::string& body) {
                 if (gjk.first) {
                     if (obj->isCollider()) {
                         CollisionPoint epa = _objects[body]->EPA(gjk.second, obj);
-                        Solver::solveCollision(_objects[body], obj, epa);
+                        _objects[body]->solveCollision(epa);
                     }
                     if (_objects[body]->collisionCallBack() != nullptr)
                         _objects[body]->collisionCallBack()(name, obj);
diff --git a/engine/physics/RigidBody.cpp b/engine/physics/RigidBody.cpp
index be954f7..9954ddf 100644
--- a/engine/physics/RigidBody.cpp
+++ b/engine/physics/RigidBody.cpp
@@ -286,6 +286,17 @@ std::vector<std::pair<size_t, size_t>> RigidBody::_addIfUniqueEdge(const std::ve
     return newEdges;
 }
 
+void RigidBody::solveCollision(const CollisionPoint& collision) {
+
+    Vec3D velocity_parallel = collision.normal * velocity().dot(collision.normal);
+    Vec3D velocity_perpendicular = velocity() - velocity_parallel;
+
+    if(velocity().dot(collision.normal) > 0)
+        setVelocity(velocity_perpendicular);
+
+    translate(-collision.normal * collision.depth);
+}
+
 void RigidBody::updatePhysicsState() {
     translate(*_velocity * Time::deltaTime());
     _velocity = std::make_unique<Vec3D>(*_velocity + *_acceleration * Time::deltaTime());
diff --git a/engine/physics/RigidBody.h b/engine/physics/RigidBody.h
index 5118e81..adf9002 100644
--- a/engine/physics/RigidBody.h
+++ b/engine/physics/RigidBody.h
@@ -44,7 +44,6 @@ private:
     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});;
@@ -61,6 +60,7 @@ public:
 
     [[nodiscard]] std::pair<bool, Simplex> checkGJKCollision(std::shared_ptr<RigidBody> obj);
     [[nodiscard]] CollisionPoint EPA(const Simplex& simplex, std::shared_ptr<RigidBody> obj);
+    void solveCollision(const CollisionPoint& collision);
 
     [[nodiscard]] Vec3D collisionNormal() const { return *_collisionNormal; }
 
diff --git a/engine/physics/Solver.cpp b/engine/physics/Solver.cpp
deleted file mode 100644
index 8eab497..0000000
--- a/engine/physics/Solver.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-//
-// Created by Иван Ильин on 10.03.2021.
-//
-
-#include "Solver.h"
-#include "../utils/Log.h"
-
-void Solver::solveCollision(std::shared_ptr<RigidBody> obj1, std::shared_ptr<RigidBody> obj2, const CollisionPoint& collision) {
-
-    Vec3D obj1_velocity_parallel = collision.normal * obj1->velocity().dot(collision.normal);
-    Vec3D obj1_velocity_perpendicular = obj1->velocity() - obj1_velocity_parallel;
-
-    Vec3D obj2_velocity_parallel = collision.normal * obj2->velocity().dot(collision.normal);
-    Vec3D obj2_velocity_perpendicular = obj2->velocity() - obj2_velocity_parallel;
-
-    if(obj1->isCollision()) {
-        if(obj1->velocity().dot(collision.normal) > 0) {
-            obj1->setVelocity(obj1_velocity_perpendicular);
-        }
-    }
-    if(obj2->isCollision()) {
-        obj2->setVelocity(obj2_velocity_perpendicular - obj2_velocity_parallel * 0.8);
-    }
-
-    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()) {
-        obj1->translate(-collision.normal * collision.depth);
-    }
-    else
-        obj2->translate(collision.normal * collision.depth);
-}
-
diff --git a/engine/physics/Solver.h b/engine/physics/Solver.h
deleted file mode 100644
index d2b95a1..0000000
--- a/engine/physics/Solver.h
+++ /dev/null
@@ -1,16 +0,0 @@
-//
-// Created by Иван Ильин on 10.03.2021.
-//
-
-#ifndef ENGINE_SOLVER_H
-#define ENGINE_SOLVER_H
-
-#include "RigidBody.h"
-
-class Solver final {
-public:
-    static void solveCollision(std::shared_ptr<RigidBody> obj1, std::shared_ptr<RigidBody> obj2, const CollisionPoint& collision);
-};
-
-
-#endif //INC_3DZAVR_SOLVER_H