112 lines
3.2 KiB
C++
112 lines
3.2 KiB
C++
#include "collisions.h"
|
|
|
|
#include <algorithm>
|
|
#include <iostream>
|
|
#include <vector>
|
|
|
|
static std::vector<vec2d> edges_of(polygon& p) {
|
|
std::vector<vec2d> edges;
|
|
edges.reserve(p.global_points.size());
|
|
|
|
for (uint i = 0; i < p.global_points.size(); ++i)
|
|
edges.push_back(p.global_points[(i + 1) % p.global_points.size()] -
|
|
p.global_points[i]);
|
|
return edges;
|
|
}
|
|
|
|
struct projection_t {
|
|
double min_proj, max_proj;
|
|
vec2d min_point, max_point;
|
|
};
|
|
|
|
static projection_t project(polygon& p, vec2d axis) {
|
|
projection_t ret{INFINITY, -INFINITY};
|
|
|
|
double proj;
|
|
for (auto& point : p.global_points) {
|
|
proj = vec2d::dot(point, axis);
|
|
|
|
if (proj < ret.min_proj) {
|
|
ret.min_proj = proj;
|
|
ret.min_point = point;
|
|
}
|
|
|
|
if (proj > ret.max_proj) {
|
|
ret.max_proj = proj;
|
|
ret.max_point = point;
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static collision convex_collides(polygon& p, polygon& q) {
|
|
collision ret{false};
|
|
|
|
std::vector<vec2d> edges_p = edges_of(p);
|
|
std::vector<vec2d> edges_q = edges_of(q);
|
|
|
|
std::vector<vec2d> edges;
|
|
edges.reserve(edges_p.size() + edges_q.size());
|
|
edges.insert(edges.end(), edges_p.begin(), edges_p.end());
|
|
edges.insert(edges.end(), edges_q.begin(), edges_q.end());
|
|
|
|
std::vector<vec2d> orthogonals;
|
|
orthogonals.reserve(edges.size());
|
|
for (auto& v : edges)
|
|
orthogonals.push_back(v.orthogonal());
|
|
|
|
std::vector<collision> candidate_collisions;
|
|
candidate_collisions.reserve(orthogonals.size());
|
|
|
|
vec2d d = q.centroid() - p.centroid();
|
|
|
|
double min_overlap = INFINITY;
|
|
for (auto& o : orthogonals) {
|
|
vec2d axis = vec2d::normalize(o);
|
|
if (vec2d::dot(d, axis) > 0)
|
|
axis *= -1;
|
|
|
|
|
|
projection_t p_proj = project(p, axis);
|
|
projection_t q_proj = project(q, axis);
|
|
if (p_proj.max_proj < q_proj.min_proj ||
|
|
q_proj.max_proj < p_proj.min_proj)
|
|
// the axis is separating (the projections don't overlap)
|
|
return ret;
|
|
|
|
|
|
double overlap = std::min(p_proj.max_proj, q_proj.max_proj) -
|
|
std::max(p_proj.min_proj, q_proj.min_proj);
|
|
|
|
if (overlap < min_overlap) {
|
|
min_overlap = overlap;
|
|
ret.n = axis;
|
|
|
|
ret.impact_point = p_proj.max_proj > q_proj.min_proj
|
|
? p_proj.min_point
|
|
: q_proj.max_point;
|
|
|
|
// if (vec2d::dot(axis, d) > 0) {
|
|
// ret.impact_point = p_proj.max_proj > q_proj.min_proj
|
|
// ? q_proj.min_point
|
|
// : q_proj.max_point;
|
|
// } else {
|
|
// ret.impact_point = p_proj.max_proj > q_proj.min_proj
|
|
// ? p_proj.min_point
|
|
// : p_proj.max_point;
|
|
// }
|
|
}
|
|
}
|
|
// if no axis is sperating, then they must be colliding
|
|
ret.collides = true;
|
|
|
|
// if (vec2d::dot(d, ret.n) > 0)
|
|
// ret.n *= -1;
|
|
return ret;
|
|
}
|
|
|
|
collision collides(polygon& p, polygon& q) {
|
|
return convex_collides(p, q);
|
|
}
|