summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/body_2d_sw.cpp65
-rw-r--r--servers/physics_2d/body_2d_sw.h1
-rw-r--r--servers/physics_2d/body_pair_2d_sw.cpp4
-rw-r--r--servers/physics_2d/broad_phase_2d_hash_grid.cpp2
-rw-r--r--servers/physics_2d/collision_object_2d_sw.cpp50
-rw-r--r--servers/physics_2d/collision_object_2d_sw.h14
-rw-r--r--servers/physics_2d/collision_solver_2d_sat.cpp27
-rw-r--r--servers/physics_2d/collision_solver_2d_sw.cpp2
-rw-r--r--servers/physics_2d/joints_2d_sw.cpp6
-rw-r--r--servers/physics_2d/physics_2d_server_sw.cpp96
-rw-r--r--servers/physics_2d/physics_2d_server_sw.h18
-rw-r--r--servers/physics_2d/physics_2d_server_wrap_mt.cpp2
-rw-r--r--servers/physics_2d/physics_2d_server_wrap_mt.h12
-rw-r--r--servers/physics_2d/shape_2d_sw.cpp50
-rw-r--r--servers/physics_2d/space_2d_sw.cpp91
-rw-r--r--servers/physics_2d/space_2d_sw.h1
16 files changed, 267 insertions, 174 deletions
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 60bcb2e28e..6ba159ca0a 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -47,8 +47,10 @@ void Body2DSW::update_inertias() {
case Physics2DServer::BODY_MODE_RIGID: {
- if (user_inertia) break;
-
+ if (user_inertia) {
+ _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
+ break;
+ }
//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
real_t total_area = 0;
@@ -57,7 +59,7 @@ void Body2DSW::update_inertias() {
total_area += get_shape_aabb(i).get_area();
}
- real_t _inertia = 0;
+ inertia = 0;
for (int i = 0; i < get_shape_count(); i++) {
@@ -73,15 +75,10 @@ void Body2DSW::update_inertias() {
Transform2D mtx = get_shape_transform(i);
Vector2 scale = mtx.get_scale();
- _inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared();
- //Rect2 ab = get_shape_aabb(i);
- //_inertia+=mass*ab.size.dot(ab.size)/12.0f;
+ inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared();
}
- if (_inertia != 0)
- _inv_inertia = 1.0 / _inertia;
- else
- _inv_inertia = 0.0; //wathever
+ _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
if (mass)
_inv_mass = 1.0 / mass;
@@ -160,6 +157,7 @@ void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, real_t p_value)
_update_inertia();
} else {
user_inertia = true;
+ inertia = p_value;
_inv_inertia = 1.0 / p_value;
}
} break;
@@ -174,7 +172,8 @@ void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, real_t p_value)
angular_damp = p_value;
} break;
- default: {}
+ default: {
+ }
}
}
@@ -184,29 +183,30 @@ real_t Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
case Physics2DServer::BODY_PARAM_BOUNCE: {
return bounce;
- } break;
+ }
case Physics2DServer::BODY_PARAM_FRICTION: {
return friction;
- } break;
+ }
case Physics2DServer::BODY_PARAM_MASS: {
return mass;
- } break;
+ }
case Physics2DServer::BODY_PARAM_INERTIA: {
- return _inv_inertia == 0 ? 0 : 1.0 / _inv_inertia;
- } break;
+ return inertia;
+ }
case Physics2DServer::BODY_PARAM_GRAVITY_SCALE: {
return gravity_scale;
- } break;
+ }
case Physics2DServer::BODY_PARAM_LINEAR_DAMP: {
return linear_damp;
- } break;
+ }
case Physics2DServer::BODY_PARAM_ANGULAR_DAMP: {
return angular_damp;
- } break;
- default: {}
+ }
+ default: {
+ }
}
return 0;
@@ -224,6 +224,7 @@ void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {
_set_inv_transform(get_transform().affine_inverse());
_inv_mass = 0;
+ _inv_inertia = 0;
_set_static(p_mode == Physics2DServer::BODY_MODE_STATIC);
set_active(p_mode == Physics2DServer::BODY_MODE_KINEMATIC && contacts.size());
linear_velocity = Vector2();
@@ -235,17 +236,21 @@ void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {
case Physics2DServer::BODY_MODE_RIGID: {
_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+ _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0;
_set_static(false);
} break;
case Physics2DServer::BODY_MODE_CHARACTER: {
_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+ _inv_inertia = 0;
_set_static(false);
+ angular_velocity = 0;
} break;
}
-
- _update_inertia();
+ if (p_mode == Physics2DServer::BODY_MODE_RIGID && _inv_inertia == 0) {
+ _update_inertia();
+ }
/*
if (get_space())
_update_queries();
@@ -341,19 +346,19 @@ Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const {
switch (p_state) {
case Physics2DServer::BODY_STATE_TRANSFORM: {
return get_transform();
- } break;
+ }
case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
return linear_velocity;
- } break;
+ }
case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
return angular_velocity;
- } break;
+ }
case Physics2DServer::BODY_STATE_SLEEPING: {
return !is_active();
- } break;
+ }
case Physics2DServer::BODY_STATE_CAN_SLEEP: {
return can_sleep;
- } break;
+ }
}
return Variant();
@@ -443,7 +448,8 @@ void Body2DSW::integrate_forces(real_t p_step) {
_compute_area_gravity_and_dampenings(aa[i].area);
stopped = mode == Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE;
} break;
- default: {}
+ default: {
+ }
}
}
}
@@ -605,7 +611,7 @@ void Body2DSW::call_queries() {
set_force_integration_callback(0, StringName());
} else {
Variant::CallError ce;
- if (fi_callback->callback_udata.get_type()) {
+ if (fi_callback->callback_udata.get_type() != Variant::NIL) {
obj->call(fi_callback->method, vp, 2, ce);
@@ -665,6 +671,7 @@ Body2DSW::Body2DSW() :
angular_velocity = 0;
biased_angular_velocity = 0;
mass = 1;
+ inertia = 0;
user_inertia = false;
_inv_inertia = 0;
_inv_mass = 1;
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 8c64dc9230..5df184c894 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -52,6 +52,7 @@ class Body2DSW : public CollisionObject2DSW {
real_t gravity_scale;
real_t mass;
+ real_t inertia;
real_t bounce;
real_t friction;
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index ccdaf0b508..46ea0fd65d 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -303,7 +303,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
Contact &c = contacts[i];
if (!c.reused)
continue;
- if (c.normal.dot(direction) < 0)
+ if (c.normal.dot(direction) > 0) //greater (normal inverted)
continue;
valid = true;
@@ -326,7 +326,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
Contact &c = contacts[i];
if (!c.reused)
continue;
- if (c.normal.dot(direction) < 0)
+ if (c.normal.dot(direction) < 0) //less (normal ok)
continue;
valid = true;
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 1bbb50c974..6dd19c2868 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -673,7 +673,7 @@ public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth)
// "A Fast Voxel Traversal Algorithm for Ray Tracing"
// John Amanatides, Andrew Woo
// http://www.cse.yorku.ca/~amana/research/grid.pdf
- // http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf
+ // https://web.archive.org/web/20100616193049/http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf
// NOTES:
// * This code assumes that the ray's position and direction are in 'cell coordinates', which means
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index 4fbc0769ba..f556638db1 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -29,21 +29,27 @@
/*************************************************************************/
#include "collision_object_2d_sw.h"
+#include "servers/physics_2d/physics_2d_server_sw.h"
#include "space_2d_sw.h"
-void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_transform) {
+void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_transform, bool p_disabled) {
Shape s;
s.shape = p_shape;
s.xform = p_transform;
s.xform_inv = s.xform.affine_inverse();
s.bpid = 0; //needs update
- s.disabled = false;
+ s.disabled = p_disabled;
s.one_way_collision = false;
+ s.one_way_collision_margin = 0;
shapes.push_back(s);
p_shape->add_owner(this);
- _update_shapes();
- _shapes_changed();
+
+ if (!pending_shape_update_list.in_list()) {
+ Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+ }
+ // _update_shapes();
+ // _shapes_changed();
}
void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
@@ -53,8 +59,12 @@ void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
shapes.write[p_index].shape = p_shape;
p_shape->add_owner(this);
- _update_shapes();
- _shapes_changed();
+
+ if (!pending_shape_update_list.in_list()) {
+ Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+ }
+ // _update_shapes();
+ // _shapes_changed();
}
void CollisionObject2DSW::set_shape_metadata(int p_index, const Variant &p_metadata) {
@@ -69,8 +79,12 @@ void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_
shapes.write[p_index].xform = p_transform;
shapes.write[p_index].xform_inv = p_transform.affine_inverse();
- _update_shapes();
- _shapes_changed();
+
+ if (!pending_shape_update_list.in_list()) {
+ Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+ }
+ // _update_shapes();
+ // _shapes_changed();
}
void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) {
@@ -88,9 +102,15 @@ void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) {
if (p_disabled && shape.bpid != 0) {
space->get_broadphase()->remove(shape.bpid);
shape.bpid = 0;
- _update_shapes();
+ if (!pending_shape_update_list.in_list()) {
+ Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+ }
+ //_update_shapes();
} else if (!p_disabled && shape.bpid == 0) {
- _update_shapes(); // automatically adds shape with bpid == 0
+ if (!pending_shape_update_list.in_list()) {
+ Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+ }
+ //_update_shapes(); // automatically adds shape with bpid == 0
}
}
@@ -121,8 +141,11 @@ void CollisionObject2DSW::remove_shape(int p_index) {
shapes[p_index].shape->remove_owner(this);
shapes.remove(p_index);
- _update_shapes();
- _shapes_changed();
+ if (!pending_shape_update_list.in_list()) {
+ Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+ }
+ // _update_shapes();
+ // _shapes_changed();
}
void CollisionObject2DSW::_set_static(bool p_static) {
@@ -238,7 +261,8 @@ void CollisionObject2DSW::_shape_changed() {
_shapes_changed();
}
-CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
+CollisionObject2DSW::CollisionObject2DSW(Type p_type) :
+ pending_shape_update_list(this) {
_static = true;
type = p_type;
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 6946217dbd..ed59469878 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -62,9 +62,11 @@ private:
Variant metadata;
bool disabled;
bool one_way_collision;
+ float one_way_collision_margin;
Shape() {
disabled = false;
one_way_collision = false;
+ one_way_collision_margin = 0;
}
};
@@ -76,6 +78,8 @@ private:
uint32_t collision_layer;
bool _static;
+ SelfList<CollisionObject2DSW> pending_shape_update_list;
+
void _update_shapes();
protected:
@@ -109,7 +113,7 @@ public:
void _shape_changed();
_FORCE_INLINE_ Type get_type() const { return type; }
- void add_shape(Shape2DSW *p_shape, const Transform2D &p_transform = Transform2D());
+ void add_shape(Shape2DSW *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false);
void set_shape(int p_index, Shape2DSW *p_shape);
void set_shape_transform(int p_index, const Transform2D &p_transform);
void set_shape_metadata(int p_index, const Variant &p_metadata);
@@ -150,15 +154,21 @@ public:
return shapes[p_idx].disabled;
}
- _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision) {
+ _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, float p_margin) {
CRASH_BAD_INDEX(p_idx, shapes.size());
shapes.write[p_idx].one_way_collision = p_one_way_collision;
+ shapes.write[p_idx].one_way_collision_margin = p_margin;
}
_FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const {
CRASH_BAD_INDEX(p_idx, shapes.size());
return shapes[p_idx].one_way_collision;
}
+ _FORCE_INLINE_ float get_shape_one_way_collision_margin(int p_idx) const {
+ CRASH_BAD_INDEX(p_idx, shapes.size());
+ return shapes[p_idx].one_way_collision_margin;
+ }
+
void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; }
_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
index d6c3fff421..19e4b8c1d9 100644
--- a/servers/physics_2d/collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -172,8 +172,8 @@ static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_po
points_B = p_points_B;
}
- int version_A = (pointcount_A > 3 ? 3 : pointcount_A) - 1;
- int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1;
+ int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1;
+ int version_B = (pointcount_B > 2 ? 2 : pointcount_B) - 1;
GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
ERR_FAIL_COND(!contacts_func);
@@ -237,8 +237,8 @@ public:
Vector2 axis = p_axis;
- if (Math::abs(axis.x) < CMP_EPSILON &&
- Math::abs(axis.y) < CMP_EPSILON) {
+ if (Math::is_zero_approx(axis.x) &&
+ Math::is_zero_approx(axis.y)) {
// strange case, try an upwards separator
axis = Vector2(0.0, 1.0);
}
@@ -313,10 +313,12 @@ public:
if (best_axis == Vector2(0.0, 0.0))
return;
- callback->collided = true;
+ if (callback) {
+ callback->collided = true;
- if (!callback->callback)
- return; //only collide, no callback
+ if (!callback->callback)
+ return; //only collide, no callback
+ }
static const int max_supports = 2;
Vector2 supports_A[max_supports];
@@ -354,12 +356,13 @@ public:
supports_B[i] += best_axis * margin_B;
}
}
+ if (callback) {
+ callback->normal = best_axis;
+ _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
- callback->normal = best_axis;
- _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
-
- if (callback && callback->sep_axis && *callback->sep_axis != Vector2())
- *callback->sep_axis = Vector2(); //invalidate previous axis (no test)
+ if (callback->sep_axis && *callback->sep_axis != Vector2())
+ *callback->sep_axis = Vector2(); //invalidate previous axis (no test)
+ }
}
_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) {
diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp
index e49961c048..03c0fd5981 100644
--- a/servers/physics_2d/collision_solver_2d_sw.cpp
+++ b/servers/physics_2d/collision_solver_2d_sw.cpp
@@ -249,6 +249,4 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p
return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B);
}
-
- return false;
}
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index 5c2242c4c5..acdaa6e6df 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -92,7 +92,7 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto
bool PinJoint2DSW::setup(real_t p_step) {
Space2DSW *space = A->get_space();
- ERR_FAIL_COND_V(!space, false;)
+ ERR_FAIL_COND_V(!space, false);
rA = A->get_transform().basis_xform(anchor_A);
rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B;
@@ -299,9 +299,7 @@ bool GrooveJoint2DSW::setup(real_t p_step) {
Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
- // FIXME: We used to do this assignment and then override it with 0.001 right after. Investigate why.
- //real_t _b = get_bias();
- real_t _b = 0.001;
+ 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
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index daea013d35..80e204087a 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -36,11 +36,8 @@
#include "core/project_settings.h"
#include "core/script_language.h"
-#define FLUSH_QUERY_CHECK \
- if (flushing_queries) { \
- ERR_EXPLAIN("Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead"); \
- ERR_FAIL(); \
- }
+#define FLUSH_QUERY_CHECK(m_object) \
+ ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead.");
RID Physics2DServerSW::_shape_create(ShapeType p_shape) {
@@ -209,12 +206,14 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &
return;
cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
cbk->ptr[min_depth_idx * 2 + 1] = p_point_B;
+ cbk->passed++;
} else {
cbk->ptr[cbk->amount * 2 + 0] = p_point_A;
cbk->ptr[cbk->amount * 2 + 1] = p_point_B;
cbk->amount++;
+ cbk->passed++;
}
}
@@ -233,6 +232,7 @@ bool Physics2DServerSW::shape_collide(RID p_shape_A, const Transform2D &p_xform_
CollCbkData cbk;
cbk.max = p_result_max;
cbk.amount = 0;
+ cbk.passed = 0;
cbk.ptr = r_results;
bool res = CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk);
@@ -313,11 +313,7 @@ Physics2DDirectSpaceState *Physics2DServerSW::space_get_direct_state(RID p_space
Space2DSW *space = space_owner.get(p_space);
ERR_FAIL_COND_V(!space, NULL);
- if ((using_threads && !doing_sync) || space->is_locked()) {
-
- ERR_EXPLAIN("Space state is inaccessible right now, wait for iteration or physics process notification.");
- ERR_FAIL_V(NULL);
- }
+ ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), NULL, "Space state is inaccessible right now, wait for iteration or physics process notification.");
return space->get_direct_state();
}
@@ -375,7 +371,7 @@ Physics2DServer::AreaSpaceOverrideMode Physics2DServerSW::area_get_space_overrid
return area->get_space_override_mode();
}
-void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform) {
+void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) {
Area2DSW *area = area_owner.get(p_area);
ERR_FAIL_COND(!area);
@@ -383,7 +379,7 @@ void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Transform2
Shape2DSW *shape = shape_owner.get(p_shape);
ERR_FAIL_COND(!shape);
- area->add_shape(shape, p_transform);
+ area->add_shape(shape, p_transform, p_disabled);
}
void Physics2DServerSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
@@ -407,12 +403,11 @@ void Physics2DServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, co
void Physics2DServerSW::area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) {
- FLUSH_QUERY_CHECK
-
Area2DSW *area = area_owner.get(p_area);
ERR_FAIL_COND(!area);
-
ERR_FAIL_INDEX(p_shape, area->get_shape_count());
+ FLUSH_QUERY_CHECK(area);
+
area->set_shape_as_disabled(p_shape, p_disabled);
}
@@ -458,7 +453,7 @@ void Physics2DServerSW::area_clear_shapes(RID p_area) {
area->remove_shape(0);
}
-void Physics2DServerSW::area_attach_object_instance_id(RID p_area, ObjectID p_ID) {
+void Physics2DServerSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) {
if (space_owner.owns(p_area)) {
Space2DSW *space = space_owner.get(p_area);
@@ -466,7 +461,7 @@ void Physics2DServerSW::area_attach_object_instance_id(RID p_area, ObjectID p_ID
}
Area2DSW *area = area_owner.get(p_area);
ERR_FAIL_COND(!area);
- area->set_instance_id(p_ID);
+ area->set_instance_id(p_id);
}
ObjectID Physics2DServerSW::area_get_object_instance_id(RID p_area) const {
@@ -479,7 +474,7 @@ ObjectID Physics2DServerSW::area_get_object_instance_id(RID p_area) const {
return area->get_instance_id();
}
-void Physics2DServerSW::area_attach_canvas_instance_id(RID p_area, ObjectID p_ID) {
+void Physics2DServerSW::area_attach_canvas_instance_id(RID p_area, ObjectID p_id) {
if (space_owner.owns(p_area)) {
Space2DSW *space = space_owner.get(p_area);
@@ -487,7 +482,7 @@ void Physics2DServerSW::area_attach_canvas_instance_id(RID p_area, ObjectID p_ID
}
Area2DSW *area = area_owner.get(p_area);
ERR_FAIL_COND(!area);
- area->set_canvas_instance_id(p_ID);
+ area->set_canvas_instance_id(p_id);
}
ObjectID Physics2DServerSW::area_get_canvas_instance_id(RID p_area) const {
@@ -547,10 +542,9 @@ void Physics2DServerSW::area_set_pickable(RID p_area, bool p_pickable) {
void Physics2DServerSW::area_set_monitorable(RID p_area, bool p_monitorable) {
- FLUSH_QUERY_CHECK
-
Area2DSW *area = area_owner.get(p_area);
ERR_FAIL_COND(!area);
+ FLUSH_QUERY_CHECK(area);
area->set_monitorable(p_monitorable);
}
@@ -627,10 +621,9 @@ RID Physics2DServerSW::body_get_space(RID p_body) const {
void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
- FLUSH_QUERY_CHECK
-
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
+ FLUSH_QUERY_CHECK(body);
body->set_mode(p_mode);
};
@@ -643,7 +636,7 @@ Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body) const {
return body->get_mode();
};
-void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform) {
+void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform, bool p_disabled) {
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
@@ -651,7 +644,7 @@ void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Transform2
Shape2DSW *shape = shape_owner.get(p_shape);
ERR_FAIL_COND(!shape);
- body->add_shape(shape, p_transform);
+ body->add_shape(shape, p_transform, p_disabled);
}
void Physics2DServerSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
@@ -731,23 +724,21 @@ void Physics2DServerSW::body_clear_shapes(RID p_body) {
void Physics2DServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
- FLUSH_QUERY_CHECK
-
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
-
ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
+ FLUSH_QUERY_CHECK(body);
body->set_shape_as_disabled(p_shape_idx, p_disabled);
}
-void Physics2DServerSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable) {
+void Physics2DServerSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, float p_margin) {
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
-
ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
+ FLUSH_QUERY_CHECK(body);
- body->set_shape_as_one_way_collision(p_shape_idx, p_enable);
+ body->set_shape_as_one_way_collision(p_shape_idx, p_enable, p_margin);
}
void Physics2DServerSW::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) {
@@ -765,12 +756,12 @@ Physics2DServerSW::CCDMode Physics2DServerSW::body_get_continuous_collision_dete
return body->get_continuous_collision_detection_mode();
}
-void Physics2DServerSW::body_attach_object_instance_id(RID p_body, uint32_t p_ID) {
+void Physics2DServerSW::body_attach_object_instance_id(RID p_body, uint32_t p_id) {
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
- body->set_instance_id(p_ID);
+ body->set_instance_id(p_id);
};
uint32_t Physics2DServerSW::body_get_object_instance_id(RID p_body) const {
@@ -781,12 +772,12 @@ uint32_t Physics2DServerSW::body_get_object_instance_id(RID p_body) const {
return body->get_instance_id();
};
-void Physics2DServerSW::body_attach_canvas_instance_id(RID p_body, uint32_t p_ID) {
+void Physics2DServerSW::body_attach_canvas_instance_id(RID p_body, uint32_t p_id) {
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
- body->set_canvas_instance_id(p_ID);
+ body->set_canvas_instance_id(p_id);
};
uint32_t Physics2DServerSW::body_get_canvas_instance_id(RID p_body) const {
@@ -904,6 +895,8 @@ void Physics2DServerSW::body_apply_torque_impulse(RID p_body, real_t p_torque) {
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
+ _update_shapes();
+
body->apply_torque_impulse(p_torque);
}
@@ -912,6 +905,8 @@ void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2 &p_pos, con
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
+ _update_shapes();
+
body->apply_impulse(p_pos, p_impulse);
body->wakeup();
};
@@ -946,6 +941,8 @@ void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2 &p_axis
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
+ _update_shapes();
+
Vector2 v = body->get_linear_velocity();
Vector2 axis = p_axis_velocity.normalized();
v -= axis * axis.dot(v);
@@ -1054,6 +1051,8 @@ bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from,
ERR_FAIL_COND_V(!body->get_space(), false);
ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
+ _update_shapes();
+
return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes);
}
@@ -1069,10 +1068,7 @@ int Physics2DServerSW::body_test_ray_separation(RID p_body, const Transform2D &p
Physics2DDirectBodyState *Physics2DServerSW::body_get_direct_state(RID p_body) {
- if ((using_threads && !doing_sync)) {
- ERR_EXPLAIN("Body state is inaccessible right now, wait for iteration or physics process notification.");
- ERR_FAIL_V(NULL);
- }
+ ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), NULL, "Body state is inaccessible right now, wait for iteration or physics process notification.");
if (!body_owner.owns(p_body))
return NULL;
@@ -1080,12 +1076,7 @@ Physics2DDirectBodyState *Physics2DServerSW::body_get_direct_state(RID p_body) {
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND_V(!body, NULL);
ERR_FAIL_COND_V(!body->get_space(), NULL);
-
- if (body->get_space()->is_locked()) {
-
- ERR_EXPLAIN("Body state is inaccessible right now, wait for iteration or physics process notification.");
- ERR_FAIL_V(NULL);
- }
+ ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), NULL, "Body state is inaccessible right now, wait for iteration or physics process notification.");
direct_state->body = body;
return direct_state;
@@ -1240,6 +1231,8 @@ Physics2DServer::JointType Physics2DServerSW::joint_get_type(RID p_joint) const
void Physics2DServerSW::free(RID p_rid) {
+ _update_shapes(); // just in case
+
if (shape_owner.owns(p_rid)) {
Shape2DSW *shape = shape_owner.get(p_rid);
@@ -1313,8 +1306,7 @@ void Physics2DServerSW::free(RID p_rid) {
} else {
- ERR_EXPLAIN("Invalid ID");
- ERR_FAIL();
+ ERR_FAIL_MSG("Invalid ID.");
}
};
@@ -1337,6 +1329,8 @@ void Physics2DServerSW::step(real_t p_step) {
if (!active)
return;
+ _update_shapes();
+
doing_sync = false;
last_step = p_step;
@@ -1420,6 +1414,14 @@ void Physics2DServerSW::finish() {
memdelete(direct_state);
};
+void Physics2DServerSW::_update_shapes() {
+
+ while (pending_shape_update_list.first()) {
+ pending_shape_update_list.first()->self()->_shape_changed();
+ pending_shape_update_list.remove(pending_shape_update_list.first());
+ }
+}
+
int Physics2DServerSW::get_process_info(ProcessInfo p_info) {
switch (p_info) {
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index da402ee16e..72625c397c 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -70,6 +70,9 @@ class Physics2DServerSW : public Physics2DServer {
static Physics2DServerSW *singletonsw;
//void _clear_query(Query2DSW *p_query);
+ friend class CollisionObject2DSW;
+ SelfList<CollisionObject2DSW>::List pending_shape_update_list;
+ void _update_shapes();
RID _shape_create(ShapeType p_shape);
@@ -80,6 +83,7 @@ public:
real_t valid_depth;
int max;
int amount;
+ int passed;
int invalid_by_dir;
Vector2 *ptr;
};
@@ -130,7 +134,7 @@ public:
virtual void area_set_space(RID p_area, RID p_space);
virtual RID area_get_space(RID p_area) const;
- virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D());
+ virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false);
virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape);
virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform);
@@ -143,10 +147,10 @@ public:
virtual void area_remove_shape(RID p_area, int p_shape_idx);
virtual void area_clear_shapes(RID p_area);
- virtual void area_attach_object_instance_id(RID p_area, ObjectID p_ID);
+ virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id);
virtual ObjectID area_get_object_instance_id(RID p_area) const;
- virtual void area_attach_canvas_instance_id(RID p_area, ObjectID p_ID);
+ virtual void area_attach_canvas_instance_id(RID p_area, ObjectID p_id);
virtual ObjectID area_get_canvas_instance_id(RID p_area) const;
virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value);
@@ -174,7 +178,7 @@ public:
virtual void body_set_mode(RID p_body, BodyMode p_mode);
virtual BodyMode body_get_mode(RID p_body) const;
- virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D());
+ virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false);
virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape);
virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform);
virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant &p_metadata);
@@ -188,12 +192,12 @@ public:
virtual void body_clear_shapes(RID p_body);
virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled);
- virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable);
+ virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, float p_margin);
- virtual void body_attach_object_instance_id(RID p_body, uint32_t p_ID);
+ virtual void body_attach_object_instance_id(RID p_body, uint32_t p_id);
virtual uint32_t body_get_object_instance_id(RID p_body) const;
- virtual void body_attach_canvas_instance_id(RID p_body, uint32_t p_ID);
+ virtual void body_attach_canvas_instance_id(RID p_body, uint32_t p_id);
virtual uint32_t body_get_canvas_instance_id(RID p_body) const;
virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode);
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.cpp b/servers/physics_2d/physics_2d_server_wrap_mt.cpp
index 71c00c0abf..c698290fd9 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.cpp
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.cpp
@@ -54,8 +54,6 @@ void Physics2DServerWrapMT::thread_loop() {
server_thread = Thread::get_caller_id();
- OS::get_singleton()->make_rendering_thread();
-
physics_2d_server->init();
exit = false;
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index 467d301ad5..33a184ba3f 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -140,7 +140,7 @@ public:
FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode);
FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID);
- FUNC3(area_add_shape, RID, RID, const Transform2D &);
+ FUNC4(area_add_shape, RID, RID, const Transform2D &, bool);
FUNC3(area_set_shape, RID, int, RID);
FUNC3(area_set_shape_transform, RID, int, const Transform2D &);
FUNC3(area_set_shape_disabled, RID, int, bool);
@@ -183,7 +183,7 @@ public:
FUNC2(body_set_mode, RID, BodyMode);
FUNC1RC(BodyMode, body_get_mode, RID);
- FUNC3(body_add_shape, RID, RID, const Transform2D &);
+ FUNC4(body_add_shape, RID, RID, const Transform2D &, bool);
FUNC3(body_set_shape, RID, int, RID);
FUNC3(body_set_shape_transform, RID, int, const Transform2D &);
FUNC3(body_set_shape_metadata, RID, int, const Variant &);
@@ -194,7 +194,7 @@ public:
FUNC2RC(RID, body_get_shape, RID, int);
FUNC3(body_set_shape_disabled, RID, int, bool);
- FUNC3(body_set_shape_as_one_way_collision, RID, int, bool);
+ FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, float);
FUNC2(body_remove_shape, RID, int);
FUNC1(body_clear_shapes, RID);
@@ -327,11 +327,11 @@ public:
static Physics2DServer *init_server() {
int tm = GLOBAL_DEF("physics/2d/thread_model", 1);
- if (tm == 0) //single unsafe
+ if (tm == 0) // single unsafe
return memnew(T);
- else if (tm == 1) //single saef
+ else if (tm == 1) // single safe
return memnew(Physics2DServerWrapMT(memnew(T), false));
- else //single unsafe
+ else // multi threaded
return memnew(Physics2DServerWrapMT(memnew(T), true));
}
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 558700f400..48799a56fb 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -31,7 +31,7 @@
#include "shape_2d_sw.h"
#include "core/math/geometry.h"
-#include "core/sort.h"
+#include "core/sort_array.h"
void Shape2DSW::configure(const Rect2 &p_aabb) {
aabb = p_aabb;
@@ -240,12 +240,7 @@ bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
real_t SegmentShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
- Vector2 s[2] = { a * p_scale, b * p_scale };
-
- real_t l = s[1].distance_to(s[0]);
- Vector2 ofs = (s[0] + s[1]) * 0.5;
-
- return p_mass * (l * l / 12.0 + ofs.length_squared());
+ return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12;
}
void SegmentShape2DSW::set_data(const Variant &p_data) {
@@ -318,7 +313,9 @@ bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p
real_t CircleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
- return (radius * radius) * (p_scale.x * 0.5 + p_scale.y * 0.5);
+ real_t a = radius * p_scale.x;
+ real_t b = radius * p_scale.y;
+ return p_mass * (a * a + b * b) / 4;
}
void CircleShape2DSW::set_data(const Variant &p_data) {
@@ -369,8 +366,11 @@ void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_suppor
}
bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const {
-
- return Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y;
+ float x = p_point.x;
+ float y = p_point.y;
+ float edge_x = half_extents.x;
+ float edge_y = half_extents.y;
+ return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y);
}
bool RectangleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
@@ -583,7 +583,7 @@ bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const {
in = true;
}
- return (in && !out) || (!in && out);
+ return in != out;
}
bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
@@ -634,7 +634,7 @@ real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2
aabb.expand_to(points[i].pos * p_scale);
}
- return p_mass * aabb.size.dot(aabb.size) / 12.0 + p_mass * (aabb.position + aabb.size * 0.5).length_squared();
+ return p_mass * aabb.size.dot(aabb.size) / 12.0;
}
void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
@@ -775,22 +775,22 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve
while (true) {
uint32_t node = stack[level] & NODE_IDX_MASK;
- const BVH &b = bvhptr[node];
+ const BVH &bvh = bvhptr[node];
bool done = false;
switch (stack[level] >> VISITED_BIT_SHIFT) {
case TEST_AABB_BIT: {
- bool valid = b.aabb.intersects_segment(p_begin, p_end);
+ bool valid = bvh.aabb.intersects_segment(p_begin, p_end);
if (!valid) {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {
- if (b.left < 0) {
+ if (bvh.left < 0) {
- const Segment &s = segmentptr[b.right];
+ const Segment &s = segmentptr[bvh.right];
Vector2 a = pointptr[s.points[0]];
Vector2 b = pointptr[s.points[1]];
@@ -820,14 +820,14 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve
case VISIT_LEFT_BIT: {
stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
- stack[level + 1] = b.left | TEST_AABB_BIT;
+ stack[level + 1] = bvh.left | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_RIGHT_BIT: {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
- stack[level + 1] = b.right | TEST_AABB_BIT;
+ stack[level + 1] = bvh.right | TEST_AABB_BIT;
level++;
}
continue;
@@ -986,7 +986,7 @@ Variant ConcavePolygonShape2DSW::get_data() const {
w[(i << 1) + 1] = points[segments[i].points[1]];
}
- w = PoolVector<Vector2>::Write();
+ w.release();
return rsegments;
}
@@ -1025,21 +1025,21 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac
while (true) {
uint32_t node = stack[level] & NODE_IDX_MASK;
- const BVH &b = bvhptr[node];
+ const BVH &bvh = bvhptr[node];
switch (stack[level] >> VISITED_BIT_SHIFT) {
case TEST_AABB_BIT: {
- bool valid = p_local_aabb.intersects(b.aabb);
+ bool valid = p_local_aabb.intersects(bvh.aabb);
if (!valid) {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {
- if (b.left < 0) {
+ if (bvh.left < 0) {
- const Segment &s = segmentptr[b.right];
+ const Segment &s = segmentptr[bvh.right];
Vector2 a = pointptr[s.points[0]];
Vector2 b = pointptr[s.points[1]];
@@ -1058,14 +1058,14 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac
case VISIT_LEFT_BIT: {
stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
- stack[level + 1] = b.left | TEST_AABB_BIT;
+ stack[level + 1] = bvh.left | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_RIGHT_BIT: {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
- stack[level + 1] = b.right | TEST_AABB_BIT;
+ stack[level + 1] = bvh.right | TEST_AABB_BIT;
level++;
}
continue;
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 3d8005cd4d..2778775446 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -280,7 +280,7 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transfor
real_t hi = 1;
Vector2 mnormal = p_motion.normalized();
- for (int i = 0; i < 8; i++) { //steps should be customizable..
+ for (int j = 0; j < 8; j++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
@@ -328,14 +328,11 @@ bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Transform2D &
Physics2DServerSW::CollCbkData cbk;
cbk.max = p_result_max;
cbk.amount = 0;
+ cbk.passed = 0;
cbk.ptr = r_results;
- CollisionSolver2DSW::CallbackResult cbkres = NULL;
+ CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk;
- Physics2DServerSW::CollCbkData *cbkptr = NULL;
- if (p_result_max > 0) {
- cbkptr = &cbk;
- cbkres = Physics2DServerSW::_shape_col_cbk;
- }
+ Physics2DServerSW::CollCbkData *cbkptr = &cbk;
for (int i = 0; i < amount; i++) {
@@ -352,7 +349,7 @@ bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Transform2D &
cbk.valid_depth = 0;
if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) {
- collided = p_result_max == 0 || cbk.amount > 0;
+ collided = cbk.amount > 0;
}
}
@@ -374,6 +371,7 @@ struct _RestCallbackData2D {
real_t best_len;
Vector2 valid_dir;
real_t valid_depth;
+ real_t min_allowed_depth;
};
static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
@@ -389,6 +387,10 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
Vector2 contact_rel = p_point_B - p_point_A;
real_t len = contact_rel.length();
+
+ if (len < rd->min_allowed_depth)
+ return;
+
if (len <= rd->best_len)
return;
@@ -415,6 +417,7 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_sh
rcd.best_len = 0;
rcd.best_object = NULL;
rcd.best_shape = 0;
+ rcd.min_allowed_depth = space->test_motion_min_contact_depth;
for (int i = 0; i < amount; i++) {
@@ -437,7 +440,7 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_sh
continue;
}
- if (rcd.best_len == 0)
+ if (rcd.best_len == 0 || !rcd.best_object)
return false;
r_info->collider_id = rcd.best_object->get_instance_id();
@@ -511,6 +514,9 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
if (p_body->is_shape_set_as_disabled(i))
continue;
+ if (p_body->get_shape(i)->get_type() != Physics2DServer::SHAPE_RAY)
+ continue;
+
if (!shapes_found) {
body_aabb = p_body->get_shape_aabb(i);
shapes_found = true;
@@ -554,7 +560,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
bool collided = false;
int amount = _cull_aabb_for_body(p_body, body_aabb);
- int ray_index = 0;
for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j))
@@ -573,6 +578,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
int shape_idx = intersection_query_subindex_results[i];
cbk.amount = 0;
+ cbk.passed = 0;
cbk.ptr = sr;
cbk.invalid_by_dir = 0;
@@ -585,6 +591,10 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+ /*
+ * There is no point in supporting one way collisions with ray shapes, as they will always collide in the desired
+ * direction. Use a short ray shape if you want to achieve a similar effect.
+ *
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
@@ -592,10 +602,15 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
cbk.invalid_by_dir = 0;
} else {
- cbk.valid_dir = Vector2();
- cbk.valid_depth = 0;
- cbk.invalid_by_dir = 0;
+*/
+
+ cbk.valid_dir = Vector2();
+ cbk.valid_depth = 0;
+ cbk.invalid_by_dir = 0;
+
+ /*
}
+ */
Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, NULL, p_margin)) {
@@ -603,7 +618,20 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
collided = true;
}
- if (ray_index < p_result_max) {
+ int ray_index = -1; //reuse shape
+ for (int k = 0; k < rays_found; k++) {
+ if (r_results[ray_index].collision_local_shape == j) {
+ ray_index = k;
+ }
+ }
+
+ if (ray_index == -1 && rays_found < p_result_max) {
+ ray_index = rays_found;
+ rays_found++;
+ }
+
+ if (ray_index != -1) {
+
Physics2DServer::SeparationResult &result = r_results[ray_index];
for (int k = 0; k < cbk.amount; k++) {
@@ -618,7 +646,8 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
result.collision_depth = depth;
result.collision_point = b;
result.collision_normal = (b - a).normalized();
- result.collision_local_shape = shape_idx;
+ result.collision_local_shape = j;
+ result.collider_shape = shape_idx;
result.collider = col_obj->get_self();
result.collider_id = col_obj->get_instance_id();
result.collider_metadata = col_obj->get_shape_metadata(shape_idx);
@@ -633,12 +662,8 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
}
}
}
-
- ray_index++;
}
- rays_found = MAX(ray_index, rays_found);
-
if (!collided || recover_motion == Vector2()) {
break;
}
@@ -684,6 +709,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
if (p_body->is_shape_set_as_disabled(i))
continue;
+ if (p_exclude_raycast_shapes && p_body->get_shape(i)->get_type() == Physics2DServer::SHAPE_RAY)
+ continue;
+
if (!shapes_found) {
body_aabb = p_body->get_shape_aabb(i);
shapes_found = true;
@@ -693,6 +721,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
if (!shapes_found) {
+ if (r_result) {
+ *r_result = Physics2DServer::MotionResult();
+ r_result->motion = p_motion;
+ }
return false;
}
@@ -720,6 +752,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
Physics2DServerSW::CollCbkData cbk;
cbk.max = max_results;
cbk.amount = 0;
+ cbk.passed = 0;
cbk.ptr = sr;
cbk.invalid_by_dir = 0;
excluded_shape_pair_count = 0; //last step is the one valid
@@ -759,7 +792,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
- cbk.valid_depth = p_margin; //only valid depth is the collision margin
+ float owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
+ cbk.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work
cbk.invalid_by_dir = 0;
if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
@@ -780,12 +814,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
cbk.invalid_by_dir = 0;
}
- int current_collisions = cbk.amount;
+ int current_passed = cbk.passed; //save how many points passed collision
bool did_collide = false;
Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, NULL, separation_margin)) {
- did_collide = cbk.amount > current_collisions;
+ did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
}
if (!did_collide && cbk.invalid_by_dir > 0) {
@@ -933,6 +967,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
Physics2DServerSW::CollCbkData cbk;
cbk.max = 1;
cbk.amount = 0;
+ cbk.passed = 0;
cbk.ptr = cd;
cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
@@ -985,15 +1020,24 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
rcd.best_len = 0;
rcd.best_object = NULL;
rcd.best_shape = 0;
+ rcd.min_allowed_depth = test_motion_min_contact_depth;
//optimization
int from_shape = best_shape != -1 ? best_shape : 0;
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
for (int j = from_shape; j < to_shape; j++) {
+
+ if (p_body->is_shape_set_as_disabled(j))
+ continue;
+
Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j);
+ if (p_exclude_raycast_shapes && body_shape->get_type() == Physics2DServer::SHAPE_RAY) {
+ continue;
+ }
+
body_aabb.position += p_motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb);
@@ -1244,6 +1288,7 @@ void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_valu
case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: body_angular_velocity_sleep_threshold = p_value; break;
case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break;
case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break;
+ case Physics2DServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: test_motion_min_contact_depth = p_value; break;
}
}
@@ -1258,6 +1303,7 @@ real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {
case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: return body_angular_velocity_sleep_threshold;
case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
+ case Physics2DServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: return test_motion_min_contact_depth;
}
return 0;
}
@@ -1294,6 +1340,7 @@ Space2DSW::Space2DSW() {
contact_recycle_radius = 1.0;
contact_max_separation = 1.5;
contact_max_allowed_penetration = 0.3;
+ test_motion_min_contact_depth = 0.005;
constraint_bias = 0.2;
body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0);
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index 64f8c9e156..14c24959b7 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -104,6 +104,7 @@ private:
real_t contact_max_separation;
real_t contact_max_allowed_penetration;
real_t constraint_bias;
+ real_t test_motion_min_contact_depth;
enum {
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-09 01:34:30 +0000