550 lines
24 KiB
C++
550 lines
24 KiB
C++
#pragma once
|
|
#include "Helpers/MathExtras.hpp"
|
|
#include <Eigen/Dense>
|
|
#include <array>
|
|
#include <limits>
|
|
#include <iostream>
|
|
#include <ostream>
|
|
#include <Eigen/Geometry>
|
|
|
|
using namespace Eigen;
|
|
|
|
namespace SCollision {
|
|
template <typename T> class AABB {
|
|
public:
|
|
typedef T UnderlyingType;
|
|
|
|
AABB() = default; // NOTE: Should not use the min and max numeric limits to initialize, since that wont work for WorldScalar
|
|
AABB(const Vector<T, 2>& initMin, const Vector<T, 2>& initMax):
|
|
min(initMin),
|
|
max(initMax)
|
|
{}
|
|
Vector<T, 2> min;
|
|
Vector<T, 2> max;
|
|
Vector<T, 2> center() const { return (min + max) / T(2); }
|
|
Vector<T, 2> bottom_left() const { return Vector<T, 2>{min.x(), max.y()}; }
|
|
Vector<T, 2> top_right() const { return Vector<T, 2>{max.x(), min.y()}; }
|
|
Vector<T, 2> dim() const { return max - min; }
|
|
T width() const { return max.x() - min.x(); }
|
|
T height() const { return max.y() - min.y(); }
|
|
// Do not try to get intersection between AABBs unless you know they collide
|
|
AABB<T> get_intersection_between_aabbs(const AABB<T>& other) const {
|
|
return {
|
|
cwise_vec_max(other.min, min),
|
|
cwise_vec_min(other.max, max)
|
|
};
|
|
}
|
|
void include_aabb_in_bounds(const AABB<T>& other) {
|
|
min = cwise_vec_min(other.min, min);
|
|
max = cwise_vec_max(other.max, max);
|
|
}
|
|
void include_point_in_bounds(const Vector<T, 2>& p) {
|
|
min = cwise_vec_min(p, min);
|
|
max = cwise_vec_max(p, max);
|
|
}
|
|
bool operator==(const AABB<T>& other) const {
|
|
return min == other.min && max == other.max;
|
|
}
|
|
bool fully_contains_aabb(const AABB<T>& other) const {
|
|
return other.min.x() >= min.x() && other.min.y() >= min.y() && other.max.x() <= max.x() && other.max.y() <= max.y();
|
|
}
|
|
Vector<T, 2> clamp_point_to_aabb(const Vector<T, 2>& p) const {
|
|
return {
|
|
std::clamp(p.x(), min.x(), max.x()),
|
|
std::clamp(p.y(), min.y(), max.y())
|
|
};
|
|
}
|
|
template <typename S> AABB<S> transform(const std::function<Vector<S, 2>(const Vector<T, 2>&)>& transformFunc, const std::function<S(const T&)>& transformLinearFunc) const {
|
|
Vector<S, 2> aabb1 = transformFunc(min);
|
|
Vector<S, 2> aabb2 = transformFunc(max);
|
|
Vector<S, 2> aabb3 = transformFunc(top_right());
|
|
Vector<S, 2> aabb4 = transformFunc(bottom_left());
|
|
|
|
AABB<S> toRet{aabb1, aabb1};
|
|
|
|
toRet.include_point_in_bounds(aabb1);
|
|
toRet.include_point_in_bounds(aabb2);
|
|
toRet.include_point_in_bounds(aabb3);
|
|
toRet.include_point_in_bounds(aabb4);
|
|
|
|
return toRet;
|
|
}
|
|
template <typename S> AABB<S> cast() {
|
|
AABB<S> toRet{min.template cast<S>(), max.template cast<S>()};
|
|
return toRet;
|
|
}
|
|
template <class Archive> void serialize(Archive& a) {
|
|
a(min, max);
|
|
}
|
|
};
|
|
template <typename T> class Circle {
|
|
public:
|
|
typedef T UnderlyingType;
|
|
|
|
Circle() = default;
|
|
Circle(const Vector<T, 2>& initPos, T initRadius):
|
|
pos(initPos),
|
|
radius(initRadius),
|
|
bounds(pos - Vector<T, 2>{radius, radius}, pos + Vector<T, 2>{radius, radius})
|
|
{}
|
|
Vector<T, 2> pos;
|
|
T radius;
|
|
AABB<T> bounds;
|
|
template <typename S> Circle<S> transform(const std::function<Vector<S, 2>(const Vector<T, 2>&)>& transformFunc, const std::function<S(const T&)>& transformLinearFunc) const {
|
|
Circle<S> newCirc = Circle<S>();
|
|
newCirc.pos = transformFunc(pos);
|
|
newCirc.radius = transformLinearFunc(radius);
|
|
newCirc.bounds = AABB<S>(newCirc.pos - Vector<S, 2>{newCirc.radius, newCirc.radius}, newCirc.pos + Vector<S, 2>{newCirc.radius, newCirc.radius});
|
|
return newCirc;
|
|
}
|
|
template <typename S> Circle<S> cast() {
|
|
Circle<S> toRet;
|
|
toRet.pos = pos.template cast<S>();
|
|
toRet.radius = static_cast<S>(radius);
|
|
toRet.bounds = bounds.template cast<S>();
|
|
return toRet;
|
|
}
|
|
};
|
|
template <typename T> class Triangle {
|
|
public:
|
|
typedef T UnderlyingType;
|
|
|
|
Triangle() = default;
|
|
Triangle(const std::array<Vector<T, 2>, 3>& initP):
|
|
p(initP),
|
|
bounds(cwise_vec_min(cwise_vec_min(p[0], p[1]), p[2]), cwise_vec_max(cwise_vec_max(p[0], p[1]), p[2]))
|
|
{}
|
|
Triangle(const Vector<T, 2>& p1, const Vector<T, 2>& p2, const Vector<T, 2>& p3):
|
|
p({p1, p2, p3}),
|
|
bounds(cwise_vec_min(cwise_vec_min(p[0], p[1]), p[2]), cwise_vec_max(cwise_vec_max(p[0], p[1]), p[2]))
|
|
{}
|
|
template <typename S> Triangle<S> transform(const std::function<Vector<S, 2>(const Vector<T, 2>&)>& transformFunc, const std::function<S(const T&)>& transformLinearFunc) const {
|
|
Triangle<S> toRet = Triangle<S>();
|
|
toRet.p[0] = transformFunc(p[0]);
|
|
toRet.p[1] = transformFunc(p[1]);
|
|
toRet.p[2] = transformFunc(p[2]);
|
|
toRet.bounds = AABB<S>(cwise_vec_min(cwise_vec_min(toRet.p[0], toRet.p[1]), toRet.p[2]), cwise_vec_max(cwise_vec_max(toRet.p[0], toRet.p[1]), toRet.p[2]));
|
|
return toRet;
|
|
}
|
|
template <typename S> Triangle<S> cast() {
|
|
Triangle<S> toRet;
|
|
toRet.p[0] = p[0].template cast<S>();
|
|
toRet.p[1] = p[1].template cast<S>();
|
|
toRet.p[2] = p[2].template cast<S>();
|
|
toRet.bounds = bounds.template cast<S>();
|
|
return toRet;
|
|
}
|
|
std::array<Vector<T, 2>, 3> p;
|
|
AABB<T> bounds;
|
|
};
|
|
|
|
template <typename T> struct ColliderCollection {
|
|
typedef T UnderlyingType;
|
|
|
|
AABB<T> bounds;
|
|
std::vector<AABB<T>> aabb;
|
|
std::vector<Circle<T>> circle;
|
|
std::vector<Triangle<T>> triangle;
|
|
|
|
void clear() {
|
|
aabb.clear();
|
|
circle.clear();
|
|
triangle.clear();
|
|
bounds = AABB<T>({0, 0}, {0, 0});
|
|
}
|
|
size_t size() const { return aabb.size() + circle.size() + triangle.size(); }
|
|
bool empty() const { return aabb.empty() && circle.empty() && triangle.empty(); }
|
|
template <typename ColliderList> void bound_limit(const ColliderList& list) {
|
|
for(auto& o : list)
|
|
bounds.include_aabb_in_bounds(get_bounds(o));
|
|
}
|
|
void recalculate_bounds() {
|
|
bounds = AABB<T>();
|
|
if(!aabb.empty())
|
|
bounds = get_bounds(aabb.front());
|
|
else if(!circle.empty())
|
|
bounds = get_bounds(circle.front());
|
|
else if(!triangle.empty())
|
|
bounds = get_bounds(triangle.front());
|
|
bound_limit(aabb);
|
|
bound_limit(circle);
|
|
bound_limit(triangle);
|
|
}
|
|
template <typename S> ColliderCollection<S> transform(const std::function<Vector<S, 2>(const Vector<T, 2>&)>& transformFunc, const std::function<S(const T&)>& transformLinearFunc) const {
|
|
ColliderCollection<S> toRet;
|
|
for(auto& o : aabb)
|
|
toRet.aabb.emplace_back(o.transform(transformFunc, transformLinearFunc));
|
|
for(auto& o : circle)
|
|
toRet.circle.emplace_back(o.transform(transformFunc, transformLinearFunc));
|
|
for(auto& o : triangle)
|
|
toRet.triangle.emplace_back(o.transform(transformFunc, transformLinearFunc));
|
|
toRet.recalculate_bounds();
|
|
return toRet;
|
|
}
|
|
};
|
|
|
|
template <typename T> std::ostream& operator<<(std::ostream& os, const AABB<T>& a) {
|
|
return os << "min: [" << a.min.x() << " " << a.min.y() << "]\n"
|
|
<< "max: [" << a.max.x() << " " << a.max.y() << "]\n";
|
|
}
|
|
|
|
template <typename T> const AABB<T>& get_bounds(const Circle<T>& c) { return c.bounds; }
|
|
template <typename T> const AABB<T>& get_bounds(const Triangle<T>& c) { return c.bounds; }
|
|
template <typename T> const AABB<T>& get_bounds(const AABB<T>& a) { return a; }
|
|
template <typename T> const AABB<T>& get_bounds(const ColliderCollection<T>& c) { return c.bounds; }
|
|
|
|
template<typename T> void generate_wide_line(ColliderCollection<T>& collection, const Vector<T, 2>& p1, const Vector<T, 2>& p2, T width, bool circleCaps) {
|
|
if(p1 != p2) {
|
|
Vector<T, 2> dir = p1 - p2;
|
|
Vector<T, 2> perpVec = perpendicular_vec2(dir);
|
|
perpVec.normalize();
|
|
Vector<T, 2> perpWidth = (perpVec * width * T(0.5)).eval();
|
|
collection.triangle.emplace_back(Triangle<T>{p1 + perpWidth, p1 - perpWidth, p2 + perpWidth});
|
|
collection.triangle.emplace_back(Triangle<T>{p2 + perpWidth, p2 - perpWidth, p1 - perpWidth});
|
|
}
|
|
if(circleCaps) {
|
|
collection.circle.emplace_back(Circle<T>{p1, width * T(0.5)});
|
|
if(p1 != p2)
|
|
collection.circle.emplace_back(Circle<T>{p2, width * T(0.5)});
|
|
}
|
|
collection.recalculate_bounds();
|
|
}
|
|
|
|
template <typename T> Vector<T, 2> two_dim_vec_slerp(const Vector<T, 2>& v1, const Vector<T, 2>& v2) {
|
|
Vector<T, 2> v1Norm = v1.normalized();
|
|
Vector<T, 2> v2Norm = v2.normalized();
|
|
Vector<T, 2> avg = (v1Norm + v2Norm) * 0.5;
|
|
if(std::abs(avg.x()) < 0.00001 && std::abs(avg.y()) < 0.00001)
|
|
return perpendicular_vec2(v1Norm).normalized();
|
|
else
|
|
return avg.normalized();
|
|
}
|
|
|
|
template <typename T> void generate_polyline(ColliderCollection<T>& collection, const std::vector<Vector<T, 2>>& points, T width, bool closePath) {
|
|
if(points.size() < 2) {
|
|
return;
|
|
}
|
|
|
|
Vector<T, 2> perpPrev = perpendicular_vec2((points[1] - points.front()).eval());
|
|
perpPrev.normalize();
|
|
|
|
std::array<Vector<T, 2>, 3> vertArray;
|
|
unsigned newIndex = 0;
|
|
|
|
if(!closePath || points.size() == 2) {
|
|
vertArray[0] = points.front() + perpPrev * width * 0.5;
|
|
vertArray[1] = points.front() - perpPrev * width * 0.5;
|
|
newIndex = 2;
|
|
}
|
|
|
|
if(points.size() == 2) {
|
|
vertArray[newIndex] = points.back() + perpPrev * width * 0.5;
|
|
collection.triangle.emplace_back(Triangle<T>{vertArray});
|
|
newIndex = (newIndex + 1) % 3;
|
|
vertArray[newIndex] = points.back() - perpPrev * width * 0.5;
|
|
collection.triangle.emplace_back(Triangle<T>{vertArray});
|
|
return;
|
|
}
|
|
|
|
for(size_t x = 0; x <= points.size(); x++) {
|
|
size_t i = x;
|
|
size_t iP1 = x + 1;
|
|
size_t iM1 = x - 1;
|
|
if(x == 0) {
|
|
if(!closePath)
|
|
continue;
|
|
else
|
|
iM1 = points.size() - 1;
|
|
}
|
|
else if(x == points.size() - 1) {
|
|
if(!closePath)
|
|
continue;
|
|
else {
|
|
iP1 = 0;
|
|
}
|
|
}
|
|
else if(x == points.size()) {
|
|
if(!closePath)
|
|
continue;
|
|
else {
|
|
i = 0;
|
|
iM1 = points.size() - 1;
|
|
iP1 = 1;
|
|
}
|
|
}
|
|
Vector<T, 2> v1Norm = (points[iM1] - points[i]).normalized();
|
|
Vector<T, 2> v2Norm = (points[iP1] - points[i]).normalized();
|
|
Vector<T, 2> perp = two_dim_vec_slerp(v1Norm, v2Norm);
|
|
if(perp.dot(perpPrev) <= 0.0)
|
|
perp = -perp;
|
|
|
|
T angle1 = std::acos((perp.dot(v1Norm) < 0 ? -perp : perp).dot(v1Norm));
|
|
T angle2 = std::acos((perp.dot(v2Norm) < 0 ? -perp : perp).dot(v2Norm));
|
|
T sinAngle1 = std::sin(angle1 + angle2);
|
|
T alteredWidth = std::lerp(0.5, 0.707, sinAngle1);
|
|
|
|
vertArray[newIndex] = points[i] + perp * alteredWidth * width;
|
|
newIndex = (newIndex + 1) % 3;
|
|
if(x != 0)
|
|
collection.triangle.emplace_back(Triangle<T>{vertArray});
|
|
vertArray[newIndex] = points[i] - perp * alteredWidth * width;
|
|
newIndex = (newIndex + 1) % 3;
|
|
if(x != 0)
|
|
collection.triangle.emplace_back(Triangle<T>{vertArray});
|
|
|
|
perpPrev = perp;
|
|
}
|
|
|
|
if(!closePath) {
|
|
Vector<T, 2> perp = perpendicular_vec2((points[points.size() - 2] - points.back()).eval());
|
|
perp.normalize();
|
|
if(perp.dot(perpPrev) <= 0.0)
|
|
perp = -perp;
|
|
vertArray[newIndex] = points.back() + perp * width * 0.5;
|
|
newIndex = (newIndex + 1) % 3;
|
|
collection.triangle.emplace_back(Triangle<T>{vertArray});
|
|
vertArray[newIndex] = points.back() - perp * width * 0.5;
|
|
newIndex = (newIndex + 1) % 3;
|
|
collection.triangle.emplace_back(Triangle<T>{vertArray});
|
|
}
|
|
}
|
|
|
|
// https://blackpawn.com/texts/pointinpoly/default.html
|
|
template <typename T> bool collide(const Vector<T, 2>& p, const Triangle<T>& t) {
|
|
Vector<T, 2> v0 = t.p[2] - t.p[0];
|
|
Vector<T, 2> v1 = t.p[1] - t.p[0];
|
|
Vector<T, 2> v2 = p - t.p[0];
|
|
|
|
T dot00 = v0.dot(v0);
|
|
T dot01 = v0.dot(v1);
|
|
T dot02 = v0.dot(v2);
|
|
T dot11 = v1.dot(v1);
|
|
T dot12 = v1.dot(v2);
|
|
|
|
T invDenom = T(1) / (dot00 * dot11 - dot01 * dot01);
|
|
T u = (dot11 * dot02 - dot01 * dot12) * invDenom;
|
|
T v = (dot00 * dot12 - dot01 * dot02) * invDenom;
|
|
|
|
return (u >= T(0)) && (v >= T(0)) && (u + v < T(1));
|
|
}
|
|
|
|
template <typename T> bool collide(const Triangle<T>& t, const Vector<T, 2>& p) {
|
|
return collide(p, t);
|
|
}
|
|
template <typename T> bool collide(const Vector<T, 2>& p, const AABB<T>& a) {
|
|
return (p.x() > a.min.x() && p.x() < a.max.x() && p.y() > a.min.y() && p.y() < a.max.y());
|
|
}
|
|
template <typename T> bool collide(const AABB<T>& a, const Vector<T, 2>& p) {
|
|
return collide(p, a);
|
|
}
|
|
template <typename T> bool collide(const Vector<T, 2>& p, const Circle<T>& a) {
|
|
return vec_distance(p, a.pos) <= a.radius;
|
|
}
|
|
template <typename T> bool collide(const Circle<T>& a, const Vector<T, 2>& p) {
|
|
return collide(p, a);
|
|
}
|
|
|
|
template <typename T> bool collide(const Circle<T>& c1, const Circle<T>& c2) {
|
|
return vec_distance(c1.pos, c2.pos) <= (c1.radius + c2.radius);
|
|
}
|
|
|
|
// https://gamedev.stackexchange.com/a/178154
|
|
template <typename T> bool collide(const Circle<T>& c, const AABB<T>& a) {
|
|
if(!collide(get_bounds(c), a))
|
|
return false;
|
|
Vector<T, 2> aabbCenter = a.center();
|
|
Vector<T, 2> aabbBounds = aabbCenter - a.min;
|
|
Vector<T, 2> distance = c.pos - aabbCenter;
|
|
Vector<T, 2> clampDst = cwise_vec_clamp(distance, (-aabbBounds).eval(), aabbBounds);
|
|
Vector<T, 2> closestPoint = aabbCenter + clampDst;
|
|
return vec_distance(closestPoint, c.pos) <= c.radius;
|
|
}
|
|
template <typename T> bool collide(const AABB<T>& a, const Circle<T>& c) {
|
|
return collide(c, a);
|
|
}
|
|
|
|
template <typename T> bool collide(const AABB<T>& a1, const AABB<T>& a2) {
|
|
return a1.min.x() < a2.max.x() && a1.max.x() > a2.min.x() &&
|
|
a1.min.y() < a2.max.y() && a1.max.y() > a2.min.y();
|
|
}
|
|
|
|
template <typename T> bool collide(const Circle<T>& c, const Triangle<T>& t) {
|
|
if(!collide(get_bounds(c), get_bounds(t)))
|
|
return false;
|
|
return collide(t.p[0], c) ||
|
|
collide(t.p[1], c) ||
|
|
collide(t.p[2], c) ||
|
|
collide(c.pos, t) ||
|
|
collision_circle_line_segment(c.pos, c.radius, t.p[0], t.p[1]) ||
|
|
collision_circle_line_segment(c.pos, c.radius, t.p[1], t.p[2]) ||
|
|
collision_circle_line_segment(c.pos, c.radius, t.p[2], t.p[0]);
|
|
}
|
|
template <typename T> bool collide(const Triangle<T>& t, const Circle<T>& c) {
|
|
return collide(c, t);
|
|
}
|
|
|
|
template <typename T> bool collide(const AABB<T>& a, const Triangle<T>& t) {
|
|
if(!collide(a, get_bounds(t)))
|
|
return false;
|
|
return collide(t.p[0], a) ||
|
|
collide(t.p[1], a) ||
|
|
collide(t.p[2], a) ||
|
|
collide(a.min, t) ||
|
|
collide(a.max, t) ||
|
|
collide(a.bottom_left(), t) ||
|
|
collide(a.top_right(), t) ||
|
|
is_collision_line_segment_line_segment(t.p[0], t.p[1], a.min, a.top_right()) ||
|
|
is_collision_line_segment_line_segment(t.p[0], t.p[1], a.min, a.bottom_left()) ||
|
|
is_collision_line_segment_line_segment(t.p[0], t.p[1], a.max, a.top_right()) ||
|
|
is_collision_line_segment_line_segment(t.p[0], t.p[1], a.max, a.bottom_left()) ||
|
|
is_collision_line_segment_line_segment(t.p[0], t.p[2], a.min, a.top_right()) ||
|
|
is_collision_line_segment_line_segment(t.p[0], t.p[2], a.min, a.bottom_left()) ||
|
|
is_collision_line_segment_line_segment(t.p[0], t.p[2], a.max, a.top_right()) ||
|
|
is_collision_line_segment_line_segment(t.p[0], t.p[2], a.max, a.bottom_left()) ||
|
|
is_collision_line_segment_line_segment(t.p[2], t.p[1], a.min, a.top_right()) ||
|
|
is_collision_line_segment_line_segment(t.p[2], t.p[1], a.min, a.bottom_left()) ||
|
|
is_collision_line_segment_line_segment(t.p[2], t.p[1], a.max, a.top_right()) ||
|
|
is_collision_line_segment_line_segment(t.p[2], t.p[1], a.max, a.bottom_left());
|
|
}
|
|
template <typename T> bool collide(const Triangle<T>& t, const AABB<T>& a) {
|
|
return collide(a, t);
|
|
}
|
|
|
|
template <typename T> bool collide(const Triangle<T>& t1, const Triangle<T>& t2) {
|
|
//return collide(get_bounds(t1), get_bounds(t2));
|
|
if(!collide(get_bounds(t1), get_bounds(t2)))
|
|
return false;
|
|
return collide(t1.p[0], t2) ||
|
|
collide(t1.p[1], t2) ||
|
|
collide(t1.p[2], t2) ||
|
|
collide(t2.p[0], t1) ||
|
|
collide(t2.p[1], t1) ||
|
|
collide(t2.p[2], t1) ||
|
|
is_collision_line_segment_line_segment(t1.p[0], t1.p[1], t2.p[0], t2.p[1]) ||
|
|
is_collision_line_segment_line_segment(t1.p[0], t1.p[1], t2.p[1], t2.p[2]) ||
|
|
is_collision_line_segment_line_segment(t1.p[0], t1.p[1], t2.p[2], t2.p[0]) ||
|
|
is_collision_line_segment_line_segment(t1.p[0], t1.p[2], t2.p[0], t2.p[1]) ||
|
|
is_collision_line_segment_line_segment(t1.p[0], t1.p[2], t2.p[1], t2.p[2]) ||
|
|
is_collision_line_segment_line_segment(t1.p[0], t1.p[2], t2.p[2], t2.p[0]) ||
|
|
is_collision_line_segment_line_segment(t1.p[2], t1.p[1], t2.p[0], t2.p[1]) ||
|
|
is_collision_line_segment_line_segment(t1.p[2], t1.p[1], t2.p[1], t2.p[2]) ||
|
|
is_collision_line_segment_line_segment(t1.p[2], t1.p[1], t2.p[2], t2.p[0]);
|
|
}
|
|
|
|
template <typename Collection, typename Collider> bool collide(const Collection& collection, const Collider& c) {
|
|
for(auto& o : collection.circle)
|
|
if(collide(c, o))
|
|
return true;
|
|
for(auto& o : collection.aabb)
|
|
if(collide(c, o))
|
|
return true;
|
|
for(auto& o : collection.triangle)
|
|
if(collide(c, o))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
template <typename T> class BVHContainer {
|
|
public:
|
|
typedef T UnderlyingType;
|
|
|
|
std::vector<BVHContainer<T>> children;
|
|
ColliderCollection<T> objects;
|
|
|
|
// NOTE: BVH TRANSFORMATION LEADS TO A BROKEN BVH
|
|
//void transform(const std::function<Vector<T, 2>(const Vector<T, 2>&)>& transformFunc, const std::function<T(const T&)>& transformLinearFunc) {
|
|
// objects.transform(transformFunc, transformLinearFunc);
|
|
// for(auto& a : children)
|
|
// a.transform(transformFunc, transformLinearFunc);
|
|
//}
|
|
|
|
void clear() {
|
|
children.clear();
|
|
objects.clear();
|
|
}
|
|
|
|
bool empty() const {
|
|
return children.empty() && objects.empty();
|
|
}
|
|
|
|
unsigned assign_quad_to_point(const Vector<T, 2>& point, const Vector<T, 2>& origin) const {
|
|
bool negativeX = point.x() <= origin.x();
|
|
bool negativeY = point.y() <= origin.y();
|
|
|
|
if(negativeX && negativeY)
|
|
return 0;
|
|
else if(negativeX && !negativeY)
|
|
return 1;
|
|
else if(!negativeX && !negativeY)
|
|
return 2;
|
|
else
|
|
return 3;
|
|
}
|
|
|
|
void calculate_bvh_recursive(ColliderCollection<T>& bvhObjects, int maxDepth = -1) {
|
|
bvhObjects.recalculate_bounds();
|
|
|
|
objects.bounds = bvhObjects.bounds;
|
|
|
|
if(bvhObjects.empty()) {
|
|
return;
|
|
}
|
|
|
|
if(bvhObjects.size() <= 2 || maxDepth == 0) {
|
|
objects = bvhObjects;
|
|
return;
|
|
}
|
|
|
|
ColliderCollection<T> parts[4];
|
|
Vector<T, 2> halfwayPoint = bvhObjects.bounds.center();
|
|
|
|
for(auto& o : bvhObjects.aabb)
|
|
parts[assign_quad_to_point(get_bounds(o).center(), halfwayPoint)].aabb.emplace_back(o);
|
|
for(auto& o : bvhObjects.circle)
|
|
parts[assign_quad_to_point(get_bounds(o).center(), halfwayPoint)].circle.emplace_back(o);
|
|
for(auto& o : bvhObjects.triangle)
|
|
parts[assign_quad_to_point(get_bounds(o).center(), halfwayPoint)].triangle.emplace_back(o);
|
|
|
|
for(size_t i = 0; i < 4; i++) {
|
|
if(parts[i].empty())
|
|
continue;
|
|
else if(parts[i].size() == bvhObjects.size()) {
|
|
objects = bvhObjects;
|
|
break;
|
|
}
|
|
else {
|
|
children.emplace_back();
|
|
children.back().calculate_bvh_recursive(parts[i], maxDepth - 1);
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename Collider> bool is_collide(const Collider& collider) const {
|
|
if(!collide(get_bounds(collider), get_bounds(objects)))
|
|
return false;
|
|
if(collide(objects, collider))
|
|
return true;
|
|
for(const auto& c : children)
|
|
if(c.is_collide(collider))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
template <typename BVH> bool is_collide_other_bvh(const BVH& bvh) const {
|
|
if(!collide(get_bounds(objects), get_bounds(bvh.objects)))
|
|
return false;
|
|
if(is_collide(bvh.objects))
|
|
return true;
|
|
if(bvh.is_collide(objects))
|
|
return true;
|
|
for(const auto& c : children) {
|
|
for(const auto& d : bvh.children) {
|
|
if(c.is_collide_other_bvh(d))
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
};
|
|
};
|
|
|