378 lines
12 KiB
C++
378 lines
12 KiB
C++
#include <assert.h>
|
|
|
|
#include <a8/a8.h>
|
|
|
|
#include <algorithm>
|
|
#include <cfloat>
|
|
#include <glm/gtx/intersect.hpp>
|
|
#include <glm/vec3.hpp>
|
|
|
|
#include <a8/collision.h>
|
|
|
|
namespace a8
|
|
{
|
|
bool IntersectSegmentCircle(a8::Vec2 p0, a8::Vec2 p1, a8::Vec2 pos, float rad)
|
|
{
|
|
a8::Vec2 t = p1 - p0;
|
|
float d = std::max(t.Norm(), 0.0001f);
|
|
a8::Vec2 n = p0 - pos;
|
|
float v = n.Dot(t);
|
|
float z = n.Dot(n) - rad*rad;
|
|
if (z > 0 && v > 0) {
|
|
return false;
|
|
}
|
|
float u = v*v - z;
|
|
if (u < 0) {
|
|
return false;
|
|
}
|
|
float i = sqrt(u);
|
|
float x = -v - i;
|
|
if (x < 0 && (x = -v + i), x <= d) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool IntersectSegmentAabb(a8::Vec2 p0, a8::Vec2 p1, a8::Vec2 _min, a8::Vec2 _max)
|
|
{
|
|
a8::Vec2 c = (_min + _max) * 0.5f;
|
|
a8::Vec2 e = _max - c;
|
|
a8::Vec2 m = (p0 + p1) * 0.5f;
|
|
a8::Vec2 d = p1 - m;
|
|
m = m - c;
|
|
|
|
float adx = std::abs(d.x);
|
|
if (std::abs(m.x) > e.x + adx) {
|
|
return false;
|
|
}
|
|
float ady = std::abs(d.y);
|
|
if (std::abs(m.y) > e.y + ady) {
|
|
return false;
|
|
}
|
|
if (p0.x > p1.x) {
|
|
float tmp = p0.x;
|
|
p0.x = p1.x;
|
|
p1.x = tmp;
|
|
}
|
|
if (p0.y > p1.y) {
|
|
float tmp = p0.y;
|
|
p0.y = p1.y;
|
|
p1.y = tmp;
|
|
}
|
|
return IntersectAabbAabb(p0, p1, _min, _max);
|
|
}
|
|
|
|
bool IntersectAabbCircle(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_pos, float b_rad)
|
|
{
|
|
if (b_pos.x >= a_min.x && b_pos.x <= a_max.x &&
|
|
b_pos.y >= a_min.y && b_pos.y <= a_max.y) {
|
|
return true;
|
|
}
|
|
a8::Vec2 nearest_point(a8::Clamp(b_pos.x, a_min.x, a_max.x),
|
|
a8::Clamp(b_pos.y, a_min.y, a_max.y));
|
|
a8::Vec2 i = b_pos - nearest_point;
|
|
float n = a8::LengthSqr(i);
|
|
return n < b_rad*b_rad;
|
|
}
|
|
|
|
bool IntersectAabbAabb(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_min, a8::Vec2 b_max)
|
|
{
|
|
a8::Vec2 a_v = (a_max - a_min) * 0.5f;
|
|
a8::Vec2 a_center = a_min + a_v;
|
|
a8::Vec2 b_v = (b_max - b_min) * 0.5f;
|
|
a8::Vec2 b_center = b_min + b_v;
|
|
a8::Vec2 z = b_center - a_center;
|
|
float u1 = a_v.x + b_v.x - std::abs(z.x);
|
|
float u2 = a_v.y + b_v.y - std::abs(z.y);
|
|
return u1 > 0.000001f && u2 > 0.000001;
|
|
}
|
|
|
|
bool IntersectAabbObb(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_pos, float rotate)
|
|
{
|
|
struct Segment
|
|
{
|
|
a8::Vec2 point1;
|
|
a8::Vec2 point2;
|
|
a8::Vec2 dir;
|
|
};
|
|
std::vector<Segment> a_edges;
|
|
std::vector<Segment> b_edges;
|
|
for (int i = 0; i < 3; ++i) {
|
|
Segment seg;
|
|
switch (i) {
|
|
case 0:
|
|
{
|
|
seg.point1 = a_min;
|
|
seg.point2 = a_min + a8::Vec2(a_max.x - a_min.x, 0);
|
|
seg.dir = seg.point2 - seg.point1;
|
|
}
|
|
break;
|
|
case 1:
|
|
{
|
|
seg.point1 = a_min + a8::Vec2(a_max.x - a_min.x, 0);
|
|
seg.point2 = a_max;
|
|
seg.dir = seg.point2 - seg.point1;
|
|
}
|
|
break;
|
|
case 2:
|
|
{
|
|
seg.point1 = a_max;
|
|
seg.point2 = a_min + a8::Vec2(0, a_max.y - a_min.y);
|
|
seg.dir = seg.point2 - seg.point1;
|
|
}
|
|
break;
|
|
case 3:
|
|
{
|
|
seg.point1 = a_min + a8::Vec2(0, a_max.y - a_min.y);
|
|
seg.point2 = a_min;
|
|
seg.dir = seg.point2 - seg.point1;
|
|
}
|
|
break;
|
|
default:
|
|
{
|
|
abort();
|
|
}
|
|
}
|
|
a_edges.push_back(seg);
|
|
}
|
|
for (int i = 0; i < 3; ++i) {
|
|
Segment seg;
|
|
switch (i) {
|
|
case 0:
|
|
{
|
|
seg.point1 = a_min;
|
|
seg.point2 = a_min + a8::Vec2(a_max.x - a_min.x, 0);
|
|
seg.dir = seg.point2 - seg.point1;
|
|
}
|
|
break;
|
|
case 1:
|
|
{
|
|
seg.point1 = a_min + a8::Vec2(a_max.x - a_min.x, 0);
|
|
seg.point2 = a_max;
|
|
seg.dir = seg.point2 - seg.point1;
|
|
}
|
|
break;
|
|
case 2:
|
|
{
|
|
seg.point1 = a_max;
|
|
seg.point2 = a_min + a8::Vec2(0, a_max.y - a_min.y);
|
|
seg.dir = seg.point2 - seg.point1;
|
|
}
|
|
break;
|
|
case 3:
|
|
{
|
|
seg.point1 = a_min + a8::Vec2(0, a_max.y - a_min.y);
|
|
seg.point2 = a_min;
|
|
seg.dir = seg.point2 - seg.point1;
|
|
}
|
|
break;
|
|
default:
|
|
{
|
|
abort();
|
|
}
|
|
}
|
|
b_edges.push_back(seg);
|
|
}
|
|
#if 0
|
|
auto GetProjectionWithAxis =
|
|
[] (std::vector<a8::Vec2> vertices, a8::Vec2 axis) -> a8::Vec2 {
|
|
axis.Normalize();
|
|
float _min = vertices[0].Dot(axis);
|
|
float _max = _min;
|
|
for (a8::Vec2& v : vertices) {
|
|
|
|
}
|
|
};
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
bool IntersectAabbArc(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_pos, float width)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
bool IntersectCircleArc(a8::Vec2 a_pos,
|
|
float a_rad,
|
|
a8::Vec2 b_pos,
|
|
float b_rad,
|
|
float b_angle,
|
|
float b_width,
|
|
float b_height)
|
|
{
|
|
float distance = b_pos.Distance(a_pos);
|
|
float _min = b_rad - b_height / 2 - a_rad;
|
|
float _max = b_rad + b_height / 2 + a_rad;
|
|
if (distance < _min) {
|
|
return false;
|
|
}
|
|
if (distance > _max) {
|
|
return false;
|
|
}
|
|
//float angle = (a_pos - b_pos).CalcAngleEx(a8::Vec2::UP);
|
|
return false;
|
|
}
|
|
|
|
bool IntersectCircleCircle(a8::Vec2 a_pos, float a_rad, a8::Vec2 b_pos, float b_rad)
|
|
{
|
|
float t = a_rad + b_rad;
|
|
a8::Vec2 d = b_pos - a_pos;
|
|
float n = a8::LengthSqr(d);
|
|
return n < t*t;
|
|
}
|
|
|
|
bool IntersectSectorCircle(a8::Vec2 a_pos, float angle, float a_rad, a8::Vec2 b_pos, float b_rad)
|
|
{
|
|
{
|
|
a8::Vec2 d = b_pos - a_pos;
|
|
float n = a8::LengthSqr(d);
|
|
if (n < a_rad || n < b_rad) {
|
|
return true;
|
|
}
|
|
if (n > a_rad + b_rad) {
|
|
return false;
|
|
}
|
|
}
|
|
{
|
|
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool IntersectSectorAabb(a8::Vec2 a_pos, float angle, float a_rad, a8::Vec2 b_min, a8::Vec2 b_max)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
bool CircleContainCircle(glm::vec2 a_pos, float a_rad, glm::vec2 b_pos, float b_rad)
|
|
{
|
|
float distance = glm::length(a_pos - b_pos);
|
|
return distance < a_rad - b_rad;
|
|
}
|
|
|
|
bool CalcCircleAabbSafePoint(a8::Vec2 a_pos, float b_rad, a8::Vec2 b_min, a8::Vec2 b_max,
|
|
a8::Vec2& new_pos)
|
|
{
|
|
new_pos = a_pos;
|
|
bool at_left = std::abs(a_pos.x - b_min.x) < std::abs(a_pos.x - b_max.x);
|
|
bool at_down = std::abs(a_pos.y - b_min.y) < std::abs(a_pos.y - b_max.y);
|
|
float x_len = at_left ? std::abs(a_pos.x - b_min.x) : std::abs(a_pos.x - b_max.x);
|
|
float y_len = at_down ? std::abs(a_pos.y - b_min.y) : std::abs(a_pos.y - b_max.y);
|
|
if (at_left) {
|
|
if (x_len < y_len) {
|
|
//左
|
|
new_pos.x = b_min.x - b_rad - 1;
|
|
} else {
|
|
if (at_down) {
|
|
new_pos.y = b_min.y - b_rad - 1;
|
|
} else {
|
|
new_pos.y = b_max.y + b_rad + 1;
|
|
}
|
|
}
|
|
} else {
|
|
if (x_len < y_len) {
|
|
//右
|
|
new_pos.x = b_max.x + b_rad + 1;
|
|
} else {
|
|
if (at_down) {
|
|
new_pos.y = b_min.y - b_rad - 1;
|
|
} else {
|
|
new_pos.y = b_max.y + b_rad + 1;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool CalcCircleCircleSafePoint(a8::Vec2 a_pos, float a_rad, a8::Vec2 b_pos, float b_rad,
|
|
a8::Vec2& new_pos)
|
|
{
|
|
a8::Vec2 dir = a_pos - b_pos;
|
|
dir.Normalize();
|
|
new_pos = b_pos + dir*(a_rad + b_rad) + 1;
|
|
return true;
|
|
}
|
|
|
|
bool CalcAabbAabbSafePoint(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_min, a8::Vec2 b_max,
|
|
a8::Vec2& new_pos)
|
|
{
|
|
a8::Vec2 a_pos = a_min + (a_max - a_min)/2.0f;
|
|
new_pos = a_pos;
|
|
bool at_left = std::abs(a_pos.x - b_min.x) < std::abs(a_pos.x - b_max.x);
|
|
bool at_down = std::abs(a_pos.y - b_min.y) < std::abs(a_pos.y - b_max.y);
|
|
float x_len = at_left ? std::abs(a_pos.x - b_min.x) : std::abs(a_pos.x - b_max.x);
|
|
float y_len = at_down ? std::abs(a_pos.y - b_min.y) : std::abs(a_pos.y - b_max.y);
|
|
if (at_left) {
|
|
if (x_len < y_len) {
|
|
//左
|
|
new_pos.x = b_min.x - (a_max.x - a_min.x)/2.0f - 1;
|
|
} else {
|
|
if (at_down) {
|
|
new_pos.y = b_min.y - (a_max.y - a_min.y)/2.0f - 1;
|
|
} else {
|
|
new_pos.y = b_max.y + (a_max.y - a_min.y)/2.0f + 1;
|
|
}
|
|
}
|
|
} else {
|
|
if (x_len < y_len) {
|
|
//右
|
|
new_pos.x = b_max.x + (a_max.x - a_min.x)/2.0f + 1;
|
|
} else {
|
|
if (at_down) {
|
|
new_pos.y = b_min.y - (a_max.y - a_min.y)/2.0f - 1;
|
|
} else {
|
|
new_pos.y = b_max.y + (a_max.y - a_min.y)/2.0f + 1;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool CalcAabbCircleSafePoint(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_pos, float b_rad,
|
|
a8::Vec2& new_pos)
|
|
{
|
|
a8::Vec2 a_pos = a_min + (a_max - a_min)/2.0f;
|
|
new_pos = a_pos;
|
|
bool at_left = std::abs(a_min.x - b_pos.x) < std::abs(a_max.x - b_pos.x);
|
|
bool at_down = std::abs(a_min.y - b_pos.y) < std::abs(a_max.y - b_pos.y);
|
|
float x_len = at_left ? std::abs(a_min.x - b_pos.x) : std::abs(a_max.x - b_pos.x);
|
|
float y_len = at_down ? std::abs(a_min.y - b_pos.y) : std::abs(a_max.y - b_pos.y);
|
|
if (at_left) {
|
|
if (x_len < y_len) {
|
|
//左
|
|
new_pos.x = b_pos.x - (a_max.x - a_min.x)/2.0f - 1;
|
|
} else {
|
|
if (at_down) {
|
|
new_pos.y = b_pos.y - (a_max.y - a_min.y)/2.0f - 1;
|
|
} else {
|
|
new_pos.y = b_pos.y + (a_max.y - a_min.y)/2.0f + 1;
|
|
}
|
|
}
|
|
} else {
|
|
if (x_len < y_len) {
|
|
//右
|
|
new_pos.x = b_pos.x + (a_max.x - a_min.x)/2.0f + 1;
|
|
} else {
|
|
if (at_down) {
|
|
new_pos.y = b_pos.y - (a_max.y - a_min.y)/2.0f - 1;
|
|
} else {
|
|
new_pos.y = b_pos.y + (a_max.y - a_min.y)/2.0f + 1;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool IntersectCylinderCylinder(const glm::vec3& a_pos, float a_rad, float a_height,
|
|
const glm::vec3& b_pos, float b_rad, float b_height)
|
|
{
|
|
glm::vec2 v = glm::vec2(b_pos.x, b_pos.z) - glm::vec2(a_pos.x, a_pos.z);
|
|
float n = glm::length(v);
|
|
if (n > a_rad + b_rad) {
|
|
return false;
|
|
}
|
|
return (a_pos.y - b_pos.y) < (a_height + b_height) / 2.0f;
|
|
}
|
|
|
|
}
|