asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
virtualx-engine/servers/physics_2d/collision_solver_2d_sat.cpp
Rémi Verschelde d8223ffa75 Welcome in 2017, dear changelog reader! 
That year should bring the long-awaited OpenGL ES 3.0 compatible renderer
with state-of-the-art rendering techniques tuned to work as low as middle
end handheld devices - without compromising with the possibilities given
for higher end desktop games of course. Great times ahead for the Godot
community and the gamers that will play our games!

(cherry picked from commit c7bc44d5ad)
2017-01-12 19:15:30 +01:00

1605 lines
48 KiB
C++
Raw Blame History

/*************************************************************************/
/* collision_solver_2d_sat.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2017 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 "collision_solver_2d_sat.h"
#include "geometry.h"
struct _CollectorCallback2D {
CollisionSolver2DSW::CallbackResult callback;
void *userdata;
bool swap;
bool collided;
Vector2 normal;
Vector2 *sep_axis;
_FORCE_INLINE_ void call(const Vector2& p_point_A, const Vector2& p_point_B) {
//if (normal.dot(p_point_A) >= normal.dot(p_point_B))
// return;
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 = Geometry::get_closest_point_to_segment_uncapped_2d(*p_points_A, p_points_B );
p_collector->call(*p_points_A,closest_B);
}
struct _generate_contacts_Pair {
bool a;
int idx;
float d;
_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
# if 0
Vector2 rel_A=p_points_A[1]-p_points_A[0];
Vector2 rel_B=p_points_B[1]-p_points_B[0];
Vector2 t = p_collector->normal.tangent();
print_line("tangent: "+t);
real_t dA[2]={t.dot(p_points_A[0]),t.dot(p_points_A[1])};
Vector2 pA[2]={p_points_A[0],p_points_A[1]};
if (dA[0]>dA[1]) {
SWAP(dA[0],dA[1]);
SWAP(pA[0],pA[1]);
}
float dB[2]={t.dot(p_points_B[0]),t.dot(p_points_B[1])};
Vector2 pB[2]={p_points_B[0],p_points_B[1]};
if (dB[0]>dB[1]) {
SWAP(dB[0],dB[1]);
SWAP(pB[0],pB[1]);
}
if (dA[0]<dB[0]) {
Vector2 n = (p_points_A[1]-p_points_A[0]).normalized().tangent();
real_t d = n.dot(p_points_A[1]);
if (dA[1]>dB[1]) {
//A contains B
for(int i=0;i<2;i++) {
Vector2 b = p_points_B[i];
Vector2 a = n.plane_project(d,b);
if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-CMP_EPSILON)
continue;
p_collector->call(a,b);
}
} else {
// B0,A1 containment
Vector2 n_B = (p_points_B[1]-p_points_B[0]).normalized().tangent();
real_t d_B = n_B.dot(p_points_B[1]);
// first, B on A
{
Vector2 b = p_points_B[0];
Vector2 a = n.plane_project(d,b);
if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON)
p_collector->call(a,b);
}
// second, A on B
{
Vector2 a = p_points_A[1];
Vector2 b = n_B.plane_project(d_B,a);
if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON)
p_collector->call(a,b);
}
}
} else {
Vector2 n = (p_points_B[1]-p_points_B[0]).normalized().tangent();
real_t d = n.dot(p_points_B[1]);
if (dB[1]>dA[1]) {
//B contains A
for(int i=0;i<2;i++) {
Vector2 a = p_points_A[i];
Vector2 b = n.plane_project(d,a);
if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-CMP_EPSILON)
continue;
p_collector->call(a,b);
}
} else {
// A0,B1 containment
Vector2 n_A = (p_points_A[1]-p_points_A[0]).normalized().tangent();
real_t d_A = n_A.dot(p_points_A[1]);
// first A on B
{
Vector2 a = p_points_A[0];
Vector2 b = n.plane_project(d,a);
if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON)
p_collector->call(a,b);
}
//second, B on A
{
Vector2 b = p_points_B[1];
Vector2 a = n_A.plane_project(d_A,b);
if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON)
p_collector->call(a,b);
}
}
}
#endif
#if 1
Vector2 n = p_collector->normal;
Vector2 t = n.tangent();
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);
}
}
#elif 0
Vector2 axis = rel_A.normalized(); //make an axis
Vector2 axis_B = rel_B.normalized();
if (axis.dot(axis_B)<0)
axis_B=-axis_B;
axis=(axis+axis_B)*0.5;
Vector2 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]);
Vector2 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]);
//sort all 4 points in axis
float dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) };
//todo , find max/min and then use 2 central points
SortArray<float> sa;
sa.sort(dvec,4);
//use the middle ones as contacts
for (int i=1;i<=2;i++) {
Vector2 a = base_A+axis*dvec[i];
Vector2 b = base_B+axis*dvec[i];
if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-0.01) {
print_line("fail a: "+a);
print_line("fail b: "+b);
continue;
}
print_line("res a: "+a);
print_line("res b: "+b);
p_collector->call(a,b);
}
#endif
}
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,
},{
0,
_generate_contacts_edge_edge,
}
};
int pointcount_B;
int pointcount_A;
const Vector2 *points_A;
const Vector2 *points_B;
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 > 3 ? 3 : pointcount_A) -1;
int version_B = (pointcount_B > 3 ? 3 : 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;
const ShapeB *shape_B;
const Matrix32 *transform_A;
const Matrix32 *transform_B;
real_t best_depth;
Vector2 best_axis;
int best_axis_count;
int best_axis_index;
Vector2 motion_A;
Vector2 motion_B;
real_t margin_A;
real_t margin_B;
_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.tangent()))
return false;
}
if (castB) {
Vector2 nb = motion_B.normalized();
if (!test_axis(nb))
return false;
if (!test_axis(nb.tangent()))
return false;
}
return true;
}
_FORCE_INLINE_ bool test_axis(const Vector2& p_axis) {
Vector2 axis=p_axis;
if ( Math::abs(axis.x)<CMP_EPSILON &&
Math::abs(axis.y)<CMP_EPSILON) {
// 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
}
}
// print_line("test axis: "+p_axis+" depth: "+rtos(best_depth));
#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;
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;
}
}
/*
print_line("**************************");
printf("CBK: %p\n",callback->userdata);
print_line("type A: "+itos(shape_A->get_type()));
print_line("type B: "+itos(shape_B->get_type()));
print_line("xform A: "+*transform_A);
print_line("xform B: "+*transform_B);
print_line("normal: "+best_axis);
print_line("depth: "+rtos(best_depth));
print_line("index: "+itos(best_axis_index));
for(int i=0;i<support_count_A;i++) {
print_line("A-"+itos(i)+": "+supports_A[i]);
}
for(int i=0;i<support_count_B;i++) {
print_line("B-"+itos(i)+": "+supports_B[i]);
}
//*/
callback->normal=best_axis;
_generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback);
if (callback && callback->sep_axis && *callback->sep_axis!=Vector2())
*callback->sep_axis=Vector2(); //invalidate previous axis (no test)
//CollisionSolver2DSW::CallbackResult cbk=NULL;
//cbk(Vector2(),Vector2(),NULL);
}
_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& 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;
best_depth=1e15;
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;
#ifdef DEBUG_ENABLED
best_axis_count=0;
best_axis_index=-1;
#endif
}
};
/****** SAT TESTS *******/
/****** SAT TESTS *******/
/****** SAT TESTS *******/
/****** 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 Shape2DSW*,const Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&,float,float);
template<bool castA, bool castB,bool withMargin>
static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,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_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(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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,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().tangent()
))
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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,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.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_b.elements[1].normalized()))
return;
if (withMargin) {
Matrix32 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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,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.elements[0].normalized()))
return;
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
return;
if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
return;
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
return;
if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
return;
separator.generate_contacts();
}
template<bool castA, bool castB,bool withMargin>
static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,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.elements[2];
const Vector2 *axis=&p_transform_b.elements[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;
Matrix32 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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,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.elements[0].normalized()))
return;
//capsule endpoints
if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
return;
if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
return;
separator.generate_contacts();
}
template<bool castA, bool castB,bool withMargin>
static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,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.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_a.elements[1].normalized()))
return;
//box faces B
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_b.elements[1].normalized()))
return;
if (withMargin) {
Matrix32 invA=p_transform_a.affine_inverse();
Matrix32 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) {
Matrix32 aofs = p_transform_a;
aofs.elements[2]+=p_motion_a;
Matrix32 bofs = p_transform_b;
bofs.elements[2]+=p_motion_b;
Matrix32 aofsinv = aofs.affine_inverse();
Matrix32 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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,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.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_a.elements[1].normalized()))
return;
//capsule axis
if (!separator.test_axis(p_transform_b.elements[0].normalized()))
return;
//box endpoints to capsule circles
Matrix32 boxinv = p_transform_a.affine_inverse();
for(int i=0;i<2;i++) {
{
Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
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.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
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.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
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.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
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;
}
}
separator.generate_contacts();
}
template<bool castA, bool castB,bool withMargin>
static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,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.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_a.elements[1].normalized()))
return;
//convex faces
Matrix32 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 Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,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.elements[0].normalized()))
return;
if (!separator.test_axis(p_transform_a.elements[0].normalized()))
return;
//capsule endpoints
for(int i=0;i<2;i++) {
Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(i==0?0.5:-0.5);
for(int j=0;j<2;j++) {
Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5);
if (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) )
return;
}
}
separator.generate_contacts();
}
template<bool castA, bool castB,bool withMargin>
static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,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.elements[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));
for(int j=0;j<2;j++) {
Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5);
if (TEST_POINT(capsule_endpoint_A,cpoint ))
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_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,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 Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
Physics2DServer::ShapeType type_A=p_shape_A->get_type();
ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_LINE,false);
//ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_RAY,false);
ERR_FAIL_COND_V(p_shape_A->is_concave(),false);
Physics2DServer::ShapeType type_B=p_shape_B->get_type();
ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_LINE,false);
//ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_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>},
{0,
_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>},
{0,
0,
_collision_rectangle_rectangle<false,false,false>,
_collision_rectangle_capsule<false,false,false>,
_collision_rectangle_convex_polygon<false,false,false>},
{0,
0,
0,
_collision_capsule_capsule<false,false,false>,
_collision_capsule_convex_polygon<false,false,false>},
{0,
0,
0,
0,
_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>},
{0,
_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>},
{0,
0,
_collision_rectangle_rectangle<true,false,false>,
_collision_rectangle_capsule<true,false,false>,
_collision_rectangle_convex_polygon<true,false,false>},
{0,
0,
0,
_collision_capsule_capsule<true,false,false>,
_collision_capsule_convex_polygon<true,false,false>},
{0,
0,
0,
0,
_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>},
{0,
_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>},
{0,
0,
_collision_rectangle_rectangle<false,true,false>,
_collision_rectangle_capsule<false,true,false>,
_collision_rectangle_convex_polygon<false,true,false>},
{0,
0,
0,
_collision_capsule_capsule<false,true,false>,
_collision_capsule_convex_polygon<false,true,false>},
{0,
0,
0,
0,
_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>},
{0,
_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>},
{0,
0,
_collision_rectangle_rectangle<true,true,false>,
_collision_rectangle_capsule<true,true,false>,
_collision_rectangle_convex_polygon<true,true,false>},
{0,
0,
0,
_collision_capsule_capsule<true,true,false>,
_collision_capsule_convex_polygon<true,true,false>},
{0,
0,
0,
0,
_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>},
{0,
_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>},
{0,
0,
_collision_rectangle_rectangle<false,false,true>,
_collision_rectangle_capsule<false,false,true>,
_collision_rectangle_convex_polygon<false,false,true>},
{0,
0,
0,
_collision_capsule_capsule<false,false,true>,
_collision_capsule_convex_polygon<false,false,true>},
{0,
0,
0,
0,
_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>},
{0,
_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>},
{0,
0,
_collision_rectangle_rectangle<true,false,true>,
_collision_rectangle_capsule<true,false,true>,
_collision_rectangle_convex_polygon<true,false,true>},
{0,
0,
0,
_collision_capsule_capsule<true,false,true>,
_collision_capsule_convex_polygon<true,false,true>},
{0,
0,
0,
0,
_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>},
{0,
_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>},
{0,
0,
_collision_rectangle_rectangle<false,true,true>,
_collision_rectangle_capsule<false,true,true>,
_collision_rectangle_convex_polygon<false,true,true>},
{0,
0,
0,
_collision_capsule_capsule<false,true,true>,
_collision_capsule_convex_polygon<false,true,true>},
{0,
0,
0,
0,
_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>},
{0,
_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>},
{0,
0,
_collision_rectangle_rectangle<true,true,true>,
_collision_rectangle_capsule<true,true,true>,
_collision_rectangle_convex_polygon<true,true,true>},
{0,
0,
0,
_collision_capsule_capsule<true,true,true>,
_collision_capsule_convex_polygon<true,true,true>},
{0,
0,
0,
0,
_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 Shape2DSW *A=p_shape_A;
const Shape2DSW *B=p_shape_B;
const Matrix32 *transform_A=&p_transform_A;
const Matrix32 *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;
}
Reference in a new issue View git blame Copy permalink