Skip to content

Commit

Permalink
Fix and clean disabled shapes handling in godot physics servers
Browse files Browse the repository at this point in the history 
In 3D, disabled shapes are now not added to the broadphase anymore.
Since they are removed right away when disabled, no need to check for
disabled shapes for any query that comes from the broadphase.
Also Fixes raycast queries returning disabled shapes.

In 2D, disabled shapes where already not added to the broadphase.
Remove the same unnecessary checks as in 3D.

Overall harmonized API for disabled shapes in the physics servers and
removed duplicate method.
  • Loading branch information
@pouleyKetchoupp
pouleyKetchoupp committed Jun 22, 2021
1 parent 2827d6b commit 10fdd19
Show file tree
Hide file tree
Showing 12 changed files with 76 additions and 86 deletions.
9 changes: 2 additions & 7 deletions servers/physics/area_pair_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,7 @@

bool AreaPairSW::setup(real_t p_step) {
bool result = false;

if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) {
result = false;
} else if (area->test_collision_mask(body) && CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) {
if (area->test_collision_mask(body) && CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) {
result = true;
}

Expand Down Expand Up @@ -97,9 +94,7 @@ AreaPairSW::~AreaPairSW() {

bool Area2PairSW::setup(real_t p_step) {
bool result = false;
if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) {
result = false;
} else if (area_a->test_collision_mask(area_b) && CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) {
if (area_a->test_collision_mask(area_b) && CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) {
result = true;
}

Expand Down
5 changes: 0 additions & 5 deletions servers/physics/body_pair_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -216,11 +216,6 @@ bool BodyPairSW::setup(real_t p_step) {
return false;
}

if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
collided = false;
return false;
}

offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();

validate_contacts();
Expand Down
42 changes: 30 additions & 12 deletions servers/physics/collision_object_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,6 @@ void CollisionObjectSW::add_shape(ShapeSW *p_shape, const Transform &p_transform
if (!pending_shape_update_list.in_list()) {
PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
//_update_shapes();
//_shapes_changed();
}

void CollisionObjectSW::set_shape(int p_index, ShapeSW *p_shape) {
Expand All @@ -58,8 +56,6 @@ void CollisionObjectSW::set_shape(int p_index, ShapeSW *p_shape) {
if (!pending_shape_update_list.in_list()) {
PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
//_update_shapes();
//_shapes_changed();
}
void CollisionObjectSW::set_shape_transform(int p_index, const Transform &p_transform) {
ERR_FAIL_INDEX(p_index, shapes.size());
Expand All @@ -69,14 +65,32 @@ void CollisionObjectSW::set_shape_transform(int p_index, const Transform &p_tran
if (!pending_shape_update_list.in_list()) {
PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
//_update_shapes();
//_shapes_changed();
}

void CollisionObjectSW::set_shape_as_disabled(int p_idx, bool p_enable) {
shapes.write[p_idx].disabled = p_enable;
if (!pending_shape_update_list.in_list()) {
PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list);
void CollisionObjectSW::set_shape_disabled(int p_idx, bool p_disabled) {
ERR_FAIL_INDEX(p_idx, shapes.size());

CollisionObjectSW::Shape &shape = shapes.write[p_idx];
if (shape.disabled == p_disabled) {
return;
}

shape.disabled = p_disabled;

if (!space) {
return;
}

if (p_disabled && shape.bpid != 0) {
space->get_broadphase()->remove(shape.bpid);
shape.bpid = 0;
if (!pending_shape_update_list.in_list()) {
PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
} else if (!p_disabled && shape.bpid == 0) {
if (!pending_shape_update_list.in_list()) {
PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
}
}

Expand Down Expand Up @@ -107,8 +121,6 @@ void CollisionObjectSW::remove_shape(int p_index) {
if (!pending_shape_update_list.in_list()) {
PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list);
}
//_update_shapes();
//_shapes_changed();
}

void CollisionObjectSW::_set_static(bool p_static) {
Expand Down Expand Up @@ -145,6 +157,9 @@ void CollisionObjectSW::_update_shapes() {

for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
if (s.disabled) {
continue;
}

//not quite correct, should compute the next matrix..
AABB shape_aabb = s.shape->get_aabb();
Expand Down Expand Up @@ -172,6 +187,9 @@ void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) {

for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
if (s.disabled) {
continue;
}

//not quite correct, should compute the next matrix..
AABB shape_aabb = s.shape->get_aabb();
Expand Down
31 changes: 21 additions & 10 deletions servers/physics/collision_object_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,15 +119,26 @@ class CollisionObjectSW : public ShapeOwnerSW {
void set_shape(int p_index, ShapeSW *p_shape);
void set_shape_transform(int p_index, const Transform &p_transform);
_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
_FORCE_INLINE_ bool is_shape_disabled(int p_index) const {
_FORCE_INLINE_ ShapeSW *get_shape(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].disabled;
return shapes[p_index].shape;
}
_FORCE_INLINE_ const Transform &get_shape_transform(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].xform;
}
_FORCE_INLINE_ const Transform &get_shape_inv_transform(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].xform_inv;
}
_FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].aabb_cache;
}
_FORCE_INLINE_ real_t get_shape_area(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].area_cache;
}
_FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { return shapes[p_index].shape; }
_FORCE_INLINE_ const Transform &get_shape_transform(int p_index) const { return shapes[p_index].xform; }
_FORCE_INLINE_ const Transform &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
_FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
_FORCE_INLINE_ real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; }

_FORCE_INLINE_ Transform get_transform() const { return transform; }
_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
Expand All @@ -136,9 +147,9 @@ class CollisionObjectSW : public ShapeOwnerSW {
_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }

void set_shape_as_disabled(int p_idx, bool p_enable);
_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const {
CRASH_BAD_INDEX(p_idx, shapes.size());
void set_shape_disabled(int p_idx, bool p_disabled);
_FORCE_INLINE_ bool is_shape_disabled(int p_idx) const {
ERR_FAIL_INDEX_V(p_idx, shapes.size(), false);
return shapes[p_idx].disabled;
}

Expand Down
4 changes: 2 additions & 2 deletions servers/physics/physics_server_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -320,7 +320,7 @@ void PhysicsServerSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool
ERR_FAIL_COND(!area);
ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count());
FLUSH_QUERY_CHECK(area);
area->set_shape_as_disabled(p_shape_idx, p_disabled);
area->set_shape_disabled(p_shape_idx, p_disabled);
}

void PhysicsServerSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) {
Expand Down Expand Up @@ -533,7 +533,7 @@ void PhysicsServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool
ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
FLUSH_QUERY_CHECK(body);

body->set_shape_as_disabled(p_shape_idx, p_disabled);
body->set_shape_disabled(p_shape_idx, p_disabled);
}

Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
Expand Down
16 changes: 7 additions & 9 deletions servers/physics/space_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -485,7 +485,7 @@ Vector3 PhysicsDirectSpaceStateSW::get_closest_point_to_object_volume(RID p_obje
bool shapes_found = false;

for (int i = 0; i < obj->get_shape_count(); i++) {
if (obj->is_shape_set_as_disabled(i)) {
if (obj->is_shape_disabled(i)) {
continue;
}

Expand Down Expand Up @@ -530,8 +530,6 @@ int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const AABB &p_aabb) {
keep = false;
} else if (static_cast<BodySW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
keep = false;
} else if (static_cast<BodySW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) {
keep = false;
}

if (!keep) {
Expand All @@ -554,7 +552,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
bool shapes_found = false;

for (int i = 0; i < p_body->get_shape_count(); i++) {
if (p_body->is_shape_set_as_disabled(i)) {
if (p_body->is_shape_disabled(i)) {
continue;
}

Expand Down Expand Up @@ -601,7 +599,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
int amount = _cull_aabb_for_body(p_body, body_aabb);

for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j)) {
if (p_body->is_shape_disabled(j)) {
continue;
}

Expand Down Expand Up @@ -716,7 +714,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
bool shapes_found = false;

for (int i = 0; i < p_body->get_shape_count(); i++) {
if (p_body->is_shape_set_as_disabled(i)) {
if (p_body->is_shape_disabled(i)) {
continue;
}

Expand Down Expand Up @@ -769,7 +767,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
int amount = _cull_aabb_for_body(p_body, body_aabb);

for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j)) {
if (p_body->is_shape_disabled(j)) {
continue;
}

Expand Down Expand Up @@ -847,7 +845,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
int amount = _cull_aabb_for_body(p_body, motion_aabb);

for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j)) {
if (p_body->is_shape_disabled(j)) {
continue;
}

Expand Down Expand Up @@ -966,7 +964,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
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)) {
if (p_body->is_shape_disabled(j)) {
continue;
}

Expand Down
9 changes: 2 additions & 7 deletions servers/physics_2d/area_pair_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,7 @@

bool AreaPair2DSW::setup(real_t p_step) {
bool result = false;

if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) {
result = false;
} else if (area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) {
if (area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) {
result = true;
}

Expand Down Expand Up @@ -97,9 +94,7 @@ AreaPair2DSW::~AreaPair2DSW() {

bool Area2Pair2DSW::setup(real_t p_step) {
bool result = false;
if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) {
result = false;
} else if (area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) {
if (area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) {
result = true;
}

Expand Down
5 changes: 0 additions & 5 deletions servers/physics_2d/body_pair_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -226,11 +226,6 @@ bool BodyPair2DSW::setup(real_t p_step) {
return false;
}

if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
collided = false;
return false;
}

//use local A coordinates to avoid numerical issues on collision detection
offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();

Expand Down
13 changes: 1 addition & 12 deletions servers/physics_2d/collision_object_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,6 @@ void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_tra
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) {
Expand All @@ -61,8 +59,6 @@ void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
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) {
Expand All @@ -79,11 +75,9 @@ void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_
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) {
void CollisionObject2DSW::set_shape_disabled(int p_idx, bool p_disabled) {
ERR_FAIL_INDEX(p_idx, shapes.size());

CollisionObject2DSW::Shape &shape = shapes.write[p_idx];
Expand All @@ -103,12 +97,10 @@ void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) {
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) {
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
}
}

Expand Down Expand Up @@ -139,8 +131,6 @@ void CollisionObject2DSW::remove_shape(int p_index) {
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) {
Expand Down Expand Up @@ -177,7 +167,6 @@ void CollisionObject2DSW::_update_shapes() {

for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];

if (s.disabled) {
continue;
}
Expand Down
10 changes: 3 additions & 7 deletions servers/physics_2d/collision_object_2d_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,10 +118,6 @@ class CollisionObject2DSW : public ShapeOwner2DSW {
void set_shape_metadata(int p_index, const Variant &p_metadata);

_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
_FORCE_INLINE_ bool is_shape_disabled(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].disabled;
}
_FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].shape;
Expand All @@ -147,9 +143,9 @@ class CollisionObject2DSW : public ShapeOwner2DSW {
_FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ Space2DSW *get_space() const { return space; }

void set_shape_as_disabled(int p_idx, bool p_disabled);
_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const {
CRASH_BAD_INDEX(p_idx, shapes.size());
void set_shape_disabled(int p_idx, bool p_disabled);
_FORCE_INLINE_ bool is_shape_disabled(int p_idx) const {
ERR_FAIL_INDEX_V(p_idx, shapes.size(), false);
return shapes[p_idx].disabled;
}

Expand Down
4 changes: 2 additions & 2 deletions servers/physics_2d/physics_2d_server_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -361,7 +361,7 @@ void Physics2DServerSW::area_set_shape_disabled(RID p_area, int p_shape, bool p_
ERR_FAIL_INDEX(p_shape, area->get_shape_count());
FLUSH_QUERY_CHECK(area);

area->set_shape_as_disabled(p_shape, p_disabled);
area->set_shape_disabled(p_shape, p_disabled);
}

int Physics2DServerSW::area_get_shape_count(RID p_area) const {
Expand Down Expand Up @@ -651,7 +651,7 @@ void Physics2DServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo
ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
FLUSH_QUERY_CHECK(body);

body->set_shape_as_disabled(p_shape_idx, p_disabled);
body->set_shape_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, float p_margin) {
Body2DSW *body = body_owner.get(p_body);
Expand Down
Loading

0 comments on commit 10fdd19

Please sign in to comment.