Revamped collision detection, missing the vertex inside vertex case
(which is happening a bit too often imho)
This commit is contained in:
Karma Riuk
parent
2cfa921547
commit
37a76e3e8d
254
collisions.cc
254
collisions.cc
@ -1,108 +1,204 @@
|
|||||||
#include "collisions.h"
|
#include "collisions.h"
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
|
#include <cassert>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
static std::vector<vec2d> edges_of(polygon& p) {
|
struct vertex {
|
||||||
std::vector<vec2d> edges;
|
vec2d v, p1, p2;
|
||||||
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) {
|
typedef std::pair<vec2d, vec2d> segment;
|
||||||
projection_t ret{INFINITY, -INFINITY};
|
|
||||||
|
|
||||||
double proj;
|
enum Orientation {
|
||||||
for (auto& point : p.global_points) {
|
COLINEAR,
|
||||||
proj = vec2d::dot(point, axis);
|
CLOCKWISE,
|
||||||
|
COUNTER_CLOCKWISE,
|
||||||
|
};
|
||||||
|
|
||||||
if (proj < ret.min_proj) {
|
static std::vector<vertex> vertices_of(polygon& p) {
|
||||||
ret.min_proj = proj;
|
std::vector<vertex> vertices;
|
||||||
ret.min_point = point;
|
vertices.reserve(p.global_points.size());
|
||||||
}
|
|
||||||
|
|
||||||
if (proj > ret.max_proj) {
|
// i = 1 and <= points.size() cuz { -1 % n = -1 } et c'est chiant
|
||||||
ret.max_proj = proj;
|
// so start from 1, "overflow" with i = points.size() and gg
|
||||||
ret.max_point = point;
|
for (uint i = 1; i <= p.global_points.size(); ++i)
|
||||||
}
|
vertices.push_back({
|
||||||
}
|
p.global_points[i % p.points.size()],
|
||||||
|
p.global_points[(i + 1) % p.points.size()],
|
||||||
|
p.global_points[(i - 1) % p.points.size()],
|
||||||
|
});
|
||||||
|
return vertices;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Given three collinear points p, q, r, the function checks if
|
||||||
|
// point q lies on line segment 'pr'
|
||||||
|
static bool on_segment(vec2d& q, segment& pr) {
|
||||||
|
return q.x <= std::max(pr.first.x, pr.second.x) &&
|
||||||
|
q.x >= std::min(pr.first.x, pr.second.x) &&
|
||||||
|
q.y <= std::max(pr.first.y, pr.second.y) &&
|
||||||
|
q.y >= std::min(pr.first.y, pr.second.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
static Orientation orientation(vec2d& p, vec2d& q, vec2d& r) {
|
||||||
|
int v = (q.y - p.y) * (r.x - q.x) - (q.x - p.x) * (r.y - q.y);
|
||||||
|
if (v == 0)
|
||||||
|
return COLINEAR;
|
||||||
|
return v > 0 ? CLOCKWISE : COUNTER_CLOCKWISE;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool do_intersect(segment s1, segment s2) {
|
||||||
|
// Find the four orientations needed for general and
|
||||||
|
// special cases
|
||||||
|
Orientation o1 = orientation(s1.first, s1.second, s2.first);
|
||||||
|
Orientation o2 = orientation(s1.first, s1.second, s2.second);
|
||||||
|
Orientation o3 = orientation(s2.first, s2.second, s1.first);
|
||||||
|
Orientation o4 = orientation(s2.first, s2.second, s1.second);
|
||||||
|
|
||||||
|
// General case
|
||||||
|
if (o1 != o2 && o3 != o4)
|
||||||
|
return true;
|
||||||
|
|
||||||
|
// Special Cases
|
||||||
|
// p1, q1 and p2 are collinear and p2 lies on segment p1q1
|
||||||
|
if (o1 == COLINEAR && on_segment(s2.first, s1))
|
||||||
|
return true;
|
||||||
|
|
||||||
|
// p1, q1 and q2 are collinear and q2 lies on segment p1q1
|
||||||
|
if (o2 == COLINEAR && on_segment(s2.second, s1))
|
||||||
|
return true;
|
||||||
|
|
||||||
|
// p2, q2 and p1 are collinear and p1 lies on segment p2q2
|
||||||
|
if (o3 == COLINEAR && on_segment(s1.first, s2))
|
||||||
|
return true;
|
||||||
|
|
||||||
|
// p2, q2 and q1 are collinear and q1 lies on segment p2q2
|
||||||
|
if (o4 == COLINEAR && on_segment(s1.second, s2))
|
||||||
|
return true;
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
static std::vector<segment> edges_of(polygon& p) {
|
||||||
|
std::vector<segment> ret;
|
||||||
|
ret.reserve(p.points.size());
|
||||||
|
for (uint i = 0; i < p.points.size(); ++i)
|
||||||
|
ret.push_back(
|
||||||
|
{p.global_points[i], p.global_points[(i + 1) % p.points.size()]});
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
static collision convex_collides(polygon& p, polygon& q) {
|
static collision penetration(segment& edge, vertex& vertex, vec2d& d) {
|
||||||
collision ret{false};
|
collision ret{true};
|
||||||
|
ret.impact_point = vertex.v;
|
||||||
|
|
||||||
std::vector<vec2d> edges_p = edges_of(p);
|
vec2d n = (edge.second - edge.first).orthogonal();
|
||||||
std::vector<vec2d> edges_q = edges_of(q);
|
ret.n = vec2d::normalize(n);
|
||||||
|
|
||||||
std::vector<vec2d> edges;
|
if (vec2d::dot(n, d) > 0)
|
||||||
edges.reserve(edges_p.size() + edges_q.size());
|
ret.n *= -1;
|
||||||
edges.insert(edges.end(), edges_p.begin(), edges_p.end());
|
std::cout << "Double pene omg" << std::endl;
|
||||||
edges.insert(edges.end(), edges_q.begin(), edges_q.end());
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
std::vector<vec2d> orthogonals;
|
static collision parallel(segment edge_p, segment edge_q, vec2d d) {
|
||||||
orthogonals.reserve(edges.size());
|
collision ret{true};
|
||||||
for (auto& v : edges)
|
|
||||||
orthogonals.push_back(v.orthogonal());
|
|
||||||
|
|
||||||
std::vector<collision> candidate_collisions;
|
vec2d line_start = edge_p.first;
|
||||||
candidate_collisions.reserve(orthogonals.size());
|
vec2d base = vec2d::normalize(edge_p.second - line_start);
|
||||||
|
|
||||||
|
std::pair<double, vec2d> proj_p1, proj_p2, proj_q1, proj_q2;
|
||||||
|
proj_p1 = {0, edge_p.first};
|
||||||
|
proj_p2 = {vec2d::dot(edge_p.second - line_start, base), edge_p.second};
|
||||||
|
proj_q1 = {vec2d::dot(edge_q.first - line_start, base), edge_q.first};
|
||||||
|
proj_q2 = {vec2d::dot(edge_q.second - line_start, base), edge_q.second};
|
||||||
|
|
||||||
|
std::pair<double, vec2d>*p_min, *q_min, *p_max, *q_max;
|
||||||
|
if (proj_p1.first < proj_p2.first) {
|
||||||
|
p_min = &proj_p1;
|
||||||
|
p_max = &proj_p2;
|
||||||
|
} else {
|
||||||
|
p_min = &proj_p2;
|
||||||
|
p_max = &proj_p1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (proj_q1.first < proj_q2.first) {
|
||||||
|
q_min = &proj_q1;
|
||||||
|
q_max = &proj_q2;
|
||||||
|
} else {
|
||||||
|
q_min = &proj_q2;
|
||||||
|
q_max = &proj_q1;
|
||||||
|
}
|
||||||
|
|
||||||
|
vec2d min = p_min->first < q_min->first ? q_min->second : p_min->second;
|
||||||
|
vec2d max = p_max->first < q_max->first ? p_max->second : q_max->second;
|
||||||
|
|
||||||
|
ret.impact_point = (min + max) / 2;
|
||||||
|
std::cout << "impact point: " << ret.impact_point << std::endl;
|
||||||
|
ret.n = base.orthogonal();
|
||||||
|
if (vec2d::dot(ret.n, d) > 0)
|
||||||
|
ret.n *= -1;
|
||||||
|
std::cout << "Parallel lol" << std::endl;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool are_vecs_parallel(vec2d s1, vec2d s2) {
|
||||||
|
return std::abs(vec2d::dot(vec2d::normalize(s1), vec2d::normalize(s2))) >
|
||||||
|
.99;
|
||||||
|
}
|
||||||
|
|
||||||
|
static double distance_between_parallel_segments(segment s1, segment s2) {
|
||||||
|
double area = vec2d::cross(s1.first - s2.first, s2.second - s2.first);
|
||||||
|
double base = vec2d::norm(s2.second - s2.first);
|
||||||
|
return std::abs(area / base);
|
||||||
|
}
|
||||||
|
|
||||||
|
#define SMALLEST_DIST 5
|
||||||
|
|
||||||
|
static collision vertex_edge_collision(polygon& p, polygon& q) {
|
||||||
|
std::vector<vertex> vertices_p = vertices_of(p);
|
||||||
|
std::vector<segment> edges_q = edges_of(q);
|
||||||
vec2d d = q.centroid() - p.centroid();
|
vec2d d = q.centroid() - p.centroid();
|
||||||
|
|
||||||
double min_overlap = INFINITY;
|
bool col1, col2;
|
||||||
for (auto& o : orthogonals) {
|
for (auto& vertex : vertices_p)
|
||||||
vec2d axis = vec2d::normalize(o);
|
for (auto& edge : edges_q) {
|
||||||
if (vec2d::dot(d, axis) > 0)
|
col1 = do_intersect(edge, {vertex.v, vertex.p1});
|
||||||
axis *= -1;
|
col2 = do_intersect(edge, {vertex.v, vertex.p2});
|
||||||
|
if (col1 || col2) {
|
||||||
|
if (col1 && col2)
|
||||||
|
return penetration(edge, vertex, d);
|
||||||
|
|
||||||
|
vec2d edge_v = edge.second - edge.first;
|
||||||
|
if (are_vecs_parallel(edge_v, vertex.v - vertex.p1) &&
|
||||||
|
distance_between_parallel_segments(
|
||||||
|
edge, {vertex.v, vertex.p1}) < SMALLEST_DIST)
|
||||||
|
return parallel(edge, {vertex.v, vertex.p1}, d);
|
||||||
|
|
||||||
projection_t p_proj = project(p, axis);
|
double dist = distance_between_parallel_segments(
|
||||||
projection_t q_proj = project(q, axis);
|
edge, {vertex.v, vertex.p2});
|
||||||
if (p_proj.max_proj < q_proj.min_proj ||
|
std::cout << "dist: " << dist << std::endl;
|
||||||
q_proj.max_proj < p_proj.min_proj)
|
if (are_vecs_parallel(edge_v, vertex.v - vertex.p2) &&
|
||||||
// the axis is separating (the projections don't overlap)
|
distance_between_parallel_segments(
|
||||||
return ret;
|
edge, {vertex.v, vertex.p2}) < SMALLEST_DIST)
|
||||||
|
return parallel(edge, {vertex.v, vertex.p2}, d);
|
||||||
|
}
|
||||||
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;
|
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
}
|
return {false};
|
||||||
// if no axis is sperating, then they must be colliding
|
}
|
||||||
ret.collides = true;
|
|
||||||
|
static collision convex_collides(polygon& p, polygon& q) {
|
||||||
|
collision ret;
|
||||||
|
|
||||||
|
std::cout << "Checking P is penetrates Q" << std::endl;
|
||||||
|
if ((ret = vertex_edge_collision(p, q)).collides)
|
||||||
|
return ret;
|
||||||
|
|
||||||
|
std::cout << "Checking Q is penetrates P" << std::endl;
|
||||||
|
if ((ret = vertex_edge_collision(q, p)).collides)
|
||||||
|
ret.n *= -1;
|
||||||
|
|
||||||
// if (vec2d::dot(d, ret.n) > 0)
|
|
||||||
// ret.n *= -1;
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user