shooter/engine/physics/RigidBody.cpp

319 lines
9.9 KiB
C++
Raw Normal View History

2021-09-13 15:53:43 +03:00
//
// Created by Иван Ильин on 05.02.2021.
//
#include "RigidBody.h"
#include "../Plane.h"
#include "../utils/Log.h"
#include "../utils/Time.h"
#include <iostream>
#include <cmath>
Vec3D RigidBody::_findFurthestPoint(const Vec3D& direction) {
std::unique_ptr<Vec3D> maxPoint = std::make_unique<Vec3D>(Vec3D{0, 0, 0});
auto maxDistance = -std::numeric_limits<double>::max();
2021-09-13 15:53:43 +03:00
for(auto& tri : triangles()){
for(int i = 0; i < 3; i++){
Vec3D point = Vec3D(tri[i] + position().makePoint4D());
2021-09-13 15:53:43 +03:00
double distance = point.dot(direction);
if(distance > maxDistance) {
maxDistance = distance;
maxPoint = std::make_unique<Vec3D>(point);
2021-09-13 15:53:43 +03:00
}
}
}
return *maxPoint;
2021-09-13 15:53:43 +03:00
}
Vec3D RigidBody::_support(std::shared_ptr<RigidBody> obj, const Vec3D& direction) {
Vec3D p1 = _findFurthestPoint(direction);
Vec3D p2 = obj->_findFurthestPoint(-direction);
2021-09-13 15:53:43 +03:00
return p1 - p2;
}
NextSimplex RigidBody::_nextSimplex(const Simplex &points) {
switch (points.type()) {
case SimplexType::Line: return _lineCase(points);
case SimplexType::Triangle: return _triangleCase(points);
case SimplexType::Tetrahedron: return _tetrahedronCase(points);
2021-09-13 15:53:43 +03:00
default: throw std::logic_error{"RigidBody::_nextSimplex: simplex is not Line, Triangle or Tetrahedron"};
}
2021-09-13 15:53:43 +03:00
}
NextSimplex RigidBody::_lineCase(const Simplex& points) {
std::unique_ptr<Simplex> newPoints = std::make_unique<Simplex>(points);
std::unique_ptr<Vec3D> newDirection;
2021-09-13 15:53:43 +03:00
Vec3D a = points[0];
Vec3D b = points[1];
Vec3D ab = b - a;
Vec3D ao = - a;
2021-09-13 15:53:43 +03:00
if (ab.dot(ao) > 0) {
newDirection = std::make_unique<Vec3D>(ab.cross(ao).cross(ab));
2021-09-13 15:53:43 +03:00
} else {
newPoints = std::make_unique<Simplex>(Simplex{a});
newDirection = std::make_unique<Vec3D>(ao);
2021-09-13 15:53:43 +03:00
}
return NextSimplex{*newPoints, *newDirection, false};
2021-09-13 15:53:43 +03:00
}
NextSimplex RigidBody::_triangleCase(const Simplex &points) {
std::unique_ptr<Simplex> newPoints = std::make_unique<Simplex>(points);
std::unique_ptr<Vec3D> newDirection;
Vec3D a = points[0];
Vec3D b = points[1];
Vec3D c = points[2];
2021-09-13 15:53:43 +03:00
Vec3D ab = b - a;
Vec3D ac = c - a;
Vec3D ao = - a;
2021-09-13 15:53:43 +03:00
Vec3D abc = ab.cross(ac);
2021-09-13 15:53:43 +03:00
if (abc.cross(ac).dot(ao) > 0) {
2021-09-13 15:53:43 +03:00
if (ac.dot(ao) > 0) {
newPoints = std::make_unique<Simplex>(Simplex{ a, c });
newDirection = std::make_unique<Vec3D>(ac.cross(ao).cross(ac));
2021-09-13 15:53:43 +03:00
}
else {
return _lineCase(Simplex { a, b });
2021-09-13 15:53:43 +03:00
}
} else {
if (ab.cross(abc).dot(ao) > 0) {
return _lineCase(Simplex { a, b });
2021-09-13 15:53:43 +03:00
}
else {
2021-09-13 15:53:43 +03:00
if (abc.dot(ao) > 0) {
newDirection = std::make_unique<Vec3D>(abc);
2021-09-13 15:53:43 +03:00
} else {
newPoints = std::make_unique<Simplex>(Simplex{ a, c, b });
newDirection = std::make_unique<Vec3D>(-abc);
2021-09-13 15:53:43 +03:00
}
}
}
return NextSimplex{*newPoints, *newDirection, false};
2021-09-13 15:53:43 +03:00
}
NextSimplex RigidBody::_tetrahedronCase(const Simplex &points) {
Vec3D a = points[0];
Vec3D b = points[1];
Vec3D c = points[2];
Vec3D d = points[3];
2021-09-13 15:53:43 +03:00
Vec3D ab = b - a;
Vec3D ac = c - a;
Vec3D ad = d - a;
Vec3D ao = - a;
2021-09-13 15:53:43 +03:00
Vec3D abc = ab.cross(ac);
Vec3D acd = ac.cross(ad);
Vec3D adb = ad.cross(ab);
2021-09-13 15:53:43 +03:00
if (abc.dot(ao) > 0) {
return _triangleCase(Simplex{ a, b, c });
2021-09-13 15:53:43 +03:00
}
if (acd.dot(ao) > 0) {
return _triangleCase(Simplex{ a, c, d });
2021-09-13 15:53:43 +03:00
}
if (adb.dot(ao) > 0) {
return _triangleCase(Simplex{ a, d, b });
2021-09-13 15:53:43 +03:00
}
return NextSimplex{points, Vec3D(), true};
2021-09-13 15:53:43 +03:00
}
std::pair<bool, Simplex> RigidBody::checkGJKCollision(std::shared_ptr<RigidBody> obj) {
2021-09-13 15:53:43 +03:00
// Get initial support point in any direction
std::unique_ptr<Vec3D> support = std::make_unique<Vec3D>(_support(obj, Vec3D{1, 0, 0}));
2021-09-13 15:53:43 +03:00
// Simplex is an array of points, max count is 4
std::unique_ptr<Simplex> points = std::make_unique<Simplex>();
points->push_front(*support);
2021-09-13 15:53:43 +03:00
// New direction is towards the origin
std::unique_ptr<Vec3D> direction = std::make_unique<Vec3D>(-*support);
2021-09-13 15:53:43 +03:00
while (true) {
support = std::make_unique<Vec3D>(_support(obj, *direction));
if (support->dot(*direction) <= 0)
return std::make_pair(false, *points); // no collision
2021-09-13 15:53:43 +03:00
points->push_front(*support);
2021-09-13 15:53:43 +03:00
NextSimplex nextSimplex = _nextSimplex(*points);
2021-09-13 15:53:43 +03:00
direction = std::make_unique<Vec3D>(nextSimplex.newDirection);
points = std::make_unique<Simplex>(nextSimplex.newSimplex);
if (nextSimplex.finishSearching) {
2021-09-13 15:53:43 +03:00
if(obj->isCollider())
_inCollision = true;
return std::make_pair(true, *points);
2021-09-13 15:53:43 +03:00
}
}
}
CollisionPoint RigidBody::EPA(const Simplex& simplex, std::shared_ptr<RigidBody> obj) {
2021-09-13 15:53:43 +03:00
std::vector<Vec3D> polytope(simplex.begin(), simplex.end());
2021-09-13 15:53:43 +03:00
std::vector<size_t> faces = {
0, 1, 2,
0, 3, 1,
0, 2, 3,
1, 3, 2
};
// list: vector4(normal, distance), index: min distance
auto [normals, minFace] = std::move(_getFaceNormals(polytope, faces));
2021-09-13 15:53:43 +03:00
std::shared_ptr<Vec3D> minNormal{};
double minDistance = std::numeric_limits<double>::max();
2021-09-13 15:53:43 +03:00
while (minDistance == std::numeric_limits<double>::max()) {
minNormal = std::make_shared<Vec3D>(normals[minFace]->normal);
minDistance = normals[minFace]->distance;
2021-09-13 15:53:43 +03:00
Vec3D support = _support(obj, *minNormal);
double sDistance = minNormal->dot(support);
2021-09-13 15:53:43 +03:00
if (std::abs(sDistance - minDistance) > Consts::EPA_EPS) {
minDistance = std::numeric_limits<double>::max();
2021-09-13 15:53:43 +03:00
std::vector<std::pair<size_t, size_t>> uniqueEdges;
for (size_t i = 0; i < normals.size(); i++) {
if (normals[i]->normal.dot(support) > 0) {
2021-09-13 15:53:43 +03:00
size_t f = i * 3;
uniqueEdges = _addIfUniqueEdge(uniqueEdges, faces, f + 0, f + 1);
uniqueEdges = _addIfUniqueEdge(uniqueEdges, faces, f + 1, f + 2);
uniqueEdges = _addIfUniqueEdge(uniqueEdges, faces, f + 2, f + 0);
2021-09-13 15:53:43 +03:00
faces.erase(faces.begin() + f);
faces.erase(faces.begin() + f);
faces.erase(faces.begin() + f);
normals.erase(normals.begin() + i--);
2021-09-13 15:53:43 +03:00
}
}
2021-09-13 15:53:43 +03:00
std::vector<size_t> newFaces;
for (auto [edgeIndex1, edgeIndex2] : uniqueEdges) {
newFaces.push_back(edgeIndex1);
newFaces.push_back(edgeIndex2);
newFaces.push_back(polytope.size());
}
polytope.push_back(support);
2021-10-03 07:47:05 +03:00
auto [newNormals, newMinFace] = _getFaceNormals(polytope, newFaces);
2021-09-13 15:53:43 +03:00
if(newNormals.empty())
break;
double oldMinDistance = std::numeric_limits<double>::max();
2021-09-13 15:53:43 +03:00
for (size_t i = 0; i < normals.size(); i++) {
if (normals[i]->distance < oldMinDistance) {
oldMinDistance = normals[i]->distance;
2021-09-13 15:53:43 +03:00
minFace = i;
}
}
if (newNormals[newMinFace]->distance < oldMinDistance) {
2021-09-13 15:53:43 +03:00
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()};
2021-09-13 15:53:43 +03:00
}
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();
2021-09-13 15:53:43 +03:00
for (size_t i = 0; i < faces.size(); i += 3) {
Vec3D a = polytope[faces[i + 0]];
Vec3D b = polytope[faces[i + 1]];
Vec3D c = polytope[faces[i + 2]];
2021-09-13 15:53:43 +03:00
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);
2021-09-13 15:53:43 +03:00
if (distance < 0) {
normal = std::make_unique<Vec3D>(-*normal);
2021-09-13 15:53:43 +03:00
distance *= -1;
}
normal = std::make_shared<Vec3D>(Vec3D{normal->x(), normal->y(), normal->z()});
normals.emplace_back(std::make_shared<FaceNormal>(FaceNormal{*normal, distance}));
2021-09-13 15:53:43 +03:00
if (distance < minDistance) {
nearestFaceIndex = i / 3;
2021-09-13 15:53:43 +03:00
minDistance = distance;
}
}
return {normals, nearestFaceIndex};
2021-09-13 15:53:43 +03:00
}
std::vector<std::pair<size_t, size_t>> RigidBody::_addIfUniqueEdge(const std::vector<std::pair<size_t, size_t>>& edges, const std::vector<size_t>& faces, size_t a, size_t b) {
2021-09-13 15:53:43 +03:00
std::vector<std::pair<size_t, size_t>> newEdges = edges;
2021-09-13 15:53:43 +03:00
// We are interested in reversed edge
// 0--<--3
// / \ B / A: 2-0
// / A \ / B: 0-2
// 1-->--2
auto reverse = std::find(newEdges.begin(), newEdges.end(), std::make_pair(faces[b], faces[a]));
2021-09-13 15:53:43 +03:00
if (reverse != newEdges.end()) {
newEdges.erase(reverse);
} else {
newEdges.emplace_back(faces[a], faces[b]);
2021-09-13 15:53:43 +03:00
}
return newEdges;
2021-09-13 15:53:43 +03:00
}
void RigidBody::updatePhysicsState() {
translate(*_velocity * Time::deltaTime());
_velocity = std::make_unique<Vec3D>(*_velocity + *_acceleration * Time::deltaTime());
2021-09-13 15:53:43 +03:00
}
void RigidBody::setVelocity(const Vec3D& velocity) {
_velocity = std::make_unique<Vec3D>(velocity);
2021-09-13 15:53:43 +03:00
}
void RigidBody::addVelocity(const Vec3D &velocity) {
_velocity = std::make_unique<Vec3D>(*_velocity + velocity);
2021-09-13 15:53:43 +03:00
}
void RigidBody::setAcceleration(const Vec3D& acceleration) {
_acceleration = std::make_unique<Vec3D>(acceleration);
}
RigidBody::RigidBody(const Mesh &mesh) : Mesh(mesh) {
2021-09-13 15:53:43 +03:00
}