summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/area_pair_2d_sw.cpp102
-rw-r--r--servers/physics_2d/area_pair_2d_sw.h32
-rw-r--r--servers/physics_2d/body_2d_sw.cpp35
-rw-r--r--servers/physics_2d/body_pair_2d_sw.cpp89
-rw-r--r--servers/physics_2d/body_pair_2d_sw.h24
-rw-r--r--servers/physics_2d/collision_object_2d_sw.h4
-rw-r--r--servers/physics_2d/constraint_2d_sw.h1
-rw-r--r--servers/physics_2d/joints_2d_sw.cpp92
-rw-r--r--servers/physics_2d/joints_2d_sw.h31
-rw-r--r--servers/physics_2d/step_2d_sw.cpp157
-rw-r--r--servers/physics_2d/step_2d_sw.h15
11 files changed, 365 insertions, 217 deletions
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index 21ad57e344..eb9fc0e800 100644
--- a/servers/physics_2d/area_pair_2d_sw.cpp
+++ b/servers/physics_2d/area_pair_2d_sw.cpp
@@ -40,31 +40,48 @@ bool AreaPair2DSW::setup(real_t p_step) {
result = true;
}
+ process_collision = false;
if (result != colliding) {
- if (result) {
- if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
- body->add_area(area);
- }
- if (area->has_monitor_callback()) {
- area->add_body_to_query(body, body_shape, area_shape);
- }
-
- } else {
- if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
- body->remove_area(area);
- }
- if (area->has_monitor_callback()) {
- area->remove_body_from_query(body, body_shape, area_shape);
- }
+ if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+ process_collision = true;
+ } else if (area->has_monitor_callback()) {
+ process_collision = true;
}
colliding = result;
}
- return false; //never do any post solving
+ return process_collision;
+}
+
+bool AreaPair2DSW::pre_solve(real_t p_step) {
+ if (!process_collision) {
+ return false;
+ }
+
+ if (colliding) {
+ if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+ body->add_area(area);
+ }
+
+ if (area->has_monitor_callback()) {
+ area->add_body_to_query(body, body_shape, area_shape);
+ }
+ } else {
+ if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+ body->remove_area(area);
+ }
+
+ if (area->has_monitor_callback()) {
+ area->remove_body_from_query(body, body_shape, area_shape);
+ }
+ }
+
+ return false; // Never do any post solving.
}
void AreaPair2DSW::solve(real_t p_step) {
+ // Nothing to do.
}
AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) {
@@ -72,7 +89,6 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area,
area = p_area;
body_shape = p_body_shape;
area_shape = p_area_shape;
- colliding = false;
body->add_constraint(this, 0);
area->add_constraint(this);
if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair
@@ -103,33 +119,48 @@ bool Area2Pair2DSW::setup(real_t p_step) {
result = true;
}
+ process_collision = false;
if (result != colliding) {
- if (result) {
- if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
- area_b->add_area_to_query(area_a, shape_a, shape_b);
- }
-
- if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
- area_a->add_area_to_query(area_b, shape_b, shape_a);
- }
-
- } else {
- if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
- area_b->remove_area_from_query(area_a, shape_a, shape_b);
- }
-
- if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
- area_a->remove_area_from_query(area_b, shape_b, shape_a);
- }
+ if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
+ process_collision = true;
+ } else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
+ process_collision = true;
}
colliding = result;
}
- return false; //never do any post solving
+ return process_collision;
+}
+
+bool Area2Pair2DSW::pre_solve(real_t p_step) {
+ if (!process_collision) {
+ return false;
+ }
+
+ if (colliding) {
+ if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
+ area_b->add_area_to_query(area_a, shape_a, shape_b);
+ }
+
+ if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
+ area_a->add_area_to_query(area_b, shape_b, shape_a);
+ }
+ } else {
+ if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
+ area_b->remove_area_from_query(area_a, shape_a, shape_b);
+ }
+
+ if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
+ area_a->remove_area_from_query(area_b, shape_b, shape_a);
+ }
+ }
+
+ return false; // Never do any post solving.
}
void Area2Pair2DSW::solve(real_t p_step) {
+ // Nothing to do.
}
Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) {
@@ -137,7 +168,6 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area
area_b = p_area_b;
shape_a = p_shape_a;
shape_b = p_shape_b;
- colliding = false;
area_a->add_constraint(this);
area_b->add_constraint(this);
}
diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h
index 4015aad5d1..4632a307d9 100644
--- a/servers/physics_2d/area_pair_2d_sw.h
+++ b/servers/physics_2d/area_pair_2d_sw.h
@@ -36,30 +36,34 @@
#include "constraint_2d_sw.h"
class AreaPair2DSW : public Constraint2DSW {
- Body2DSW *body;
- Area2DSW *area;
- int body_shape;
- int area_shape;
- bool colliding;
+ Body2DSW *body = nullptr;
+ Area2DSW *area = nullptr;
+ int body_shape = 0;
+ int area_shape = 0;
+ bool colliding = false;
+ bool process_collision = false;
public:
- bool setup(real_t p_step);
- void solve(real_t p_step);
+ virtual bool setup(real_t p_step) override;
+ virtual bool pre_solve(real_t p_step) override;
+ virtual void solve(real_t p_step) override;
AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape);
~AreaPair2DSW();
};
class Area2Pair2DSW : public Constraint2DSW {
- Area2DSW *area_a;
- Area2DSW *area_b;
- int shape_a;
- int shape_b;
- bool colliding;
+ Area2DSW *area_a = nullptr;
+ Area2DSW *area_b = nullptr;
+ int shape_a = 0;
+ int shape_b = 0;
+ bool colliding = false;
+ bool process_collision = false;
public:
- bool setup(real_t p_step);
- void solve(real_t p_step);
+ virtual bool setup(real_t p_step) override;
+ virtual bool pre_solve(real_t p_step) override;
+ virtual void solve(real_t p_step) override;
Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b);
~Area2Pair2DSW();
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 9306fea70c..f78a487e27 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -104,31 +104,17 @@ void Body2DSW::set_active(bool p_active) {
}
active = p_active;
- if (!p_active) {
- if (get_space()) {
- get_space()->body_remove_from_active_list(&active_list);
- }
- } else {
+
+ if (active) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
- return; //static bodies can't become active
- }
- if (get_space()) {
+ // Static bodies can't be active.
+ active = false;
+ } else if (get_space()) {
get_space()->body_add_to_active_list(&active_list);
}
-
- //still_time=0;
- }
- /*
- if (!space)
- return;
-
- for(int i=0;i<get_shape_count();i++) {
- Shape &s=shapes[i];
- if (s.bpid>0) {
- get_space()->get_broadphase()->set_active(s.bpid,active);
- }
+ } else if (get_space()) {
+ get_space()->body_remove_from_active_list(&active_list);
}
-*/
}
void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) {
@@ -370,13 +356,6 @@ void Body2DSW::set_space(Space2DSW *p_space) {
if (active) {
get_space()->body_add_to_active_list(&active_list);
}
- /*
- _update_queries();
- if (is_active()) {
- active=false;
- set_active(true);
- }
- */
}
first_integration = false;
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index da70ac7d4b..ff31825b83 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -87,10 +87,13 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto
int least_deep = -1;
real_t min_depth = 1e10;
+ const Transform2D &transform_A = A->get_transform();
+ const Transform2D &transform_B = B->get_transform();
+
for (int i = 0; i <= contact_count; i++) {
Contact &c = (i == contact_count) ? contact : contacts[i];
- Vector2 global_A = A->get_transform().basis_xform(c.local_A);
- Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
+ Vector2 global_A = transform_A.basis_xform(c.local_A);
+ Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
@@ -124,6 +127,9 @@ void BodyPair2DSW::_validate_contacts() {
real_t max_separation = space->get_contact_max_separation();
real_t max_separation2 = max_separation * max_separation;
+ const Transform2D &transform_A = A->get_transform();
+ const Transform2D &transform_B = B->get_transform();
+
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
@@ -134,8 +140,8 @@ void BodyPair2DSW::_validate_contacts() {
} else {
c.reused = false;
- Vector2 global_A = A->get_transform().basis_xform(c.local_A);
- Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
+ Vector2 global_A = transform_A.basis_xform(c.local_A);
+ Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
@@ -220,14 +226,16 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) {
}
bool BodyPair2DSW::setup(real_t p_step) {
- //cannot collide
+ dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+ dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false;
return false;
}
- bool report_contacts_only = false;
- if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
+ report_contacts_only = false;
+ if (!dynamic_A && !dynamic_B) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true;
} else {
@@ -246,12 +254,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
_validate_contacts();
- Vector2 offset_A = A->get_transform().get_origin();
+ const Vector2 &offset_A = A->get_transform().get_origin();
Transform2D xform_Au = A->get_transform().untranslated();
Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
Transform2D xform_Bu = B->get_transform();
- xform_Bu.elements[2] -= A->get_transform().get_origin();
+ xform_Bu.elements[2] -= offset_A;
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
@@ -272,13 +280,13 @@ bool BodyPair2DSW::setup(real_t p_step) {
if (!collided) {
//test ccd (currently just a raycast)
- if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+ if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_A) {
if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) {
collided = true;
}
}
- if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+ if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_B) {
if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) {
collided = true;
}
@@ -338,9 +346,21 @@ bool BodyPair2DSW::setup(real_t p_step) {
}
}
+ return true;
+}
+
+bool BodyPair2DSW::pre_solve(real_t p_step) {
+ if (!collided || oneway_disabled) {
+ return false;
+ }
+
real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = 0.3;
+
+ Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
+ Shape2DSW *shape_B_ptr = B->get_shape(shape_B);
+
if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
if (shape_A_ptr->get_custom_bias() == 0) {
bias = shape_B_ptr->get_custom_bias();
@@ -351,21 +371,23 @@ bool BodyPair2DSW::setup(real_t p_step) {
}
}
- cc = 0;
-
real_t inv_dt = 1.0 / p_step;
bool do_process = false;
+ const Vector2 &offset_A = A->get_transform().get_origin();
+ const Transform2D &transform_A = A->get_transform();
+ const Transform2D &transform_B = B->get_transform();
+
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
-
c.active = false;
- Vector2 global_A = xform_Au.xform(c.local_A);
- Vector2 global_B = xform_Bu.xform(c.local_B);
+ Vector2 global_A = transform_A.basis_xform(c.local_A);
+ Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
- real_t depth = c.normal.dot(global_A - global_B);
+ Vector2 axis = global_A - global_B;
+ real_t depth = axis.dot(c.normal);
if (depth <= 0 || !c.reused) {
continue;
@@ -396,8 +418,6 @@ bool BodyPair2DSW::setup(real_t p_step) {
continue;
}
- c.active = true;
-
// Precompute normal mass, tangent mass, and bias.
real_t rnA = c.rA.dot(c.normal);
real_t rnB = c.rB.dot(c.normal);
@@ -421,8 +441,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
// Apply normal + friction impulse
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
- A->apply_impulse(-P, c.rA);
- B->apply_impulse(P, c.rB);
+ if (dynamic_A) {
+ A->apply_impulse(-P, c.rA);
+ }
+ if (dynamic_B) {
+ B->apply_impulse(P, c.rB);
+ }
}
#endif
@@ -434,6 +458,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
c.bounce = c.bounce * dv.dot(c.normal);
}
+ c.active = true;
do_process = true;
}
@@ -441,13 +466,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
}
void BodyPair2DSW::solve(real_t p_step) {
- if (!collided) {
+ if (!collided || oneway_disabled) {
return;
}
for (int i = 0; i < contact_count; ++i) {
Contact &c = contacts[i];
- cc++;
if (!c.active) {
continue;
@@ -474,8 +498,12 @@ void BodyPair2DSW::solve(real_t p_step) {
Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
- A->apply_bias_impulse(-jb, c.rA);
- B->apply_bias_impulse(jb, c.rB);
+ if (dynamic_A) {
+ A->apply_bias_impulse(-jb, c.rA);
+ }
+ if (dynamic_B) {
+ B->apply_bias_impulse(jb, c.rB);
+ }
real_t jn = -(c.bounce + vn) * c.mass_normal;
real_t jnOld = c.acc_normal_impulse;
@@ -490,8 +518,12 @@ void BodyPair2DSW::solve(real_t p_step) {
Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
- A->apply_impulse(-j, c.rA);
- B->apply_impulse(j, c.rB);
+ if (dynamic_A) {
+ A->apply_impulse(-j, c.rA);
+ }
+ if (dynamic_B) {
+ B->apply_impulse(j, c.rB);
+ }
}
}
@@ -504,9 +536,6 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh
space = A->get_space();
A->add_constraint(this, 0);
B->add_constraint(this, 1);
- contact_count = 0;
- collided = false;
- oneway_disabled = false;
}
BodyPair2DSW::~BodyPair2DSW() {
diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h
index 31ab9b9017..4b42b44c92 100644
--- a/servers/physics_2d/body_pair_2d_sw.h
+++ b/servers/physics_2d/body_pair_2d_sw.h
@@ -44,13 +44,16 @@ class BodyPair2DSW : public Constraint2DSW {
Body2DSW *B;
};
- Body2DSW *_arr[2];
+ Body2DSW *_arr[2] = { nullptr, nullptr };
};
- int shape_A;
- int shape_B;
+ int shape_A = 0;
+ int shape_B = 0;
- Space2DSW *space;
+ bool dynamic_A = false;
+ bool dynamic_B = false;
+
+ Space2DSW *space = nullptr;
struct Contact {
Vector2 position;
@@ -73,10 +76,10 @@ class BodyPair2DSW : public Constraint2DSW {
Vector2 sep_axis;
Contact contacts[MAX_CONTACTS];
- int contact_count;
- bool collided;
- bool oneway_disabled;
- int cc;
+ int contact_count = 0;
+ bool collided = false;
+ bool oneway_disabled = false;
+ bool report_contacts_only = false;
bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false);
void _validate_contacts();
@@ -84,8 +87,9 @@ class BodyPair2DSW : public Constraint2DSW {
_FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
public:
- bool setup(real_t p_step);
- void solve(real_t p_step);
+ virtual bool setup(real_t p_step) override;
+ virtual bool pre_solve(real_t p_step) override;
+ virtual void solve(real_t p_step) override;
BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B);
~BodyPair2DSW();
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 2db3961f41..c395e59694 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -143,8 +143,8 @@ public:
return shapes[p_index].metadata;
}
- _FORCE_INLINE_ Transform2D get_transform() const { return transform; }
- _FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; }
+ _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; }
+ _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ Space2DSW *get_space() const { return space; }
void set_shape_as_disabled(int p_idx, bool p_disabled);
diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h
index b724deb48e..5e8dfbe570 100644
--- a/servers/physics_2d/constraint_2d_sw.h
+++ b/servers/physics_2d/constraint_2d_sw.h
@@ -63,6 +63,7 @@ public:
_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
virtual bool setup(real_t p_step) = 0;
+ virtual bool pre_solve(real_t p_step) = 0;
virtual void solve(real_t p_step) = 0;
virtual ~Constraint2DSW() {}
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index 20d4b9aa1a..eaec582f9b 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -97,7 +97,10 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto
}
bool PinJoint2DSW::setup(real_t p_step) {
- if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
+ dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+ dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+ if (!dynamic_A && !dynamic_B) {
return false;
}
@@ -148,12 +151,6 @@ bool PinJoint2DSW::setup(real_t p_step) {
bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step);
- // apply accumulated impulse
- A->apply_impulse(-P, rA);
- if (B) {
- B->apply_impulse(P, rB);
- }
-
return true;
}
@@ -161,6 +158,18 @@ inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) {
return Vector2(p_other * p_vec.y, -p_other * p_vec.x);
}
+bool PinJoint2DSW::pre_solve(real_t p_step) {
+ // Apply accumulated impulse.
+ if (dynamic_A) {
+ A->apply_impulse(-P, rA);
+ }
+ if (B && dynamic_B) {
+ B->apply_impulse(P, rB);
+ }
+
+ return true;
+}
+
void PinJoint2DSW::solve(real_t p_step) {
// compute relative velocity
Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity());
@@ -174,8 +183,10 @@ void PinJoint2DSW::solve(real_t p_step) {
Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P);
- A->apply_impulse(-impulse, rA);
- if (B) {
+ if (dynamic_A) {
+ A->apply_impulse(-impulse, rA);
+ }
+ if (B && dynamic_B) {
B->apply_impulse(impulse, rB);
}
@@ -262,14 +273,19 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
}
bool GrooveJoint2DSW::setup(real_t p_step) {
- if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
+ dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+ dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+ if (!dynamic_A && !dynamic_B) {
return false;
}
+ Space2DSW *space = A->get_space();
+ ERR_FAIL_COND_V(!space, false);
+
// calculate endpoints in worldspace
Vector2 ta = A->get_transform().xform(A_groove_1);
Vector2 tb = A->get_transform().xform(A_groove_2);
- Space2DSW *space = A->get_space();
// calculate axis
Vector2 n = -(tb - ta).orthogonal().normalized();
@@ -308,14 +324,22 @@ bool GrooveJoint2DSW::setup(real_t p_step) {
real_t _b = get_bias();
gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).clamped(get_max_bias());
- // apply accumulated impulse
- A->apply_impulse(-jn_acc, rA);
- B->apply_impulse(jn_acc, rB);
-
correct = true;
return true;
}
+bool GrooveJoint2DSW::pre_solve(real_t p_step) {
+ // Apply accumulated impulse.
+ if (dynamic_A) {
+ A->apply_impulse(-jn_acc, rA);
+ }
+ if (dynamic_B) {
+ B->apply_impulse(jn_acc, rB);
+ }
+
+ return true;
+}
+
void GrooveJoint2DSW::solve(real_t p_step) {
// compute impulse
Vector2 vr = relative_velocity(A, B, rA, rB);
@@ -328,8 +352,12 @@ void GrooveJoint2DSW::solve(real_t p_step) {
j = jn_acc - jOld;
- A->apply_impulse(-j, rA);
- B->apply_impulse(j, rB);
+ if (dynamic_A) {
+ A->apply_impulse(-j, rA);
+ }
+ if (dynamic_B) {
+ B->apply_impulse(j, rB);
+ }
}
GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) :
@@ -351,7 +379,10 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
//////////////////////////////////////////////
bool DampedSpringJoint2DSW::setup(real_t p_step) {
- if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
+ dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+ dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+ if (!dynamic_A && !dynamic_B) {
return false;
}
@@ -373,12 +404,21 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) {
target_vrn = 0.0f;
v_coef = 1.0f - Math::exp(-damping * (p_step)*k);
- // apply spring force
+ // Calculate spring force.
real_t f_spring = (rest_length - dist) * stiffness;
- Vector2 j = n * f_spring * (p_step);
+ j = n * f_spring * (p_step);
- A->apply_impulse(-j, rA);
- B->apply_impulse(j, rB);
+ return true;
+}
+
+bool DampedSpringJoint2DSW::pre_solve(real_t p_step) {
+ // Apply spring force.
+ if (dynamic_A) {
+ A->apply_impulse(-j, rA);
+ }
+ if (dynamic_B) {
+ B->apply_impulse(j, rB);
+ }
return true;
}
@@ -393,8 +433,12 @@ void DampedSpringJoint2DSW::solve(real_t p_step) {
target_vrn = vrn + v_damp;
Vector2 j = n * v_damp * n_mass;
- A->apply_impulse(-j, rA);
- B->apply_impulse(j, rB);
+ if (dynamic_A) {
+ A->apply_impulse(-j, rA);
+ }
+ if (dynamic_B) {
+ B->apply_impulse(j, rB);
+ }
}
void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) {
diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h
index 628de972ae..ccc5c585a0 100644
--- a/servers/physics_2d/joints_2d_sw.h
+++ b/servers/physics_2d/joints_2d_sw.h
@@ -39,6 +39,10 @@ class Joint2DSW : public Constraint2DSW {
real_t bias;
real_t max_bias;
+protected:
+ bool dynamic_A = false;
+ bool dynamic_B = false;
+
public:
_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; }
_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
@@ -49,8 +53,9 @@ public:
_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; }
_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
- virtual bool setup(real_t p_step) { return false; }
- virtual void solve(real_t p_step) {}
+ virtual bool setup(real_t p_step) override { return false; }
+ virtual bool pre_solve(real_t p_step) override { return false; }
+ virtual void solve(real_t p_step) override {}
void copy_settings_from(Joint2DSW *p_joint);
@@ -90,10 +95,11 @@ class PinJoint2DSW : public Joint2DSW {
real_t softness;
public:
- virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_PIN; }
+ virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; }
- virtual bool setup(real_t p_step);
- virtual void solve(real_t p_step);
+ virtual bool setup(real_t p_step) override;
+ virtual bool pre_solve(real_t p_step) override;
+ virtual void solve(real_t p_step) override;
void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
@@ -126,10 +132,11 @@ class GrooveJoint2DSW : public Joint2DSW {
bool correct;
public:
- virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_GROOVE; }
+ virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; }
- virtual bool setup(real_t p_step);
- virtual void solve(real_t p_step);
+ virtual bool setup(real_t p_step) override;
+ virtual bool pre_solve(real_t p_step) override;
+ virtual void solve(real_t p_step) override;
GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b);
};
@@ -153,15 +160,17 @@ class DampedSpringJoint2DSW : public Joint2DSW {
Vector2 rA, rB;
Vector2 n;
+ Vector2 j;
real_t n_mass;
real_t target_vrn;
real_t v_coef;
public:
- virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; }
+ virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; }
- virtual bool setup(real_t p_step);
- virtual void solve(real_t p_step);
+ virtual bool setup(real_t p_step) override;
+ virtual bool pre_solve(real_t p_step) override;
+ virtual void solve(real_t p_step) override;
void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const;
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index 406d750776..b8cb4cddc5 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -29,45 +29,59 @@
/*************************************************************************/
#include "step_2d_sw.h"
+
#include "core/os/os.h"
#define BODY_ISLAND_COUNT_RESERVE 128
#define BODY_ISLAND_SIZE_RESERVE 512
#define ISLAND_COUNT_RESERVE 128
#define ISLAND_SIZE_RESERVE 512
+#define CONSTRAINT_COUNT_RESERVE 1024
void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) {
p_body->set_island_step(_step);
- p_body_island.push_back(p_body);
- // Faster with reversed iterations.
- for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().back(); E; E = E->prev()) {
- Constraint2DSW *c = (Constraint2DSW *)E->get().first;
- if (c->get_island_step() == _step) {
- continue; //already processed
+ if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+ // Only dynamic bodies are tested for activation.
+ p_body_island.push_back(p_body);
+ }
+
+ for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) {
+ Constraint2DSW *constraint = (Constraint2DSW *)E->get().first;
+ if (constraint->get_island_step() == _step) {
+ continue; // Already processed.
}
- c->set_island_step(_step);
- p_constraint_island.push_back(c);
+ constraint->set_island_step(_step);
+ p_constraint_island.push_back(constraint);
+ all_constraints.push_back(constraint);
- for (int i = 0; i < c->get_body_count(); i++) {
+ for (int i = 0; i < constraint->get_body_count(); i++) {
if (i == E->get().second) {
continue;
}
- Body2DSW *b = c->get_body_ptr()[i];
- if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
- continue; //no go
+ Body2DSW *other_body = constraint->get_body_ptr()[i];
+ if (other_body->get_island_step() == _step) {
+ continue; // Already processed.
+ }
+ if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) {
+ continue; // Static bodies don't connect islands.
}
- _populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island);
+ _populate_island(other_body, p_body_island, p_constraint_island);
}
}
}
-void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta) {
+void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
+ Constraint2DSW *constraint = all_constraints[p_constraint_index];
+ constraint->setup(delta);
+}
+
+void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const {
uint32_t constraint_count = p_constraint_island.size();
uint32_t valid_constraint_count = 0;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
Constraint2DSW *constraint = p_constraint_island[constraint_index];
- if (p_constraint_island[constraint_index]->setup(p_delta)) {
+ if (p_constraint_island[constraint_index]->pre_solve(delta)) {
// Keep this constraint for solving.
p_constraint_island[valid_constraint_count++] = constraint;
}
@@ -75,27 +89,25 @@ void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island,
p_constraint_island.resize(valid_constraint_count);
}
-void Step2DSW::_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta) {
- for (int i = 0; i < p_iterations; i++) {
- uint32_t constraint_count = p_constraint_island.size();
+void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const {
+ const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index];
+
+ for (int i = 0; i < iterations; i++) {
+ uint32_t constraint_count = constraint_island.size();
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
- p_constraint_island[constraint_index]->solve(p_delta);
+ constraint_island[constraint_index]->solve(delta);
}
}
}
-void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta) {
+void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const {
bool can_sleep = true;
uint32_t body_count = p_body_island.size();
for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body2DSW *body = p_body_island[body_index];
- if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
- continue; // Ignore for static.
- }
-
- if (!body->sleep_test(p_delta)) {
+ if (!body->sleep_test(delta)) {
can_sleep = false;
}
}
@@ -104,10 +116,6 @@ void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real
for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body2DSW *body = p_body_island[body_index];
- if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
- continue; // Ignore for static.
- }
-
bool active = body->is_active();
if (active == can_sleep) {
@@ -121,6 +129,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
p_space->setup(); //update inertias, etc
+ iterations = p_iterations;
+ delta = p_delta;
+
const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list();
/* INTEGRATE FORCES */
@@ -145,12 +156,39 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
profile_begtime = profile_endtime;
}
- /* GENERATE CONSTRAINT ISLANDS */
+ /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
+
+ uint32_t island_count = 0;
+
+ const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
+
+ while (aml.first()) {
+ for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
+ Constraint2DSW *constraint = E->get();
+ if (constraint->get_island_step() == _step) {
+ continue;
+ }
+ constraint->set_island_step(_step);
+
+ // Each constraint can be on a separate island for areas as there's no solving phase.
+ ++island_count;
+ if (constraint_islands.size() < island_count) {
+ constraint_islands.resize(island_count);
+ }
+ LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1];
+ constraint_island.clear();
+
+ all_constraints.push_back(constraint);
+ constraint_island.push_back(constraint);
+ }
+ p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here
+ }
+
+ /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
b = body_list->first();
uint32_t body_island_count = 0;
- uint32_t island_count = 0;
while (b) {
Body2DSW *body = b->self();
@@ -174,7 +212,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
_populate_island(body, body_island, constraint_island);
- body_islands.push_back(body_island);
+ if (body_island.is_empty()) {
+ --body_island_count;
+ }
if (constraint_island.is_empty()) {
--island_count;
@@ -185,37 +225,16 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
p_space->set_island_count((int)island_count);
- const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
-
- while (aml.first()) {
- for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
- Constraint2DSW *c = E->get();
- if (c->get_island_step() == _step) {
- continue;
- }
- c->set_island_step(_step);
- ++island_count;
- if (constraint_islands.size() < island_count) {
- constraint_islands.resize(island_count);
- }
- LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1];
- constraint_island.clear();
- constraint_island.push_back(c);
- }
- p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here
- }
-
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
- /* SETUP CONSTRAINT ISLANDS */
+ /* SETUP CONSTRAINTS / PROCESS COLLISIONS */
- for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
- _setup_island(constraint_islands[island_index], p_delta);
- }
+ uint32_t total_contraint_count = all_constraints.size();
+ work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr);
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -223,10 +242,21 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
profile_begtime = profile_endtime;
}
- /* SOLVE CONSTRAINT ISLANDS */
+ /* PRE-SOLVE CONSTRAINT ISLANDS */
+ // Warning: This doesn't run on threads, because it involves thread-unsafe processing.
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
- _solve_island(constraint_islands[island_index], p_iterations, p_delta);
+ _pre_solve_island(constraint_islands[island_index]);
+ }
+
+ /* SOLVE CONSTRAINT ISLANDS */
+
+ // Warning: _solve_island modifies the constraint islands for optimization purpose,
+ // their content is not reliable after these calls and shouldn't be used anymore.
+ if (island_count > 1) {
+ work_pool.do_work(island_count, this, &Step2DSW::_solve_island, nullptr);
+ } else if (island_count > 0) {
+ _solve_island(0);
}
{ //profile
@@ -247,7 +277,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
/* SLEEP / WAKE UP ISLANDS */
for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
- _check_suspend(body_islands[island_index], p_delta);
+ _check_suspend(body_islands[island_index]);
}
{ //profile
@@ -256,6 +286,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
//profile_begtime=profile_endtime;
}
+ all_constraints.clear();
+
p_space->update();
p_space->unlock();
_step++;
@@ -266,4 +298,11 @@ Step2DSW::Step2DSW() {
body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
constraint_islands.reserve(ISLAND_COUNT_RESERVE);
+ all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
+
+ work_pool.init();
+}
+
+Step2DSW::~Step2DSW() {
+ work_pool.finish();
}
diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h
index 5af4a36f52..c51fd73a79 100644
--- a/servers/physics_2d/step_2d_sw.h
+++ b/servers/physics_2d/step_2d_sw.h
@@ -34,21 +34,30 @@
#include "space_2d_sw.h"
#include "core/templates/local_vector.h"
+#include "core/templates/thread_work_pool.h"
class Step2DSW {
uint64_t _step;
+ int iterations = 0;
+ real_t delta = 0.0;
+
+ ThreadWorkPool work_pool;
+
LocalVector<LocalVector<Body2DSW *>> body_islands;
LocalVector<LocalVector<Constraint2DSW *>> constraint_islands;
+ LocalVector<Constraint2DSW *> all_constraints;
void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island);
- void _setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta);
- void _solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta);
- void _check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta);
+ void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
+ void _pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const;
+ void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
+ void _check_suspend(LocalVector<Body2DSW *> &p_body_island) const;
public:
void step(Space2DSW *p_space, real_t p_delta, int p_iterations);
Step2DSW();
+ ~Step2DSW();
};
#endif // STEP_2D_SW_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-26 09:20:23 +0000