Refactored collision detection (last version before changing the algo)

This commit is contained in:
Karma Riuk 2023-04-25 12:09:07 +02:00
parent 6b049eb831
commit ad841588ad
2 changed files with 57 additions and 70 deletions

View File

@ -14,66 +14,29 @@ static std::vector<vec2d> edges_of(polygon& p) {
return edges;
}
static bool separating_axis(
vec2d& axis, polygon& p, polygon& q, vec2d* pv, vec2d* impact_point) {
double min_p, max_p, min_q, max_q;
min_p = min_q = INFINITY;
max_p = max_q = -INFINITY;
struct projection_t {
double min_proj, max_proj;
vec2d min_point, max_point;
};
double projection;
vec2d min_p_point, max_p_point;
static projection_t project(polygon& p, vec2d axis) {
projection_t ret{INFINITY, -INFINITY};
double proj;
for (auto& point : p.global_points) {
projection = vec2d::dot(point, axis);
proj = vec2d::dot(point, axis);
if (projection < min_p) {
min_p = projection;
min_p_point = point;
if (proj < ret.min_proj) {
ret.min_proj = proj;
ret.min_point = point;
}
if (projection > max_p) {
max_p = projection;
max_p_point = point;
if (proj > ret.max_proj) {
ret.max_proj = proj;
ret.max_point = point;
}
}
for (auto& point : q.global_points) {
projection = vec2d::dot(point, axis);
min_q = std::min(min_q, projection);
max_q = std::max(max_q, projection);
}
if (max_p < min_q || max_q < min_p)
return true;
double d;
if (max_q - min_p < max_p - min_q) {
d = max_q - min_p;
*impact_point = min_p_point;
} else {
d = max_p - min_q;
*impact_point = max_p_point;
}
// push a bit more than needed so the shapes do not overlap in
// future tests due to float precision
double d_over_o_squared = d / vec2d::dot(axis, axis) + 1e-10;
*pv = d_over_o_squared * axis;
return false;
return true;
}
static uint get_smallest_vec_index(std::vector<vec2d> vs) {
uint ret, i = 0;
double min = INFINITY;
for (auto& v : vs) {
double dot = vec2d::dot(v, v);
if (dot < min) {
ret = i;
min = dot;
}
i++;
}
return ret;
}
@ -82,40 +45,64 @@ static collision convex_collides(polygon& p, polygon& q) {
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<vec2d> push_vectors;
push_vectors.reserve(orthogonals.size());
std::vector<vec2d> impact_points;
push_vectors.reserve(orthogonals.size());
std::vector<collision> candidate_collisions;
candidate_collisions.reserve(orthogonals.size());
vec2d push_vector;
vec2d impact_point;
vec2d d = q.centroid() - p.centroid();
double min_overlap = INFINITY;
for (auto& o : orthogonals) {
if (separating_axis(o, p, q, &push_vector, &impact_point))
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;
push_vectors.push_back(push_vector);
impact_points.push_back(impact_point);
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;
uint i = get_smallest_vec_index(push_vectors);
ret.n = vec2d::normalize(push_vectors[i]);
ret.impact_point = impact_points[i];
// assert that p pushes away from q
vec2d d = q.centroid() - p.centroid();
if (vec2d::dot(ret.n, d) > 0)
ret.n *= -1;
// if (vec2d::dot(d, ret.n) > 0)
// ret.n *= -1;
return ret;
}

View File

@ -5,7 +5,7 @@
#include "vec2d.h"
struct collision {
bool collides;
bool collides = false;
vec2d n; // minimum push vector
vec2d impact_point;
};