mirror of
https://github.com/godotengine/godot
synced 2024-10-14 11:43:32 +00:00
d95794ec8a
As many open source projects have started doing it, we're removing the current year from the copyright notice, so that we don't need to bump it every year. It seems like only the first year of publication is technically relevant for copyright notices, and even that seems to be something that many companies stopped listing altogether (in a version controlled codebase, the commits are a much better source of date of publication than a hardcoded copyright statement). We also now list Godot Engine contributors first as we're collectively the current maintainers of the project, and we clarify that the "exclusive" copyright of the co-founders covers the timespan before opensourcing (their further contributions are included as part of Godot Engine contributors). Also fixed "cf." Frenchism - it's meant as "refer to / see".
1405 lines
47 KiB
C++
1405 lines
47 KiB
C++
/**************************************************************************/
|
|
/* godot_collision_solver_2d_sat.cpp */
|
|
/**************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* https://godotengine.org */
|
|
/**************************************************************************/
|
|
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
|
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
|
/* */
|
|
/* Permission is hereby granted, free of charge, to any person obtaining */
|
|
/* a copy of this software and associated documentation files (the */
|
|
/* "Software"), to deal in the Software without restriction, including */
|
|
/* without limitation the rights to use, copy, modify, merge, publish, */
|
|
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
|
/* permit persons to whom the Software is furnished to do so, subject to */
|
|
/* the following conditions: */
|
|
/* */
|
|
/* The above copyright notice and this permission notice shall be */
|
|
/* included in all copies or substantial portions of the Software. */
|
|
/* */
|
|
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
|
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
|
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
|
|
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
|
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
|
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
|
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
|
/**************************************************************************/
|
|
|
|
#include "godot_collision_solver_2d_sat.h"
|
|
|
|
#include "core/math/geometry_2d.h"
|
|
|
|
struct _CollectorCallback2D {
|
|
GodotCollisionSolver2D::CallbackResult callback = nullptr;
|
|
void *userdata = nullptr;
|
|
bool swap = false;
|
|
bool collided = false;
|
|
Vector2 normal;
|
|
Vector2 *sep_axis = nullptr;
|
|
|
|
_FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) {
|
|
if (swap) {
|
|
callback(p_point_B, p_point_A, userdata);
|
|
} else {
|
|
callback(p_point_A, p_point_B, userdata);
|
|
}
|
|
}
|
|
};
|
|
|
|
typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *);
|
|
|
|
_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
|
|
#ifdef DEBUG_ENABLED
|
|
ERR_FAIL_COND(p_point_count_A != 1);
|
|
ERR_FAIL_COND(p_point_count_B != 1);
|
|
#endif
|
|
|
|
p_collector->call(*p_points_A, *p_points_B);
|
|
}
|
|
|
|
_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
|
|
#ifdef DEBUG_ENABLED
|
|
ERR_FAIL_COND(p_point_count_A != 1);
|
|
ERR_FAIL_COND(p_point_count_B != 2);
|
|
#endif
|
|
|
|
Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B);
|
|
p_collector->call(*p_points_A, closest_B);
|
|
}
|
|
|
|
struct _generate_contacts_Pair {
|
|
bool a = false;
|
|
int idx = 0;
|
|
real_t d = 0.0;
|
|
_FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; }
|
|
};
|
|
|
|
_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
|
|
#ifdef DEBUG_ENABLED
|
|
ERR_FAIL_COND(p_point_count_A != 2);
|
|
ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix
|
|
#endif
|
|
|
|
Vector2 n = p_collector->normal;
|
|
Vector2 t = n.orthogonal();
|
|
real_t dA = n.dot(p_points_A[0]);
|
|
real_t dB = n.dot(p_points_B[0]);
|
|
|
|
_generate_contacts_Pair dvec[4];
|
|
|
|
dvec[0].d = t.dot(p_points_A[0]);
|
|
dvec[0].a = true;
|
|
dvec[0].idx = 0;
|
|
dvec[1].d = t.dot(p_points_A[1]);
|
|
dvec[1].a = true;
|
|
dvec[1].idx = 1;
|
|
dvec[2].d = t.dot(p_points_B[0]);
|
|
dvec[2].a = false;
|
|
dvec[2].idx = 0;
|
|
dvec[3].d = t.dot(p_points_B[1]);
|
|
dvec[3].a = false;
|
|
dvec[3].idx = 1;
|
|
|
|
SortArray<_generate_contacts_Pair> sa;
|
|
sa.sort(dvec, 4);
|
|
|
|
for (int i = 1; i <= 2; i++) {
|
|
if (dvec[i].a) {
|
|
Vector2 a = p_points_A[dvec[i].idx];
|
|
Vector2 b = n.plane_project(dB, a);
|
|
if (n.dot(a) > n.dot(b) - CMP_EPSILON) {
|
|
continue;
|
|
}
|
|
p_collector->call(a, b);
|
|
} else {
|
|
Vector2 b = p_points_B[dvec[i].idx];
|
|
Vector2 a = n.plane_project(dA, b);
|
|
if (n.dot(a) > n.dot(b) - CMP_EPSILON) {
|
|
continue;
|
|
}
|
|
p_collector->call(a, b);
|
|
}
|
|
}
|
|
}
|
|
|
|
static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
|
|
#ifdef DEBUG_ENABLED
|
|
ERR_FAIL_COND(p_point_count_A < 1);
|
|
ERR_FAIL_COND(p_point_count_B < 1);
|
|
#endif
|
|
|
|
static const GenerateContactsFunc generate_contacts_func_table[2][2] = {
|
|
{
|
|
_generate_contacts_point_point,
|
|
_generate_contacts_point_edge,
|
|
},
|
|
{
|
|
nullptr,
|
|
_generate_contacts_edge_edge,
|
|
}
|
|
};
|
|
|
|
int pointcount_B = 0;
|
|
int pointcount_A = 0;
|
|
const Vector2 *points_A = nullptr;
|
|
const Vector2 *points_B = nullptr;
|
|
|
|
if (p_point_count_A > p_point_count_B) {
|
|
//swap
|
|
p_collector->swap = !p_collector->swap;
|
|
p_collector->normal = -p_collector->normal;
|
|
|
|
pointcount_B = p_point_count_A;
|
|
pointcount_A = p_point_count_B;
|
|
points_A = p_points_B;
|
|
points_B = p_points_A;
|
|
} else {
|
|
pointcount_B = p_point_count_B;
|
|
pointcount_A = p_point_count_A;
|
|
points_A = p_points_A;
|
|
points_B = p_points_B;
|
|
}
|
|
|
|
int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1;
|
|
int version_B = (pointcount_B > 2 ? 2 : pointcount_B) - 1;
|
|
|
|
GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
|
|
ERR_FAIL_COND(!contacts_func);
|
|
contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector);
|
|
}
|
|
|
|
template <class ShapeA, class ShapeB, bool castA = false, bool castB = false, bool withMargin = false>
|
|
class SeparatorAxisTest2D {
|
|
const ShapeA *shape_A = nullptr;
|
|
const ShapeB *shape_B = nullptr;
|
|
const Transform2D *transform_A = nullptr;
|
|
const Transform2D *transform_B = nullptr;
|
|
real_t best_depth = 1e15;
|
|
Vector2 best_axis;
|
|
#ifdef DEBUG_ENABLED
|
|
int best_axis_count = 0;
|
|
int best_axis_index = -1;
|
|
#endif
|
|
Vector2 motion_A;
|
|
Vector2 motion_B;
|
|
real_t margin_A = 0.0;
|
|
real_t margin_B = 0.0;
|
|
_CollectorCallback2D *callback;
|
|
|
|
public:
|
|
_FORCE_INLINE_ bool test_previous_axis() {
|
|
if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) {
|
|
return test_axis(*callback->sep_axis);
|
|
} else {
|
|
#ifdef DEBUG_ENABLED
|
|
best_axis_count++;
|
|
#endif
|
|
}
|
|
return true;
|
|
}
|
|
|
|
_FORCE_INLINE_ bool test_cast() {
|
|
if (castA) {
|
|
Vector2 na = motion_A.normalized();
|
|
if (!test_axis(na)) {
|
|
return false;
|
|
}
|
|
if (!test_axis(na.orthogonal())) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (castB) {
|
|
Vector2 nb = motion_B.normalized();
|
|
if (!test_axis(nb)) {
|
|
return false;
|
|
}
|
|
if (!test_axis(nb.orthogonal())) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
_FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) {
|
|
Vector2 axis = p_axis;
|
|
|
|
if (Math::is_zero_approx(axis.x) &&
|
|
Math::is_zero_approx(axis.y)) {
|
|
// strange case, try an upwards separator
|
|
axis = Vector2(0.0, 1.0);
|
|
}
|
|
|
|
real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0;
|
|
|
|
if (castA) {
|
|
shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A);
|
|
} else {
|
|
shape_A->project_range(axis, *transform_A, min_A, max_A);
|
|
}
|
|
|
|
if (castB) {
|
|
shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B);
|
|
} else {
|
|
shape_B->project_range(axis, *transform_B, min_B, max_B);
|
|
}
|
|
|
|
if (withMargin) {
|
|
min_A -= margin_A;
|
|
max_A += margin_A;
|
|
min_B -= margin_B;
|
|
max_B += margin_B;
|
|
}
|
|
|
|
min_B -= (max_A - min_A) * 0.5;
|
|
max_B += (max_A - min_A) * 0.5;
|
|
|
|
real_t dmin = min_B - (min_A + max_A) * 0.5;
|
|
real_t dmax = max_B - (min_A + max_A) * 0.5;
|
|
|
|
if (dmin > 0.0 || dmax < 0.0) {
|
|
if (callback && callback->sep_axis) {
|
|
*callback->sep_axis = axis;
|
|
}
|
|
#ifdef DEBUG_ENABLED
|
|
best_axis_count++;
|
|
#endif
|
|
|
|
return false; // doesn't contain 0
|
|
}
|
|
|
|
//use the smallest depth
|
|
|
|
dmin = Math::abs(dmin);
|
|
|
|
if (dmax < dmin) {
|
|
if (dmax < best_depth) {
|
|
best_depth = dmax;
|
|
best_axis = axis;
|
|
#ifdef DEBUG_ENABLED
|
|
best_axis_index = best_axis_count;
|
|
#endif
|
|
}
|
|
} else {
|
|
if (dmin < best_depth) {
|
|
best_depth = dmin;
|
|
best_axis = -axis; // keep it as A axis
|
|
#ifdef DEBUG_ENABLED
|
|
best_axis_index = best_axis_count;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
#ifdef DEBUG_ENABLED
|
|
best_axis_count++;
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
_FORCE_INLINE_ void generate_contacts() {
|
|
// nothing to do, don't generate
|
|
if (best_axis == Vector2(0.0, 0.0)) {
|
|
return;
|
|
}
|
|
|
|
if (callback) {
|
|
callback->collided = true;
|
|
|
|
if (!callback->callback) {
|
|
return; //only collide, no callback
|
|
}
|
|
}
|
|
static const int max_supports = 2;
|
|
|
|
Vector2 supports_A[max_supports];
|
|
int support_count_A;
|
|
if (castA) {
|
|
shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A);
|
|
} else {
|
|
shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A);
|
|
for (int i = 0; i < support_count_A; i++) {
|
|
supports_A[i] = transform_A->xform(supports_A[i]);
|
|
}
|
|
}
|
|
|
|
if (withMargin) {
|
|
for (int i = 0; i < support_count_A; i++) {
|
|
supports_A[i] += -best_axis * margin_A;
|
|
}
|
|
}
|
|
|
|
Vector2 supports_B[max_supports];
|
|
int support_count_B;
|
|
if (castB) {
|
|
shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B);
|
|
} else {
|
|
shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B);
|
|
for (int i = 0; i < support_count_B; i++) {
|
|
supports_B[i] = transform_B->xform(supports_B[i]);
|
|
}
|
|
}
|
|
|
|
if (withMargin) {
|
|
for (int i = 0; i < support_count_B; i++) {
|
|
supports_B[i] += best_axis * margin_B;
|
|
}
|
|
}
|
|
if (callback) {
|
|
callback->normal = best_axis;
|
|
_generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
|
|
|
|
if (callback->sep_axis && *callback->sep_axis != Vector2()) {
|
|
*callback->sep_axis = Vector2(); //invalidate previous axis (no test)
|
|
}
|
|
}
|
|
}
|
|
|
|
_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) {
|
|
margin_A = p_margin_A;
|
|
margin_B = p_margin_B;
|
|
shape_A = p_shape_A;
|
|
shape_B = p_shape_B;
|
|
transform_A = &p_transform_a;
|
|
transform_B = &p_transform_b;
|
|
motion_A = p_motion_A;
|
|
motion_B = p_motion_B;
|
|
callback = p_collector;
|
|
}
|
|
};
|
|
|
|
/****** SAT TESTS *******/
|
|
|
|
#define TEST_POINT(m_a, m_b) \
|
|
((!separator.test_axis(((m_a) - (m_b)).normalized())) || \
|
|
(castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \
|
|
(castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \
|
|
(castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized())))
|
|
|
|
typedef void (*CollisionFunc)(const GodotShape2D *, const Transform2D &, const GodotShape2D *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t);
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_segment_segment(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
|
|
const GodotSegmentShape2D *segment_B = static_cast<const GodotSegmentShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotSegmentShape2D, GodotSegmentShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
//this collision is kind of pointless
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
|
|
return;
|
|
}
|
|
if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) {
|
|
return;
|
|
}
|
|
|
|
if (withMargin) {
|
|
//points grow to circles
|
|
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) {
|
|
return;
|
|
}
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) {
|
|
return;
|
|
}
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) {
|
|
return;
|
|
}
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_segment_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
|
|
const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotSegmentShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
//segment normal
|
|
if (!separator.test_axis(
|
|
(p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) {
|
|
return;
|
|
}
|
|
|
|
//endpoint a vs circle
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) {
|
|
return;
|
|
}
|
|
//endpoint b vs circle
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) {
|
|
return;
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
|
|
const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotSegmentShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(p_transform_b.columns[1].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if (withMargin) {
|
|
Transform2D inv = p_transform_b.affine_inverse();
|
|
|
|
Vector2 a = p_transform_a.xform(segment_A->get_a());
|
|
Vector2 b = p_transform_a.xform(segment_A->get_b());
|
|
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) {
|
|
return;
|
|
}
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) {
|
|
return;
|
|
}
|
|
|
|
if constexpr (castA) {
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) {
|
|
return;
|
|
}
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castB) {
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) {
|
|
return;
|
|
}
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castA && castB) {
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) {
|
|
return;
|
|
}
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
|
|
const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotSegmentShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
|
|
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
|
|
return;
|
|
}
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
|
|
return;
|
|
}
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
|
|
return;
|
|
}
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
|
|
return;
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_segment_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
|
|
const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotSegmentShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
|
|
return;
|
|
}
|
|
|
|
for (int i = 0; i < convex_B->get_point_count(); i++) {
|
|
if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
|
|
return;
|
|
}
|
|
|
|
if (withMargin) {
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) {
|
|
return;
|
|
}
|
|
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
/////////
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_circle_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
|
|
const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotCircleShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) {
|
|
return;
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
|
|
const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotCircleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
const Vector2 &sphere = p_transform_a.columns[2];
|
|
const Vector2 *axis = &p_transform_b.columns[0];
|
|
//const Vector2& half_extents = rectangle_B->get_half_extents();
|
|
|
|
if (!separator.test_axis(axis[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(axis[1].normalized())) {
|
|
return;
|
|
}
|
|
|
|
Transform2D binv = p_transform_b.affine_inverse();
|
|
{
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castA) {
|
|
Vector2 sphereofs = sphere + p_motion_a;
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castB) {
|
|
Vector2 sphereofs = sphere - p_motion_b;
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castA && castB) {
|
|
Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
|
|
const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotCircleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
//capsule axis
|
|
if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
|
|
|
|
//capsule endpoints
|
|
if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
|
|
return;
|
|
}
|
|
if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
|
|
return;
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_circle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
|
|
const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotCircleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
//poly faces and poly points vs circle
|
|
for (int i = 0; i < convex_B->get_point_count(); i++) {
|
|
if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
/////////
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
|
|
const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotRectangleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
//box faces A
|
|
if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
|
|
return;
|
|
}
|
|
|
|
//box faces B
|
|
if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(p_transform_b.columns[1].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if constexpr (withMargin) {
|
|
Transform2D invA = p_transform_a.affine_inverse();
|
|
Transform2D invB = p_transform_b.affine_inverse();
|
|
|
|
if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) {
|
|
return;
|
|
}
|
|
|
|
if constexpr (castA || castB) {
|
|
Transform2D aofs = p_transform_a;
|
|
aofs.columns[2] += p_motion_a;
|
|
|
|
Transform2D bofs = p_transform_b;
|
|
bofs.columns[2] += p_motion_b;
|
|
|
|
[[maybe_unused]] Transform2D aofsinv = aofs.affine_inverse();
|
|
[[maybe_unused]] Transform2D bofsinv = bofs.affine_inverse();
|
|
|
|
if constexpr (castA) {
|
|
if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castB) {
|
|
if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castA && castB) {
|
|
if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
|
|
const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotRectangleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
//box faces
|
|
if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
|
|
return;
|
|
}
|
|
|
|
//capsule axis
|
|
if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
//box endpoints to capsule circles
|
|
|
|
Transform2D boxinv = p_transform_a.affine_inverse();
|
|
|
|
real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
|
|
|
|
for (int i = 0; i < 2; i++) {
|
|
{
|
|
Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
|
|
|
|
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castA) {
|
|
Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
|
|
capsule_endpoint -= p_motion_a;
|
|
|
|
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castB) {
|
|
Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
|
|
capsule_endpoint += p_motion_b;
|
|
|
|
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if constexpr (castA && castB) {
|
|
Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
|
|
capsule_endpoint -= p_motion_a;
|
|
capsule_endpoint += p_motion_b;
|
|
|
|
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
capsule_dir *= -1.0;
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
|
|
const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotRectangleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
//box faces
|
|
if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
|
|
return;
|
|
}
|
|
|
|
//convex faces
|
|
Transform2D boxinv;
|
|
if constexpr (withMargin) {
|
|
boxinv = p_transform_a.affine_inverse();
|
|
}
|
|
for (int i = 0; i < convex_B->get_point_count(); i++) {
|
|
if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
|
|
return;
|
|
}
|
|
|
|
if constexpr (withMargin) {
|
|
//all points vs all points need to be tested if margin exist
|
|
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) {
|
|
return;
|
|
}
|
|
if constexpr (castA) {
|
|
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) {
|
|
return;
|
|
}
|
|
}
|
|
if constexpr (castB) {
|
|
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) {
|
|
return;
|
|
}
|
|
}
|
|
if constexpr (castA && castB) {
|
|
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
/////////
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a);
|
|
const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotCapsuleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
//capsule axis
|
|
|
|
if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
//capsule endpoints
|
|
|
|
real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
|
|
for (int i = 0; i < 2; i++) {
|
|
Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir_A;
|
|
|
|
real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
|
|
for (int j = 0; j < 2; j++) {
|
|
Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir_B;
|
|
|
|
if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) {
|
|
return;
|
|
}
|
|
|
|
capsule_dir_B *= -1.0;
|
|
}
|
|
|
|
capsule_dir_A *= -1.0;
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a);
|
|
const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotCapsuleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
//capsule axis
|
|
|
|
if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
|
|
return;
|
|
}
|
|
|
|
//poly vs capsule
|
|
for (int i = 0; i < convex_B->get_point_count(); i++) {
|
|
Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i));
|
|
|
|
real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
|
|
for (int j = 0; j < 2; j++) {
|
|
Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir;
|
|
|
|
if (TEST_POINT(capsule_endpoint_A, cpoint)) {
|
|
return;
|
|
}
|
|
|
|
capsule_dir *= -1.0;
|
|
}
|
|
|
|
if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
/////////
|
|
|
|
template <bool castA, bool castB, bool withMargin>
|
|
static void _collision_convex_polygon_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
|
|
const GodotConvexPolygonShape2D *convex_A = static_cast<const GodotConvexPolygonShape2D *>(p_a);
|
|
const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
|
|
|
|
SeparatorAxisTest2D<GodotConvexPolygonShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
|
|
|
|
if (!separator.test_previous_axis()) {
|
|
return;
|
|
}
|
|
|
|
if (!separator.test_cast()) {
|
|
return;
|
|
}
|
|
|
|
for (int i = 0; i < convex_A->get_point_count(); i++) {
|
|
if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < convex_B->get_point_count(); i++) {
|
|
if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (withMargin) {
|
|
for (int i = 0; i < convex_A->get_point_count(); i++) {
|
|
for (int j = 0; j < convex_B->get_point_count(); j++) {
|
|
if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
separator.generate_contacts();
|
|
}
|
|
|
|
////////
|
|
|
|
bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) {
|
|
PhysicsServer2D::ShapeType type_A = p_shape_A->get_type();
|
|
|
|
ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false);
|
|
ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false);
|
|
ERR_FAIL_COND_V(p_shape_A->is_concave(), false);
|
|
|
|
PhysicsServer2D::ShapeType type_B = p_shape_B->get_type();
|
|
|
|
ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false);
|
|
ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false);
|
|
ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
|
|
|
|
static const CollisionFunc collision_table[5][5] = {
|
|
{ _collision_segment_segment<false, false, false>,
|
|
_collision_segment_circle<false, false, false>,
|
|
_collision_segment_rectangle<false, false, false>,
|
|
_collision_segment_capsule<false, false, false>,
|
|
_collision_segment_convex_polygon<false, false, false> },
|
|
{ nullptr,
|
|
_collision_circle_circle<false, false, false>,
|
|
_collision_circle_rectangle<false, false, false>,
|
|
_collision_circle_capsule<false, false, false>,
|
|
_collision_circle_convex_polygon<false, false, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
_collision_rectangle_rectangle<false, false, false>,
|
|
_collision_rectangle_capsule<false, false, false>,
|
|
_collision_rectangle_convex_polygon<false, false, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_capsule_capsule<false, false, false>,
|
|
_collision_capsule_convex_polygon<false, false, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_convex_polygon_convex_polygon<false, false, false> }
|
|
|
|
};
|
|
|
|
static const CollisionFunc collision_table_castA[5][5] = {
|
|
{ _collision_segment_segment<true, false, false>,
|
|
_collision_segment_circle<true, false, false>,
|
|
_collision_segment_rectangle<true, false, false>,
|
|
_collision_segment_capsule<true, false, false>,
|
|
_collision_segment_convex_polygon<true, false, false> },
|
|
{ nullptr,
|
|
_collision_circle_circle<true, false, false>,
|
|
_collision_circle_rectangle<true, false, false>,
|
|
_collision_circle_capsule<true, false, false>,
|
|
_collision_circle_convex_polygon<true, false, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
_collision_rectangle_rectangle<true, false, false>,
|
|
_collision_rectangle_capsule<true, false, false>,
|
|
_collision_rectangle_convex_polygon<true, false, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_capsule_capsule<true, false, false>,
|
|
_collision_capsule_convex_polygon<true, false, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_convex_polygon_convex_polygon<true, false, false> }
|
|
|
|
};
|
|
|
|
static const CollisionFunc collision_table_castB[5][5] = {
|
|
{ _collision_segment_segment<false, true, false>,
|
|
_collision_segment_circle<false, true, false>,
|
|
_collision_segment_rectangle<false, true, false>,
|
|
_collision_segment_capsule<false, true, false>,
|
|
_collision_segment_convex_polygon<false, true, false> },
|
|
{ nullptr,
|
|
_collision_circle_circle<false, true, false>,
|
|
_collision_circle_rectangle<false, true, false>,
|
|
_collision_circle_capsule<false, true, false>,
|
|
_collision_circle_convex_polygon<false, true, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
_collision_rectangle_rectangle<false, true, false>,
|
|
_collision_rectangle_capsule<false, true, false>,
|
|
_collision_rectangle_convex_polygon<false, true, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_capsule_capsule<false, true, false>,
|
|
_collision_capsule_convex_polygon<false, true, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_convex_polygon_convex_polygon<false, true, false> }
|
|
|
|
};
|
|
|
|
static const CollisionFunc collision_table_castA_castB[5][5] = {
|
|
{ _collision_segment_segment<true, true, false>,
|
|
_collision_segment_circle<true, true, false>,
|
|
_collision_segment_rectangle<true, true, false>,
|
|
_collision_segment_capsule<true, true, false>,
|
|
_collision_segment_convex_polygon<true, true, false> },
|
|
{ nullptr,
|
|
_collision_circle_circle<true, true, false>,
|
|
_collision_circle_rectangle<true, true, false>,
|
|
_collision_circle_capsule<true, true, false>,
|
|
_collision_circle_convex_polygon<true, true, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
_collision_rectangle_rectangle<true, true, false>,
|
|
_collision_rectangle_capsule<true, true, false>,
|
|
_collision_rectangle_convex_polygon<true, true, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_capsule_capsule<true, true, false>,
|
|
_collision_capsule_convex_polygon<true, true, false> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_convex_polygon_convex_polygon<true, true, false> }
|
|
|
|
};
|
|
|
|
static const CollisionFunc collision_table_margin[5][5] = {
|
|
{ _collision_segment_segment<false, false, true>,
|
|
_collision_segment_circle<false, false, true>,
|
|
_collision_segment_rectangle<false, false, true>,
|
|
_collision_segment_capsule<false, false, true>,
|
|
_collision_segment_convex_polygon<false, false, true> },
|
|
{ nullptr,
|
|
_collision_circle_circle<false, false, true>,
|
|
_collision_circle_rectangle<false, false, true>,
|
|
_collision_circle_capsule<false, false, true>,
|
|
_collision_circle_convex_polygon<false, false, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
_collision_rectangle_rectangle<false, false, true>,
|
|
_collision_rectangle_capsule<false, false, true>,
|
|
_collision_rectangle_convex_polygon<false, false, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_capsule_capsule<false, false, true>,
|
|
_collision_capsule_convex_polygon<false, false, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_convex_polygon_convex_polygon<false, false, true> }
|
|
|
|
};
|
|
|
|
static const CollisionFunc collision_table_castA_margin[5][5] = {
|
|
{ _collision_segment_segment<true, false, true>,
|
|
_collision_segment_circle<true, false, true>,
|
|
_collision_segment_rectangle<true, false, true>,
|
|
_collision_segment_capsule<true, false, true>,
|
|
_collision_segment_convex_polygon<true, false, true> },
|
|
{ nullptr,
|
|
_collision_circle_circle<true, false, true>,
|
|
_collision_circle_rectangle<true, false, true>,
|
|
_collision_circle_capsule<true, false, true>,
|
|
_collision_circle_convex_polygon<true, false, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
_collision_rectangle_rectangle<true, false, true>,
|
|
_collision_rectangle_capsule<true, false, true>,
|
|
_collision_rectangle_convex_polygon<true, false, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_capsule_capsule<true, false, true>,
|
|
_collision_capsule_convex_polygon<true, false, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_convex_polygon_convex_polygon<true, false, true> }
|
|
|
|
};
|
|
|
|
static const CollisionFunc collision_table_castB_margin[5][5] = {
|
|
{ _collision_segment_segment<false, true, true>,
|
|
_collision_segment_circle<false, true, true>,
|
|
_collision_segment_rectangle<false, true, true>,
|
|
_collision_segment_capsule<false, true, true>,
|
|
_collision_segment_convex_polygon<false, true, true> },
|
|
{ nullptr,
|
|
_collision_circle_circle<false, true, true>,
|
|
_collision_circle_rectangle<false, true, true>,
|
|
_collision_circle_capsule<false, true, true>,
|
|
_collision_circle_convex_polygon<false, true, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
_collision_rectangle_rectangle<false, true, true>,
|
|
_collision_rectangle_capsule<false, true, true>,
|
|
_collision_rectangle_convex_polygon<false, true, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_capsule_capsule<false, true, true>,
|
|
_collision_capsule_convex_polygon<false, true, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_convex_polygon_convex_polygon<false, true, true> }
|
|
|
|
};
|
|
|
|
static const CollisionFunc collision_table_castA_castB_margin[5][5] = {
|
|
{ _collision_segment_segment<true, true, true>,
|
|
_collision_segment_circle<true, true, true>,
|
|
_collision_segment_rectangle<true, true, true>,
|
|
_collision_segment_capsule<true, true, true>,
|
|
_collision_segment_convex_polygon<true, true, true> },
|
|
{ nullptr,
|
|
_collision_circle_circle<true, true, true>,
|
|
_collision_circle_rectangle<true, true, true>,
|
|
_collision_circle_capsule<true, true, true>,
|
|
_collision_circle_convex_polygon<true, true, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
_collision_rectangle_rectangle<true, true, true>,
|
|
_collision_rectangle_capsule<true, true, true>,
|
|
_collision_rectangle_convex_polygon<true, true, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_capsule_capsule<true, true, true>,
|
|
_collision_capsule_convex_polygon<true, true, true> },
|
|
{ nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
nullptr,
|
|
_collision_convex_polygon_convex_polygon<true, true, true> }
|
|
|
|
};
|
|
|
|
_CollectorCallback2D callback;
|
|
callback.callback = p_result_callback;
|
|
callback.swap = p_swap;
|
|
callback.userdata = p_userdata;
|
|
callback.collided = false;
|
|
callback.sep_axis = sep_axis;
|
|
|
|
const GodotShape2D *A = p_shape_A;
|
|
const GodotShape2D *B = p_shape_B;
|
|
const Transform2D *transform_A = &p_transform_A;
|
|
const Transform2D *transform_B = &p_transform_B;
|
|
const Vector2 *motion_A = &p_motion_A;
|
|
const Vector2 *motion_B = &p_motion_B;
|
|
real_t margin_A = p_margin_A, margin_B = p_margin_B;
|
|
|
|
if (type_A > type_B) {
|
|
SWAP(A, B);
|
|
SWAP(transform_A, transform_B);
|
|
SWAP(type_A, type_B);
|
|
SWAP(motion_A, motion_B);
|
|
SWAP(margin_A, margin_B);
|
|
callback.swap = !callback.swap;
|
|
}
|
|
|
|
CollisionFunc collision_func;
|
|
|
|
if (p_margin_A || p_margin_B) {
|
|
if (*motion_A == Vector2() && *motion_B == Vector2()) {
|
|
collision_func = collision_table_margin[type_A - 2][type_B - 2];
|
|
} else if (*motion_A != Vector2() && *motion_B == Vector2()) {
|
|
collision_func = collision_table_castA_margin[type_A - 2][type_B - 2];
|
|
} else if (*motion_A == Vector2() && *motion_B != Vector2()) {
|
|
collision_func = collision_table_castB_margin[type_A - 2][type_B - 2];
|
|
} else {
|
|
collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2];
|
|
}
|
|
} else {
|
|
if (*motion_A == Vector2() && *motion_B == Vector2()) {
|
|
collision_func = collision_table[type_A - 2][type_B - 2];
|
|
} else if (*motion_A != Vector2() && *motion_B == Vector2()) {
|
|
collision_func = collision_table_castA[type_A - 2][type_B - 2];
|
|
} else if (*motion_A == Vector2() && *motion_B != Vector2()) {
|
|
collision_func = collision_table_castB[type_A - 2][type_B - 2];
|
|
} else {
|
|
collision_func = collision_table_castA_castB[type_A - 2][type_B - 2];
|
|
}
|
|
}
|
|
|
|
ERR_FAIL_COND_V(!collision_func, false);
|
|
|
|
collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B);
|
|
|
|
return callback.collided;
|
|
}
|