summaryrefslogtreecommitdiff
path: root/servers
diff options
context:
space:
mode:
Diffstat (limited to 'servers')
-rw-r--r--servers/physics_2d/broad_phase_2d_hash_grid.cpp108
-rw-r--r--servers/physics_2d/broad_phase_2d_hash_grid.h35
-rw-r--r--servers/physics_2d/collision_solver_2d_sat.cpp61
3 files changed, 166 insertions, 38 deletions
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 6a52d5fe5b..953c87021f 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -29,6 +29,8 @@
#include "broad_phase_2d_hash_grid.h"
#include "globals.h"
+#define LARGE_ELEMENT_FI 1.01239812
+
void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element* p_with) {
Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
@@ -102,6 +104,26 @@ void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
+
+ Vector2 sz = (p_rect.size/cell_size*LARGE_ELEMENT_FI); //use magic number to avoid floating point issues
+ if (sz.width*sz.height > large_object_min_surface) {
+ //large object, do not use grid, must check against all elements
+ for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+ if (E->key()==p_elem->self)
+ continue; // do not pair against itself
+ if (E->get().owner == p_elem->owner)
+ continue;
+ if (E->get()._static && p_static)
+ continue;
+
+ _pair_attempt(p_elem,&E->get());
+ }
+
+
+ large_elements[p_elem].inc();
+ return;
+ }
+
Point2i from = (p_rect.pos/cell_size).floor();
Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
@@ -174,12 +196,40 @@ void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,boo
}
+ //pair separatedly with large elements
+
+ for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
+
+ if (E->key()==p_elem)
+ continue; // do not pair against itself
+ if (E->key()->owner == p_elem->owner)
+ continue;
+ if (E->key()->_static && p_static)
+ continue;
+
+ _pair_attempt(E->key(),p_elem);
+ }
}
void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
+ Vector2 sz = (p_rect.size/cell_size*LARGE_ELEMENT_FI);
+ if (sz.width*sz.height > large_object_min_surface) {
+
+ //unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static
+ for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) {
+
+ _unpair_attempt(p_elem,E->key());
+ }
+
+ if (large_elements[p_elem].dec()==0) {
+ large_elements.erase(p_elem);
+ }
+ return;
+ }
+
Point2i from = (p_rect.pos/cell_size).floor();
Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
@@ -274,6 +324,20 @@ void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool
}
+
+ for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
+ if (E->key()==p_elem)
+ continue; // do not pair against itself
+ if (E->key()->owner == p_elem->owner)
+ continue;
+ if (E->key()->_static && p_static)
+ continue;
+
+ //unpair from large elements
+ _unpair_attempt(p_elem,E->key());
+ }
+
+
}
@@ -526,6 +590,28 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t
}
+ for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
+
+ if (cullcount>=p_max_results)
+ break;
+ if (E->key()->pass==pass)
+ continue;
+
+ E->key()->pass=pass;
+
+// if (use_aabb && !p_aabb.intersects(E->key()->aabb))
+// continue;
+
+ if (!E->key()->aabb.intersects_segment(p_from,p_to))
+ continue;
+
+ p_results[cullcount]=E->key()->owner;
+ p_result_indices[cullcount]=E->key()->subindex;
+ cullcount++;
+
+
+ }
+
return cullcount;
}
@@ -547,6 +633,27 @@ int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_
}
+ for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
+
+ if (cullcount>=p_max_results)
+ break;
+ if (E->key()->pass==pass)
+ continue;
+
+ E->key()->pass=pass;
+
+ if (!p_aabb.intersects(E->key()->aabb))
+ continue;
+
+// if (!E->key()->aabb.intersects_segment(p_from,p_to))
+// continue;
+
+ p_results[cullcount]=E->key()->owner;
+ p_result_indices[cullcount]=E->key()->subindex;
+ cullcount++;
+
+
+ }
return cullcount;
}
@@ -581,6 +688,7 @@ BroadPhase2DHashGrid::BroadPhase2DHashGrid() {
hash_table = memnew_arr( PosBin*, hash_table_size);
cell_size = GLOBAL_DEF("physics_2d/cell_size",128);
+ large_object_min_surface = GLOBAL_DEF("physics_2d/large_object_surface_treshold_in_cells",512);
for(int i=0;i<hash_table_size;i++)
hash_table[i]=NULL;
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h
index bda5ea21cf..561d488484 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.h
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.h
@@ -55,8 +55,26 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
};
+ struct RC {
+
+ int ref;
+
+ _FORCE_INLINE_ int inc() {
+ ref++;
+ return ref;
+ }
+ _FORCE_INLINE_ int dec() {
+ ref--;
+ return ref;
+ }
+
+ _FORCE_INLINE_ RC() {
+ ref=0;
+ }
+ };
Map<ID,Element> element_map;
+ Map<Element*,RC> large_elements;
ID current;
@@ -86,6 +104,7 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
Map<PairKey,PairData> pair_map;
int cell_size;
+ int large_object_min_surface;
PairCallback pair_callback;
void *pair_userdata;
@@ -127,23 +146,7 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
};
- struct RC {
-
- int ref;
-
- _FORCE_INLINE_ int inc() {
- ref++;
- return ref;
- }
- _FORCE_INLINE_ int dec() {
- ref--;
- return ref;
- }
- _FORCE_INLINE_ RC() {
- ref=0;
- }
- };
struct PosBin {
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
index f22b676304..a6d12bdada 100644
--- a/servers/physics_2d/collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -77,6 +77,7 @@ _FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 * p_point
struct _generate_contacts_Pair {
+ bool a;
int idx;
float d;
_FORCE_INLINE_ bool operator <(const _generate_contacts_Pair& l) const { return d< l.d; }
@@ -89,12 +90,14 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points
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]};
@@ -201,41 +204,55 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points
}
}
+#endif
+
+#if 1
-#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]) };
+ 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];
- for(int i=0;i<4;i++) {
- dvec[i].d=axis.dot(p_points_A[0]-A[i]);
- dvec[i].idx=i;
- }
+
+ 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++) {
- 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);
+ 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();
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-24 03:30:10 +0000