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) 2007-2022 Juan Linietsky, Ariel Manzur. */
|
|
/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */
|
|
/* */
|
|
/* 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, max_A, min_B, max_B;
|
|
|
|
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 (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 (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 (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 (castA) {
|
|
Vector2 sphereofs = sphere + p_motion_a;
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (castB) {
|
|
Vector2 sphereofs = sphere - p_motion_b;
|
|
if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (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 (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 (castA || castB) {
|
|
Transform2D aofs = p_transform_a;
|
|
aofs.columns[2] += p_motion_a;
|
|
|
|
Transform2D bofs = p_transform_b;
|
|
bofs.columns[2] += p_motion_b;
|
|
|
|
Transform2D aofsinv = aofs.affine_inverse();
|
|
Transform2D bofsinv = bofs.affine_inverse();
|
|
|
|
if (castA) {
|
|
if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (castB) {
|
|
if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (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 (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 (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 (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 (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 (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 (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 (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 (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;
|
|
}
|