39 lines
1.4 KiB
C++
39 lines
1.4 KiB
C++
//
|
|
// 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) {
|
|
if(!collision.hasCollision)
|
|
return;
|
|
|
|
Point4D obj1_velocity_parallel = collision.normal * obj1->velocity().dot(collision.normal);
|
|
|
|
obj1_velocity_parallel = Point4D{obj1_velocity_parallel.x(), obj1_velocity_parallel.y(), obj1_velocity_parallel.z(), 0};
|
|
|
|
Point4D obj1_velocity_perpendicular = obj1->velocity() - obj1_velocity_parallel;
|
|
|
|
Point4D obj2_velocity_parallel = collision.normal * obj2->velocity().dot(collision.normal);
|
|
Point4D 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);
|
|
}
|
|
|