summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/godot_area_2d.cpp60
-rw-r--r--servers/physics_2d/godot_area_2d.h31
-rw-r--r--servers/physics_2d/godot_body_2d.cpp4
-rw-r--r--servers/physics_2d/godot_body_2d.h4
-rw-r--r--servers/physics_2d/godot_body_pair_2d.cpp12
-rw-r--r--servers/physics_2d/godot_broad_phase_2d_bvh.cpp4
-rw-r--r--servers/physics_2d/godot_broad_phase_2d_bvh.h24
-rw-r--r--servers/physics_2d/godot_collision_object_2d.h12
-rw-r--r--servers/physics_2d/godot_collision_solver_2d.cpp8
-rw-r--r--servers/physics_2d/godot_collision_solver_2d_sat.cpp68
-rw-r--r--servers/physics_2d/godot_physics_server_2d.cpp38
-rw-r--r--servers/physics_2d/godot_physics_server_2d.h5
-rw-r--r--servers/physics_2d/godot_shape_2d.cpp20
-rw-r--r--servers/physics_2d/godot_shape_2d.h6
-rw-r--r--servers/physics_2d/godot_space_2d.cpp40
-rw-r--r--servers/physics_2d/godot_space_2d.h4
-rw-r--r--servers/physics_2d/godot_step_2d.cpp15
-rw-r--r--servers/physics_2d/godot_step_2d.h4
18 files changed, 202 insertions, 157 deletions
diff --git a/servers/physics_2d/godot_area_2d.cpp b/servers/physics_2d/godot_area_2d.cpp
index 9937178550..af90f96438 100644
--- a/servers/physics_2d/godot_area_2d.cpp
+++ b/servers/physics_2d/godot_area_2d.cpp
@@ -225,27 +225,33 @@ void GodotArea2D::call_queries() {
resptr[i] = &res[i];
}
- for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
- if (E->get().state == 0) { // Nothing happened
- Map<BodyKey, BodyState>::Element *next = E->next();
- monitored_bodies.erase(E);
+ for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) {
+ if (E->value.state == 0) { // Nothing happened
+ HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
+ ++next;
+ monitored_bodies.remove(E);
E = next;
continue;
}
- res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
- res[1] = E->key().rid;
- res[2] = E->key().instance_id;
- res[3] = E->key().body_shape;
- res[4] = E->key().area_shape;
+ res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
+ res[1] = E->key.rid;
+ res[2] = E->key.instance_id;
+ res[3] = E->key.body_shape;
+ res[4] = E->key.area_shape;
- Map<BodyKey, BodyState>::Element *next = E->next();
- monitored_bodies.erase(E);
+ HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
+ ++next;
+ monitored_bodies.remove(E);
E = next;
Callable::CallError ce;
Variant ret;
- monitor_callback.call((const Variant **)resptr, 5, ret, ce);
+ monitor_callback.callp((const Variant **)resptr, 5, ret, ce);
+
+ if (ce.error != Callable::CallError::CALL_OK) {
+ ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(monitor_callback, (const Variant **)resptr, 5, ce));
+ }
}
} else {
monitored_bodies.clear();
@@ -261,27 +267,33 @@ void GodotArea2D::call_queries() {
resptr[i] = &res[i];
}
- for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
- if (E->get().state == 0) { // Nothing happened
- Map<BodyKey, BodyState>::Element *next = E->next();
- monitored_areas.erase(E);
+ for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) {
+ if (E->value.state == 0) { // Nothing happened
+ HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
+ ++next;
+ monitored_areas.remove(E);
E = next;
continue;
}
- res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
- res[1] = E->key().rid;
- res[2] = E->key().instance_id;
- res[3] = E->key().body_shape;
- res[4] = E->key().area_shape;
+ res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
+ res[1] = E->key.rid;
+ res[2] = E->key.instance_id;
+ res[3] = E->key.body_shape;
+ res[4] = E->key.area_shape;
- Map<BodyKey, BodyState>::Element *next = E->next();
- monitored_areas.erase(E);
+ HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
+ ++next;
+ monitored_areas.remove(E);
E = next;
Callable::CallError ce;
Variant ret;
- area_monitor_callback.call((const Variant **)resptr, 5, ret, ce);
+ area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce);
+
+ if (ce.error != Callable::CallError::CALL_OK) {
+ ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce));
+ }
}
} else {
monitored_areas.clear();
diff --git a/servers/physics_2d/godot_area_2d.h b/servers/physics_2d/godot_area_2d.h
index 6e8078909b..221982cf78 100644
--- a/servers/physics_2d/godot_area_2d.h
+++ b/servers/physics_2d/godot_area_2d.h
@@ -68,16 +68,15 @@ class GodotArea2D : public GodotCollisionObject2D {
uint32_t body_shape = 0;
uint32_t area_shape = 0;
- _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const {
- if (rid == p_key.rid) {
- if (body_shape == p_key.body_shape) {
- return area_shape < p_key.area_shape;
- } else {
- return body_shape < p_key.body_shape;
- }
- } else {
- return rid < p_key.rid;
- }
+ static uint32_t hash(const BodyKey &p_key) {
+ uint32_t h = hash_one_uint64(p_key.rid.get_id());
+ h = hash_murmur3_one_64(p_key.instance_id, h);
+ h = hash_murmur3_one_32(p_key.area_shape, h);
+ return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h));
+ }
+
+ _FORCE_INLINE_ bool operator==(const BodyKey &p_key) const {
+ return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape;
}
_FORCE_INLINE_ BodyKey() {}
@@ -91,12 +90,12 @@ class GodotArea2D : public GodotCollisionObject2D {
_FORCE_INLINE_ void dec() { state--; }
};
- Map<BodyKey, BodyState> monitored_bodies;
- Map<BodyKey, BodyState> monitored_areas;
+ HashMap<BodyKey, BodyState, BodyKey> monitored_bodies;
+ HashMap<BodyKey, BodyState, BodyKey> monitored_areas;
- Set<GodotConstraint2D *> constraints;
+ HashSet<GodotConstraint2D *> constraints;
- virtual void _shapes_changed();
+ virtual void _shapes_changed() override;
void _queue_monitor_update();
void _set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode);
@@ -143,7 +142,7 @@ public:
_FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint) { constraints.insert(p_constraint); }
_FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint) { constraints.erase(p_constraint); }
- _FORCE_INLINE_ const Set<GodotConstraint2D *> &get_constraints() const { return constraints; }
+ _FORCE_INLINE_ const HashSet<GodotConstraint2D *> &get_constraints() const { return constraints; }
_FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
void set_monitorable(bool p_monitorable);
@@ -151,7 +150,7 @@ public:
void set_transform(const Transform2D &p_transform);
- void set_space(GodotSpace2D *p_space);
+ void set_space(GodotSpace2D *p_space) override;
void call_queries();
diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp
index 6873504f70..268beb1a55 100644
--- a/servers/physics_2d/godot_body_2d.cpp
+++ b/servers/physics_2d/godot_body_2d.cpp
@@ -683,10 +683,10 @@ void GodotBody2D::call_queries() {
Callable::CallError ce;
Variant rv;
if (fi_callback_data->udata.get_type() != Variant::NIL) {
- fi_callback_data->callable.call(vp, 2, rv, ce);
+ fi_callback_data->callable.callp(vp, 2, rv, ce);
} else {
- fi_callback_data->callable.call(vp, 1, rv, ce);
+ fi_callback_data->callable.callp(vp, 1, rv, ce);
}
}
}
diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h
index 1335a19126..4b87a69d5c 100644
--- a/servers/physics_2d/godot_body_2d.h
+++ b/servers/physics_2d/godot_body_2d.h
@@ -103,7 +103,7 @@ class GodotBody2D : public GodotCollisionObject2D {
bool can_sleep = true;
bool first_time_kinematic = false;
void _mass_properties_changed();
- virtual void _shapes_changed();
+ virtual void _shapes_changed() override;
Transform2D new_transform;
List<Pair<GodotConstraint2D *, int>> constraint_list;
@@ -302,7 +302,7 @@ public:
_FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; }
_FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
- void set_space(GodotSpace2D *p_space);
+ void set_space(GodotSpace2D *p_space) override;
void update_mass_properties();
void reset_mass_properties();
diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp
index 2bf1e5a1d4..2f2af15da7 100644
--- a/servers/physics_2d/godot_body_pair_2d.cpp
+++ b/servers/physics_2d/godot_body_pair_2d.cpp
@@ -39,7 +39,7 @@
#define MAX_BIAS_ROTATION (Math_PI / 8)
void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
- GodotBodyPair2D *self = (GodotBodyPair2D *)p_self;
+ GodotBodyPair2D *self = static_cast<GodotBodyPair2D *>(p_self);
self->_contact_added_callback(p_point_A, p_point_B);
}
@@ -203,7 +203,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
// Check one-way collision based on motion direction.
if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) {
- Vector2 direction = p_xform_B.get_axis(1).normalized();
+ Vector2 direction = p_xform_B.columns[1].normalized();
if (direction.dot(mnormal) < CMP_EPSILON) {
collided = false;
oneway_disabled = true;
@@ -260,7 +260,7 @@ bool GodotBodyPair2D::setup(real_t p_step) {
Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
Transform2D xform_Bu = B->get_transform();
- xform_Bu.elements[2] -= offset_A;
+ xform_Bu.columns[2] -= offset_A;
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
@@ -300,7 +300,7 @@ bool GodotBodyPair2D::setup(real_t p_step) {
if (!prev_collided) {
if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) {
- Vector2 direction = xform_A.get_axis(1).normalized();
+ Vector2 direction = xform_A.columns[1].normalized();
bool valid = false;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
@@ -318,7 +318,7 @@ bool GodotBodyPair2D::setup(real_t p_step) {
}
if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) {
- Vector2 direction = xform_B.get_axis(1).normalized();
+ Vector2 direction = xform_B.columns[1].normalized();
bool valid = false;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
@@ -351,7 +351,7 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
Transform2D xform_Bu = B->get_transform();
- xform_Bu.elements[2] -= offset_A;
+ xform_Bu.columns[2] -= offset_A;
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp
index 2cc60fc765..c545fec113 100644
--- a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp
+++ b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp
@@ -82,7 +82,7 @@ int GodotBroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D
}
void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B) {
- GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self);
+ GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self);
if (!bpo->pair_callback) {
return nullptr;
}
@@ -91,7 +91,7 @@ void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollis
}
void GodotBroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B, void *pairdata) {
- GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self);
+ GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self);
if (!bpo->unpair_callback) {
return;
}
diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.h b/servers/physics_2d/godot_broad_phase_2d_bvh.h
index b11ad0e75e..512111f948 100644
--- a/servers/physics_2d/godot_broad_phase_2d_bvh.h
+++ b/servers/physics_2d/godot_broad_phase_2d_bvh.h
@@ -77,22 +77,22 @@ class GodotBroadPhase2DBVH : public GodotBroadPhase2D {
public:
// 0 is an invalid ID
- virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
- virtual void move(ID p_id, const Rect2 &p_aabb);
- virtual void set_static(ID p_id, bool p_static);
- virtual void remove(ID p_id);
+ virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) override;
+ virtual void move(ID p_id, const Rect2 &p_aabb) override;
+ virtual void set_static(ID p_id, bool p_static) override;
+ virtual void remove(ID p_id) override;
- virtual GodotCollisionObject2D *get_object(ID p_id) const;
- virtual bool is_static(ID p_id) const;
- virtual int get_subindex(ID p_id) const;
+ virtual GodotCollisionObject2D *get_object(ID p_id) const override;
+ virtual bool is_static(ID p_id) const override;
+ virtual int get_subindex(ID p_id) const override;
- virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr);
- virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr);
+ virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override;
+ virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override;
- virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
- virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
+ virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) override;
+ virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) override;
- virtual void update();
+ virtual void update() override;
static GodotBroadPhase2D *_create();
GodotBroadPhase2DBVH();
diff --git a/servers/physics_2d/godot_collision_object_2d.h b/servers/physics_2d/godot_collision_object_2d.h
index 19d6e91561..7965e8a94d 100644
--- a/servers/physics_2d/godot_collision_object_2d.h
+++ b/servers/physics_2d/godot_collision_object_2d.h
@@ -70,6 +70,7 @@ private:
Transform2D inv_transform;
uint32_t collision_mask = 1;
uint32_t collision_layer = 1;
+ real_t collision_priority = 1.0;
bool _static = true;
SelfList<GodotCollisionObject2D> pending_shape_update_list;
@@ -104,7 +105,7 @@ public:
_FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; }
_FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; }
- void _shape_changed();
+ void _shape_changed() override;
_FORCE_INLINE_ Type get_type() const { return type; }
void add_shape(GodotShape2D *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false);
@@ -166,7 +167,14 @@ public:
}
_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
- void remove_shape(GodotShape2D *p_shape);
+ _FORCE_INLINE_ void set_collision_priority(real_t p_priority) {
+ ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0.");
+ collision_priority = p_priority;
+ _shape_changed();
+ }
+ _FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; }
+
+ void remove_shape(GodotShape2D *p_shape) override;
void remove_shape(int p_index);
virtual void set_space(GodotSpace2D *p_space) = 0;
diff --git a/servers/physics_2d/godot_collision_solver_2d.cpp b/servers/physics_2d/godot_collision_solver_2d.cpp
index 263d6d939f..0d7b42b80d 100644
--- a/servers/physics_2d/godot_collision_solver_2d.cpp
+++ b/servers/physics_2d/godot_collision_solver_2d.cpp
@@ -140,7 +140,7 @@ struct _ConcaveCollisionInfo2D {
Vector2 motion_B;
real_t margin_A = 0.0;
real_t margin_B = 0.0;
- GodotCollisionSolver2D::CallbackResult result_callback;
+ GodotCollisionSolver2D::CallbackResult result_callback = nullptr;
void *userdata = nullptr;
bool swap_result = false;
bool collided = false;
@@ -150,7 +150,7 @@ struct _ConcaveCollisionInfo2D {
};
bool GodotCollisionSolver2D::concave_callback(void *p_userdata, GodotShape2D *p_convex) {
- _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata);
+ _ConcaveCollisionInfo2D &cinfo = *(static_cast<_ConcaveCollisionInfo2D *>(p_userdata));
cinfo.aabb_tests++;
bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B);
@@ -185,13 +185,13 @@ bool GodotCollisionSolver2D::solve_concave(const GodotShape2D *p_shape_A, const
cinfo.aabb_tests = 0;
Transform2D rel_transform = p_transform_A;
- rel_transform.elements[2] -= p_transform_B.get_origin();
+ rel_transform.columns[2] -= p_transform_B.get_origin();
//quickly compute a local Rect2
Rect2 local_aabb;
for (int i = 0; i < 2; i++) {
- Vector2 axis(p_transform_B.elements[i]);
+ Vector2 axis(p_transform_B.columns[i]);
real_t axis_scale = 1.0 / axis.length();
axis *= axis_scale;
diff --git a/servers/physics_2d/godot_collision_solver_2d_sat.cpp b/servers/physics_2d/godot_collision_solver_2d_sat.cpp
index ded3ff356b..77186d3810 100644
--- a/servers/physics_2d/godot_collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/godot_collision_solver_2d_sat.cpp
@@ -33,7 +33,7 @@
#include "core/math/geometry_2d.h"
struct _CollectorCallback2D {
- GodotCollisionSolver2D::CallbackResult callback;
+ GodotCollisionSolver2D::CallbackResult callback = nullptr;
void *userdata = nullptr;
bool swap = false;
bool collided = false;
@@ -477,11 +477,11 @@ static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transfor
return;
}
- if (!separator.test_axis(p_transform_b.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
return;
}
- if (!separator.test_axis(p_transform_b.elements[1].normalized())) {
+ if (!separator.test_axis(p_transform_b.columns[1].normalized())) {
return;
}
@@ -548,22 +548,22 @@ static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2
return;
}
- if (!separator.test_axis(p_transform_b.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
return;
}
real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
- if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) {
+ if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
return;
}
- if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) {
+ if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
return;
}
- if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) {
+ if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
return;
}
- if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) {
+ if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
return;
}
@@ -646,8 +646,8 @@ static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform
return;
}
- const Vector2 &sphere = p_transform_a.elements[2];
- const Vector2 *axis = &p_transform_b.elements[0];
+ const Vector2 &sphere = p_transform_a.columns[2];
+ const Vector2 *axis = &p_transform_b.columns[0];
//const Vector2& half_extents = rectangle_B->get_half_extents();
if (!separator.test_axis(axis[0].normalized())) {
@@ -705,17 +705,17 @@ static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D
}
//capsule axis
- if (!separator.test_axis(p_transform_b.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
return;
}
real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
//capsule endpoints
- if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) {
+ if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
return;
}
- if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) {
+ if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
return;
}
@@ -769,20 +769,20 @@ static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transf
}
//box faces A
- if (!separator.test_axis(p_transform_a.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
return;
}
- if (!separator.test_axis(p_transform_a.elements[1].normalized())) {
+ if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
return;
}
//box faces B
- if (!separator.test_axis(p_transform_b.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
return;
}
- if (!separator.test_axis(p_transform_b.elements[1].normalized())) {
+ if (!separator.test_axis(p_transform_b.columns[1].normalized())) {
return;
}
@@ -796,10 +796,10 @@ static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transf
if (castA || castB) {
Transform2D aofs = p_transform_a;
- aofs.elements[2] += p_motion_a;
+ aofs.columns[2] += p_motion_a;
Transform2D bofs = p_transform_b;
- bofs.elements[2] += p_motion_b;
+ bofs.columns[2] += p_motion_b;
Transform2D aofsinv = aofs.affine_inverse();
Transform2D bofsinv = bofs.affine_inverse();
@@ -843,16 +843,16 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor
}
//box faces
- if (!separator.test_axis(p_transform_a.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
return;
}
- if (!separator.test_axis(p_transform_a.elements[1].normalized())) {
+ if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
return;
}
//capsule axis
- if (!separator.test_axis(p_transform_b.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
return;
}
@@ -864,7 +864,7 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor
for (int i = 0; i < 2; i++) {
{
- Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir;
+ Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
return;
@@ -872,7 +872,7 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor
}
if (castA) {
- Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir;
+ Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
capsule_endpoint -= p_motion_a;
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
@@ -881,7 +881,7 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor
}
if (castB) {
- Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir;
+ Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
capsule_endpoint += p_motion_b;
if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
@@ -890,7 +890,7 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor
}
if (castA && castB) {
- Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir;
+ Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
capsule_endpoint -= p_motion_a;
capsule_endpoint += p_motion_b;
@@ -921,11 +921,11 @@ static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const T
}
//box faces
- if (!separator.test_axis(p_transform_a.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
return;
}
- if (!separator.test_axis(p_transform_a.elements[1].normalized())) {
+ if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
return;
}
@@ -984,11 +984,11 @@ static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2
//capsule axis
- if (!separator.test_axis(p_transform_b.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
return;
}
- if (!separator.test_axis(p_transform_a.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
return;
}
@@ -996,11 +996,11 @@ static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2
real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
for (int i = 0; i < 2; i++) {
- Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir_A;
+ Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir_A;
real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
for (int j = 0; j < 2; j++) {
- Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir_B;
+ Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir_B;
if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) {
return;
@@ -1032,7 +1032,7 @@ static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Tra
//capsule axis
- if (!separator.test_axis(p_transform_a.elements[0].normalized())) {
+ if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
return;
}
@@ -1042,7 +1042,7 @@ static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Tra
real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
for (int j = 0; j < 2; j++) {
- Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir;
+ Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir;
if (TEST_POINT(capsule_endpoint_A, cpoint)) {
return;
diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp
index a9b499c6b5..c728dccd4f 100644
--- a/servers/physics_2d/godot_physics_server_2d.cpp
+++ b/servers/physics_2d/godot_physics_server_2d.cpp
@@ -144,7 +144,7 @@ real_t GodotPhysicsServer2D::shape_get_custom_solver_bias(RID p_shape) const {
}
void GodotPhysicsServer2D::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
- CollCbkData *cbk = (CollCbkData *)p_userdata;
+ CollCbkData *cbk = static_cast<CollCbkData *>(p_userdata);
if (cbk->max == 0) {
return;
@@ -718,6 +718,20 @@ uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const {
return body->get_collision_mask();
}
+void GodotPhysicsServer2D::body_set_collision_priority(RID p_body, real_t p_priority) {
+ GodotBody2D *body = body_owner.get_or_null(p_body);
+ ERR_FAIL_COND(!body);
+
+ body->set_collision_priority(p_priority);
+}
+
+real_t GodotPhysicsServer2D::body_get_collision_priority(RID p_body) const {
+ const GodotBody2D *body = body_owner.get_or_null(p_body);
+ ERR_FAIL_COND_V(!body, 0);
+
+ return body->get_collision_priority();
+}
+
void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
GodotBody2D *body = body_owner.get_or_null(p_body);
ERR_FAIL_COND(!body);
@@ -1179,7 +1193,7 @@ void GodotPhysicsServer2D::free(RID p_rid) {
GodotShape2D *shape = shape_owner.get_or_null(p_rid);
while (shape->get_owners().size()) {
- GodotShapeOwner2D *so = shape->get_owners().front()->key();
+ GodotShapeOwner2D *so = shape->get_owners().begin()->key;
so->remove_shape(shape);
}
@@ -1212,7 +1226,7 @@ void GodotPhysicsServer2D::free(RID p_rid) {
GodotSpace2D *space = space_owner.get_or_null(p_rid);
while (space->get_objects().size()) {
- GodotCollisionObject2D *co = (GodotCollisionObject2D *)space->get_objects().front()->get();
+ GodotCollisionObject2D *co = static_cast<GodotCollisionObject2D *>(*space->get_objects().begin());
co->set_space(nullptr);
}
@@ -1250,11 +1264,11 @@ void GodotPhysicsServer2D::step(real_t p_step) {
island_count = 0;
active_objects = 0;
collision_pairs = 0;
- for (Set<const GodotSpace2D *>::Element *E = active_spaces.front(); E; E = E->next()) {
- stepper->step((GodotSpace2D *)E->get(), p_step);
- island_count += E->get()->get_island_count();
- active_objects += E->get()->get_active_objects();
- collision_pairs += E->get()->get_collision_pairs();
+ for (const GodotSpace2D *E : active_spaces) {
+ stepper->step(const_cast<GodotSpace2D *>(E), p_step);
+ island_count += E->get_island_count();
+ active_objects += E->get_active_objects();
+ collision_pairs += E->get_collision_pairs();
}
}
@@ -1271,8 +1285,8 @@ void GodotPhysicsServer2D::flush_queries() {
uint64_t time_beg = OS::get_singleton()->get_ticks_usec();
- for (Set<const GodotSpace2D *>::Element *E = active_spaces.front(); E; E = E->next()) {
- GodotSpace2D *space = (GodotSpace2D *)E->get();
+ for (const GodotSpace2D *E : active_spaces) {
+ GodotSpace2D *space = const_cast<GodotSpace2D *>(E);
space->call_queries();
}
@@ -1292,9 +1306,9 @@ void GodotPhysicsServer2D::flush_queries() {
total_time[i] = 0;
}
- for (Set<const GodotSpace2D *>::Element *E = active_spaces.front(); E; E = E->next()) {
+ for (const GodotSpace2D *E : active_spaces) {
for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) {
- total_time[i] += E->get()->get_elapsed_time(GodotSpace2D::ElapsedTime(i));
+ total_time[i] += E->get_elapsed_time(GodotSpace2D::ElapsedTime(i));
}
}
diff --git a/servers/physics_2d/godot_physics_server_2d.h b/servers/physics_2d/godot_physics_server_2d.h
index 0a84caadc5..20e492d87a 100644
--- a/servers/physics_2d/godot_physics_server_2d.h
+++ b/servers/physics_2d/godot_physics_server_2d.h
@@ -56,7 +56,7 @@ class GodotPhysicsServer2D : public PhysicsServer2D {
bool flushing_queries = false;
GodotStep2D *stepper = nullptr;
- Set<const GodotSpace2D *> active_spaces;
+ HashSet<const GodotSpace2D *> active_spaces;
mutable RID_PtrOwner<GodotShape2D, true> shape_owner;
mutable RID_PtrOwner<GodotSpace2D, true> space_owner;
@@ -199,6 +199,9 @@ public:
virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
virtual uint32_t body_get_collision_mask(RID p_body) const override;
+ virtual void body_set_collision_priority(RID p_body, real_t p_priority) override;
+ virtual real_t body_get_collision_priority(RID p_body) const override;
+
virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
diff --git a/servers/physics_2d/godot_shape_2d.cpp b/servers/physics_2d/godot_shape_2d.cpp
index b5dbb8a2dd..72ade3757b 100644
--- a/servers/physics_2d/godot_shape_2d.cpp
+++ b/servers/physics_2d/godot_shape_2d.cpp
@@ -37,7 +37,7 @@ void GodotShape2D::configure(const Rect2 &p_aabb) {
aabb = p_aabb;
configured = true;
for (const KeyValue<GodotShapeOwner2D *, int> &E : owners) {
- GodotShapeOwner2D *co = (GodotShapeOwner2D *)E.key;
+ GodotShapeOwner2D *co = const_cast<GodotShapeOwner2D *>(E.key);
co->_shape_changed();
}
}
@@ -50,20 +50,20 @@ Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const {
}
void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) {
- Map<GodotShapeOwner2D *, int>::Element *E = owners.find(p_owner);
+ HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner);
if (E) {
- E->get()++;
+ E->value++;
} else {
owners[p_owner] = 1;
}
}
void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) {
- Map<GodotShapeOwner2D *, int>::Element *E = owners.find(p_owner);
+ HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner);
ERR_FAIL_COND(!E);
- E->get()--;
- if (E->get() == 0) {
- owners.erase(E);
+ E->value--;
+ if (E->value == 0) {
+ owners.remove(E);
}
}
@@ -71,7 +71,7 @@ bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const {
return owners.has(p_owner);
}
-const Map<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const {
+const HashMap<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const {
return owners;
}
@@ -841,7 +841,7 @@ void GodotConcavePolygonShape2D::set_data(const Variant &p_data) {
const Vector2 *arr = p2arr.ptr();
- Map<Point2, int> pointmap;
+ HashMap<Point2, int> pointmap;
for (int i = 0; i < len; i += 2) {
Point2 p1 = arr[i];
Point2 p2 = arr[i + 1];
@@ -868,7 +868,7 @@ void GodotConcavePolygonShape2D::set_data(const Variant &p_data) {
}
points.resize(pointmap.size());
- aabb.position = pointmap.front()->key();
+ aabb.position = pointmap.begin()->key;
for (const KeyValue<Point2, int> &E : pointmap) {
aabb.expand_to(E.key);
points.write[E.value] = E.key;
diff --git a/servers/physics_2d/godot_shape_2d.h b/servers/physics_2d/godot_shape_2d.h
index fb52cbce20..cede01e18d 100644
--- a/servers/physics_2d/godot_shape_2d.h
+++ b/servers/physics_2d/godot_shape_2d.h
@@ -50,7 +50,7 @@ class GodotShape2D {
bool configured = false;
real_t custom_bias = 0.0;
- Map<GodotShapeOwner2D *, int> owners;
+ HashMap<GodotShapeOwner2D *, int> owners;
protected:
void configure(const Rect2 &p_aabb);
@@ -86,7 +86,7 @@ public:
void add_owner(GodotShapeOwner2D *p_owner);
void remove_owner(GodotShapeOwner2D *p_owner);
bool is_owner(GodotShapeOwner2D *p_owner) const;
- const Map<GodotShapeOwner2D *, int> &get_owners() const;
+ const HashMap<GodotShapeOwner2D *, int> &get_owners() const;
_FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const {
get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount);
@@ -134,7 +134,7 @@ public:
real_t mina, maxa; \
real_t minb, maxb; \
Transform2D ofsb = p_transform; \
- ofsb.elements[2] += p_cast; \
+ ofsb.columns[2] += p_cast; \
project_range(p_normal, p_transform, mina, maxa); \
project_range(p_normal, ofsb, minb, maxb); \
r_min = MIN(mina, minb); \
diff --git a/servers/physics_2d/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp
index 99c0566fb1..4166191be8 100644
--- a/servers/physics_2d/godot_space_2d.cpp
+++ b/servers/physics_2d/godot_space_2d.cpp
@@ -403,7 +403,7 @@ struct _RestCallbackData2D {
};
static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
- _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata;
+ _RestCallbackData2D *rd = static_cast<_RestCallbackData2D *>(p_userdata);
Vector2 contact_rel = p_point_B - p_point_A;
real_t len = contact_rel.length();
@@ -594,6 +594,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
const int max_results = 32;
int recover_attempts = 4;
Vector2 sr[max_results * 2];
+ real_t priorities[max_results];
do {
GodotPhysicsServer2D::CollCbkData cbk;
@@ -606,6 +607,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
+ int priority_amount = 0;
bool collided = false;
@@ -633,7 +635,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
- cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
+ cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
@@ -664,6 +666,10 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) {
did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
}
+ while (cbk.amount > priority_amount) {
+ priorities[priority_amount] = col_obj->get_collision_priority();
+ priority_amount++;
+ }
if (!did_collide && cbk.invalid_by_dir > 0) {
//this shape must be excluded
@@ -686,6 +692,12 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
break;
}
+ real_t inv_total_weight = 0.0;
+ for (int i = 0; i < cbk.amount; i++) {
+ inv_total_weight += priorities[i];
+ }
+ inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight;
+
recovered = true;
Vector2 recover_motion;
@@ -701,7 +713,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
real_t depth = n.dot(a + recover_motion) - d;
if (depth > min_contact_depth + CMP_EPSILON) {
// Only recover if there is penetration.
- recover_motion -= n * (depth - min_contact_depth) * 0.4;
+ recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight;
}
}
@@ -710,7 +722,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
break;
}
- body_transform.elements[2] += recover_motion;
+ body_transform.columns[2] += recover_motion;
body_aabb.position += recover_motion;
recover_attempts--;
@@ -788,7 +800,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
//test initial overlap
if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
- Vector2 direction = col_obj_shape_xform.get_axis(1).normalized();
+ Vector2 direction = col_obj_shape_xform.columns[1].normalized();
if (motion_normal.dot(direction) < 0) {
continue;
}
@@ -838,7 +850,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
cbk.amount = 0;
cbk.passed = 0;
cbk.ptr = cd;
- cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
+ cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
cbk.valid_depth = 10e20;
@@ -874,14 +886,14 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
bool collided = false;
- if (recovered || (safe < 1)) {
+ if ((p_parameters.recovery_as_collision && recovered) || (safe < 1)) {
if (safe >= 1) {
best_shape = -1; //no best shape with cast, reset to -1
}
//it collided, let's get the rest info in unsafe advance
Transform2D ugt = body_transform;
- ugt.elements[2] += p_parameters.motion * unsafe;
+ ugt.columns[2] += p_parameters.motion * unsafe;
_RestCallbackData2D rcd;
@@ -929,7 +941,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
- rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
+ rcd.valid_dir = col_obj_shape_xform.columns[1].normalized();
real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
rcd.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
@@ -1005,7 +1017,7 @@ void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A
SWAP(type_A, type_B);
}
- GodotSpace2D *self = (GodotSpace2D *)p_self;
+ GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self);
self->collision_pairs++;
if (type_A == GodotCollisionObject2D::TYPE_AREA) {
@@ -1021,7 +1033,7 @@ void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A
}
} else {
- GodotBodyPair2D *b = memnew(GodotBodyPair2D((GodotBody2D *)A, p_subindex_A, (GodotBody2D *)B, p_subindex_B));
+ GodotBodyPair2D *b = memnew(GodotBodyPair2D(static_cast<GodotBody2D *>(A), p_subindex_A, static_cast<GodotBody2D *>(B), p_subindex_B));
return b;
}
@@ -1033,9 +1045,9 @@ void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_
return;
}
- GodotSpace2D *self = (GodotSpace2D *)p_self;
+ GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self);
self->collision_pairs--;
- GodotConstraint2D *c = (GodotConstraint2D *)p_data;
+ GodotConstraint2D *c = static_cast<GodotConstraint2D *>(p_data);
memdelete(c);
}
@@ -1073,7 +1085,7 @@ void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) {
objects.erase(p_object);
}
-const Set<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const {
+const HashSet<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const {
return objects;
}
diff --git a/servers/physics_2d/godot_space_2d.h b/servers/physics_2d/godot_space_2d.h
index 5d97721176..b6fc48bd70 100644
--- a/servers/physics_2d/godot_space_2d.h
+++ b/servers/physics_2d/godot_space_2d.h
@@ -92,7 +92,7 @@ private:
static void *_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self);
static void _broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self);
- Set<GodotCollisionObject2D *> objects;
+ HashSet<GodotCollisionObject2D *> objects;
GodotArea2D *area = nullptr;
@@ -156,7 +156,7 @@ public:
void add_object(GodotCollisionObject2D *p_object);
void remove_object(GodotCollisionObject2D *p_object);
- const Set<GodotCollisionObject2D *> &get_objects() const;
+ const HashSet<GodotCollisionObject2D *> &get_objects() const;
_FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; }
_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp
index 866c415440..0603458acd 100644
--- a/servers/physics_2d/godot_step_2d.cpp
+++ b/servers/physics_2d/godot_step_2d.cpp
@@ -47,7 +47,7 @@ void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D
}
for (const Pair<GodotConstraint2D *, int> &E : p_body->get_constraint_list()) {
- GodotConstraint2D *constraint = (GodotConstraint2D *)E.first;
+ GodotConstraint2D *constraint = const_cast<GodotConstraint2D *>(E.first);
if (constraint->get_island_step() == _step) {
continue; // Already processed.
}
@@ -168,8 +168,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
const SelfList<GodotArea2D>::List &aml = p_space->get_moved_area_list();
while (aml.first()) {
- for (const Set<GodotConstraint2D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
- GodotConstraint2D *constraint = E->get();
+ for (GodotConstraint2D *E : aml.first()->self()->get_constraints()) {
+ GodotConstraint2D *constraint = E;
if (constraint->get_island_step() == _step) {
continue;
}
@@ -239,7 +239,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
uint32_t total_contraint_count = all_constraints.size();
- work_pool.do_work(total_contraint_count, this, &GodotStep2D::_setup_contraint, nullptr);
+ WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_contraint, nullptr, total_contraint_count, -1, true, SNAME("Physics2DConstraintSetup"));
+ WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -258,7 +259,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
// Warning: _solve_island modifies the constraint islands for optimization purpose,
// their content is not reliable after these calls and shouldn't be used anymore.
- work_pool.do_work(island_count, this, &GodotStep2D::_solve_island, nullptr);
+ group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics2DConstraintSolveIslands"));
+ WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
@@ -297,10 +299,7 @@ GodotStep2D::GodotStep2D() {
body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
constraint_islands.reserve(ISLAND_COUNT_RESERVE);
all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
-
- work_pool.init();
}
GodotStep2D::~GodotStep2D() {
- work_pool.finish();
}
diff --git a/servers/physics_2d/godot_step_2d.h b/servers/physics_2d/godot_step_2d.h
index 9a6d8caf9b..9f8fdd6ce3 100644
--- a/servers/physics_2d/godot_step_2d.h
+++ b/servers/physics_2d/godot_step_2d.h
@@ -33,8 +33,8 @@
#include "godot_space_2d.h"
+#include "core/object/worker_thread_pool.h"
#include "core/templates/local_vector.h"
-#include "core/templates/thread_work_pool.h"
class GodotStep2D {
uint64_t _step = 1;
@@ -42,8 +42,6 @@ class GodotStep2D {
int iterations = 0;
real_t delta = 0.0;
- ThreadWorkPool work_pool;
-
LocalVector<LocalVector<GodotBody2D *>> body_islands;
LocalVector<LocalVector<GodotConstraint2D *>> constraint_islands;
LocalVector<GodotConstraint2D *> all_constraints;
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-25 01:11:58 +0000