diff options
Diffstat (limited to 'servers/physics_2d/collision_solver_2d_sat.cpp')
| -rw-r--r-- | servers/physics_2d/collision_solver_2d_sat.cpp | 1034 |
1 files changed, 1034 insertions, 0 deletions
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp new file mode 100644 index 0000000000..4b87ff02a2 --- /dev/null +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -0,0 +1,1034 @@ +/*************************************************************************/ +/* collision_solver_2d_sat.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "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 { + 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 + + + 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(); + + 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); + } + + } + } + + +#if 0 + + Vector2 axis = rel_A.normalized(); + Vector2 axis_B = rel_B.normalized(); + if (axis.dot(axis_B)<0) + axis_B=-axis_B; + axis=(axis+axis_B)*0.5; + + Vector2 normal_A = axis.tangent(); + real_t dA = normal_A.dot(p_points_A[0]); + Vector2 normal_B = rel_B.tangent().normalized(); + real_t dB = normal_A.dot(p_points_B[0]); + + Vector2 A[4]={ normal_A.plane_project(dA,p_points_B[0]), normal_A.plane_project(dA,p_points_B[1]), p_points_A[0], p_points_A[1] }; + Vector2 B[4]={ p_points_B[0], p_points_B[1], normal_B.plane_project(dB,p_points_A[0]), normal_B.plane_project(dB,p_points_A[1]) }; + + _generate_contacts_Pair dvec[4]; + for(int i=0;i<4;i++) { + dvec[i].d=axis.dot(p_points_A[0]-A[i]); + dvec[i].idx=i; + } + + SortArray<_generate_contacts_Pair> sa; + sa.sort(dvec,4); + + for(int i=1;i<=2;i++) { + + Vector2 a = A[i]; + Vector2 b = B[i]; + if (p_collector->normal.dot(a) > p_collector->normal.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> +class SeparatorAxisTest2D { + + const ShapeA *shape_A; + const ShapeB *shape_B; + const Matrix32 *transform_A; + const Matrix32 *transform_B; + const Matrix32 *transform_inv_A; + const Matrix32 *transform_inv_B; + real_t best_depth; + Vector2 best_axis; + int best_axis_count; + int best_axis_index; + _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_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; + + shape_A->project_range(axis,*transform_A,min_A,max_A); + shape_B->project_range(axis,*transform_B,min_B,max_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; + 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]); + } + + + + Vector2 supports_B[max_supports]; + int support_count_B; + 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]); + } +/* + + + 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 Matrix32& p_transform_inv_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + best_depth=1e15; + shape_A=p_shape_A; + shape_B=p_shape_B; + transform_A=&p_transform_a; + transform_B=&p_transform_b; + transform_inv_A=&p_transform_inv_a; + transform_inv_B=&p_transform_inv_b; + callback=p_collector; +#ifdef DEBUG_ENABLED + best_axis_count=0; + best_axis_index=-1; +#endif + } + +}; + +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ + + +typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Matrix32&,const Shape2DSW*,const Matrix32&,const Matrix32&,_CollectorCallback2D *p_collector); + +static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b); + + SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,segment_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + return; + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_B->get_normal()).normalized())) + return; + + separator.generate_contacts(); + +} + +static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis( + (p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent() + )) + return; + +// if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) +// return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-p_transform_b.get_origin()).normalized())) + return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-p_transform_b.get_origin()).normalized())) + return; + + + separator.generate_contacts(); + + +} + +static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + + SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + return; + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + if (!separator.test_axis(p_transform_b.elements[1].normalized())) + return; + + separator.generate_contacts(); + +} + +static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + + SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + return; + + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + return; + if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + return; + + separator.generate_contacts(); +} + +static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized())) + return; + + for(int i=0;i<convex_B->get_point_count();i++) { + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); + +} + + +///////// + +static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); + const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis((p_transform_a.get_origin()-p_transform_b.get_origin()).normalized())) + return; + + separator.generate_contacts(); + +} + +static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + 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; + + Vector2 local_v = p_transform_inv_b.xform(p_transform_a.get_origin()); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + + if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized())) + return; + + separator.generate_contacts(); +} + +static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //capsule axis + if (!separator.test_axis(p_transform_b.elements[0].normalized())) + return; + + //capsule endpoints + if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized())) + return; + if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized())) + return; + + + separator.generate_contacts(); + + +} + +static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //poly faces and poly points vs circle + for(int i=0;i<convex_B->get_point_count();i++) { + + if (!separator.test_axis( (p_transform_b.xform(convex_B->get_point(i))-p_transform_a.get_origin()).normalized() )) + return; + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); +} + + +///////// + +static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); + const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + 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; + + separator.generate_contacts(); +} + +static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + 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 + + 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); + + const Vector2& half_extents = rectangle_A->get_half_extents(); + Vector2 local_v = p_transform_inv_a.xform(capsule_endpoint); + + Vector2 he( + (local_v.x<0) ? -half_extents.x : half_extents.x, + (local_v.y<0) ? -half_extents.y : half_extents.y + ); + + + if (!separator.test_axis(p_transform_a.xform(he).normalized())) + return; + + } + + + separator.generate_contacts(); +} + +static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + 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 + for(int i=0;i<convex_B->get_point_count();i++) { + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); + +} + + +///////// + +static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a); + const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + 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 (!separator.test_axis( (capsule_endpoint_A-capsule_endpoint_B).normalized() )) + return; + + } + } + + separator.generate_contacts(); + +} + +static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + + SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + 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 (!separator.test_axis( (cpoint - capsule_endpoint_A).normalized() )) + return; + + } + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); +} + + +///////// + + +static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) { + + + const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a); + const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b); + + SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW> separator(convex_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + for(int i=0;i<convex_A->get_point_count();i++) { + + if (!separator.test_axis( p_transform_inv_a.basis_xform_inv(convex_A->get_segment_normal(i)).normalized() )) + return; + } + + for(int i=0;i<convex_B->get_point_count();i++) { + + if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() )) + return; + } + + separator.generate_contacts(); + +} + + +//////// + +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) { + + 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, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + _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 Matrix32 *transform_inv_A=&p_transform_inv_A; + const Matrix32 *transform_inv_B=&p_transform_inv_B; + + if (type_A > type_B) { + SWAP(A,B); + SWAP(transform_A,transform_B); + SWAP(transform_inv_A,transform_inv_B); + SWAP(type_A,type_B); + callback.swap = !callback.swap; + } + + + CollisionFunc collision_func = collision_table[type_A-2][type_B-2]; + ERR_FAIL_COND_V(!collision_func,false); + + + collision_func(A,*transform_A,*transform_inv_A,B,*transform_B,*transform_inv_B,&callback); + + return callback.collided; + + +} |