From cd925866fcf573942eacdc60e992157d49ed7c52 Mon Sep 17 00:00:00 2001 From: Dragos Daian Date: Tue, 15 Aug 2023 12:37:16 +0200 Subject: [PATCH] initial impl for character body one way collision (#34) * initial impl * fix all character one way tests * fix moving of character body. --- CHANGELOG.md | 4 ++ CONTRIBUTING.md | 7 +-- src/bodies/box2d_body.cpp | 2 +- src/bodies/box2d_collision_object.cpp | 3 + src/bodies/box2d_collision_object.h | 17 +++--- src/servers/physics_server_box2d.cpp | 33 +++++++++-- src/shapes/box2d_shape.cpp | 3 + src/shapes/box2d_shape_concave_polygon.cpp | 27 +++++---- src/shapes/box2d_shape_convex_polygon.cpp | 21 ++++--- src/spaces/box2d_direct_space_state.cpp | 18 +++--- src/spaces/box2d_query_callback.cpp | 7 ++- src/spaces/box2d_query_callback.h | 2 + src/spaces/box2d_space_contact_listener.cpp | 39 ++++++------- src/spaces/box2d_space_contact_listener.h | 10 ++-- src/spaces/box2d_sweep_test.cpp | 62 +++++++++++++++------ src/spaces/box2d_sweep_test.h | 14 +++-- 16 files changed, 172 insertions(+), 97 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 10d7a9d..59894e5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog +## [v0.6](https://github.com/godot-box2d/godot-box2d/releases/tag/v0.6) + +- Fix [Fix One way Collision for CharacterBody](https://github.com/appsinacup/godot-box2d/issues/33) + ## [v0.5.4](https://github.com/godot-box2d/godot-box2d/releases/tag/v0.5.4) - Fix for [Read project global configurations](https://github.com/appsinacup/godot-box2d/issues/26) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 9a0ff96..81e24eb 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -5,16 +5,15 @@ Currently pass rate of [Godot-Physics-Tests](https://github.com/fabriceci/Godot- Test Count|Pass Status|Category|Notes|Test Names| --|--|--|--|--| -132|PASS|All| +134|PASS|All| 4|VERY CLOSE|RigidBody2D CCD|There is one line that causes it to fail for another reason. https://github.com/godot-box2d/godot-box2d/issues/29.|testing Continuous Collision Detection (CCD) 1|VERY CLOSE|RigidBody2D Collision Stability|If I increase the collision boundary to 3 pixels, test passes. Could be chance that godot physics test passes.| testing if 450 rectangles can be handled before instablity 2|VERY CLOSE|RigidBody2D Sleep|* All bodies are asleep after 3.5 seconds, while in Godot after 2s. Sleep time is not configurable in box2d|*testing [contact_monitor] > The body sleep and *testing the stability with a pyramid [tolerance (2.50, 0.00)] > All body are sleep 3|VERY CLOSE|RigidBody2D Torque|Final values are very close to Godot Physics. https://github.com/godot-box2d/godot-box2d/issues/28|* Constant torque is applied, *The impulse torque is applied to the angular velocity and *The impulse torque makes the body rotate correctly 1|FAIL|CharacterBody2D|Fail for snapping to floor. https://github.com/godot-box2d/godot-box2d/issues/11 5|FAIL|Sync to Physics|https://github.com/godot-box2d/godot-box2d/issues/7|*Sync to Physics -2|FAIL|CollisionShape One Way| https://github.com/godot-box2d/godot-box2d/issues/27|*Only collide if the platform rotation > 180° -142|PASS + VERY CLOSE|All| -8|FAIL|All| +144|PASS + VERY CLOSE|All| +6|FAIL|All| Performance Tests, before simulation goes below 30fps: diff --git a/src/bodies/box2d_body.cpp b/src/bodies/box2d_body.cpp index 27c88f8..ee3bea5 100644 --- a/src/bodies/box2d_body.cpp +++ b/src/bodies/box2d_body.cpp @@ -97,7 +97,7 @@ void Box2DBody::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_v switch (p_state) { case PhysicsServer2D::BODY_STATE_TRANSFORM: { if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - //_set_transform(p_variant); + _set_transform(p_variant); // TODO } else if (mode == PhysicsServer2D::BODY_MODE_STATIC) { _set_transform(p_variant); diff --git a/src/bodies/box2d_collision_object.cpp b/src/bodies/box2d_collision_object.cpp index 160e1b3..15fbab8 100644 --- a/src/bodies/box2d_collision_object.cpp +++ b/src/bodies/box2d_collision_object.cpp @@ -718,6 +718,9 @@ void Box2DCollisionObject::_set_space(Box2DSpace *p_space) { _update_shapes(); } } +Vector Box2DCollisionObject::get_shapes() { + return shapes; +} int Box2DCollisionObject::get_shape_count() const { return shapes.size(); } Box2DShape *Box2DCollisionObject::get_shape(int p_index) const { diff --git a/src/bodies/box2d_collision_object.h b/src/bodies/box2d_collision_object.h index f25103e..64cd7a9 100644 --- a/src/bodies/box2d_collision_object.h +++ b/src/bodies/box2d_collision_object.h @@ -29,6 +29,14 @@ class Box2DCollisionObject { bool operator<(const Box2DCollisionObject &other) const { return collision.priority < other.collision.priority; } + struct Shape { + Transform2D xform; + Box2DShape *shape = nullptr; + Vector shapes; + Vector fixtures; + bool disabled = false; + bool one_way_collision = false; + }; protected: Type type; @@ -42,14 +50,6 @@ class Box2DCollisionObject { HashSet collision_exception; Vector areas; - struct Shape { - Transform2D xform; - Box2DShape *shape = nullptr; - Vector shapes; - Vector fixtures; - bool disabled = false; - bool one_way_collision = false; - }; Vector shapes; struct Collision { @@ -199,6 +199,7 @@ class Box2DCollisionObject { virtual void clear_shape(int p_index); void set_gravity_scale(real_t p_gravity_scale); real_t get_gravity_scale(); + Vector get_shapes(); void add_collision_exception(Box2DCollisionObject *excepted_body); void remove_collision_exception(Box2DCollisionObject *excepted_body); bool is_body_collision_excepted(Box2DCollisionObject *excepted_body); diff --git a/src/servers/physics_server_box2d.cpp b/src/servers/physics_server_box2d.cpp index be12d71..6d2d7d9 100644 --- a/src/servers/physics_server_box2d.cpp +++ b/src/servers/physics_server_box2d.cpp @@ -2,6 +2,7 @@ #include "../b2_user_settings.h" +#include "../spaces/box2d_space_contact_listener.h" #include "../spaces/box2d_sweep_test.h" #include "../bodies/box2d_direct_body_state.h" @@ -144,7 +145,7 @@ bool PhysicsServerBox2D::_shape_collide(const RID &p_shape_A, const Transform2D b2Shape *b2_shape_B = shape_B->get_transformed_b2Shape(shape_info_B, nullptr); SweepShape sweep_shape_A{ shape_A, sweepA, nullptr, shape_A_transform }; SweepShape sweep_shape_B{ shape_B, sweepB, nullptr, shape_B_transform }; - SweepTestResult output = Box2DSweepTest::shape_cast(sweep_shape_A, b2_shape_A, sweep_shape_B, b2_shape_B); + SweepTestResult output = Box2DSweepTest::shape_cast(sweep_shape_A, b2_shape_A, sweep_shape_B, b2_shape_B, 0); if (output.collision) { sweep_results.append(output); } @@ -981,14 +982,36 @@ bool PhysicsServerBox2D::_body_test_motion(const RID &p_body, const Transform2D shapes.append(shape); } - Vector query_result = Box2DSweepTest::query_aabb_motion(shapes, p_from, p_motion, p_margin, 0xff, true, false, (Box2DDirectSpaceState *)body->get_space_state()); - Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shapes, p_from, p_motion, p_margin, 0xff, true, false, 2048, query_result, (Box2DDirectSpaceState *)body->get_space_state()); - // Exclude bodies that are excluded + Vector query_result = Box2DSweepTest::query_aabb_motion(shapes, p_from, p_motion, p_margin, body->get_collision_layer(), body->get_collision_mask(), true, false, (Box2DDirectSpaceState *)body->get_space_state()); + Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(body->get_shapes(), p_from, p_motion, p_margin, true, false, 2048, query_result, (Box2DDirectSpaceState *)body->get_space_state()); for (int i = 0; i < sweep_test_results.size(); i++) { Box2DCollisionObject *body_B = sweep_test_results[i].sweep_shape_B.fixture->GetBody()->GetUserData().collision_object; + // collision exception if (!body_B || body->is_body_collision_excepted(body_B) || body_B->is_body_collision_excepted(body)) { sweep_test_results.remove_at(i); i--; + continue; + } + if (body_B != nullptr) { + ERR_FAIL_COND_V(!sweep_test_results[i].sweep_shape_A.fixture, false); + ERR_FAIL_COND_V(!sweep_test_results[i].sweep_shape_B.fixture, false); + b2FixtureUserData fixtureA_user_data = sweep_test_results[i].sweep_shape_A.fixture->GetUserData(); + b2FixtureUserData fixtureB_user_data = sweep_test_results[i].sweep_shape_B.fixture->GetUserData(); + b2Vec2 one_way_collision_direction_A(fixtureA_user_data.one_way_collision_direction_x, fixtureA_user_data.one_way_collision_direction_y); + b2Vec2 one_way_collision_direction_B(fixtureB_user_data.one_way_collision_direction_x, fixtureB_user_data.one_way_collision_direction_y); + bool one_way_collision_A = fixtureA_user_data.one_way_collision; + bool one_way_collision_B = fixtureB_user_data.one_way_collision; + bool should_disable_collision = false; + if (one_way_collision_A) { + should_disable_collision = Box2DSpaceContactListener::should_disable_collision_one_way_direction(one_way_collision_direction_A, body->get_b2Body(), body_B->get_b2Body(), body_B->get_b2Body()->GetLinearVelocity()); + } + if (one_way_collision_B && !should_disable_collision) { + should_disable_collision = Box2DSpaceContactListener::should_disable_collision_one_way_direction(one_way_collision_direction_B, body_B->get_b2Body(), body->get_b2Body(), godot_to_box2d(p_motion)); + } + if (should_disable_collision) { + sweep_test_results.remove_at(i); + i--; + } } } SweepTestResult sweep_test_result = Box2DSweepTest::closest_result_in_cast(sweep_test_results); @@ -1010,7 +1033,7 @@ bool PhysicsServerBox2D::_body_test_motion(const RID &p_body, const Transform2D current_result.collision_normal = -Vector2(sweep_test_result.manifold.normal.x, sweep_test_result.manifold.normal.y).normalized(); current_result.collider_velocity = box2d_to_godot(body_B->get_b2Body()->GetLinearVelocity()); current_result.collision_safe_fraction = sweep_test_result.safe_fraction(); - current_result.collision_unsafe_fraction = sweep_test_result.unsafe_fraction(current_result.collision_safe_fraction, p_margin); + current_result.collision_unsafe_fraction = sweep_test_result.unsafe_fraction(current_result.collision_safe_fraction); current_result.travel = p_motion * current_result.collision_safe_fraction; current_result.remainder = p_motion - current_result.travel; int shape_A_index = 0; diff --git a/src/shapes/box2d_shape.cpp b/src/shapes/box2d_shape.cpp index ad9f81b..6fa32ff 100644 --- a/src/shapes/box2d_shape.cpp +++ b/src/shapes/box2d_shape.cpp @@ -11,6 +11,9 @@ b2Shape *Box2DShape::get_transformed_b2Shape(ShapeInfo shape_info, Box2DCollisionObject *body) { b2Shape *shape = _get_transformed_b2Shape(shape_info, body); + if (!shape) { + return nullptr; + } created_shapes.append(shape); if (body) { shape_body_map[shape] = body; diff --git a/src/shapes/box2d_shape_concave_polygon.cpp b/src/shapes/box2d_shape_concave_polygon.cpp index 8794676..a14b173 100644 --- a/src/shapes/box2d_shape_concave_polygon.cpp +++ b/src/shapes/box2d_shape_concave_polygon.cpp @@ -13,9 +13,9 @@ constexpr float CHAIN_SEGMENT_SQUARE_SIZE = 0.5f; void Box2DShapeConcavePolygon::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY); PackedVector2Array points_array = p_data; - points.resize(points_array.size()); - for (int i = 0; i < points_array.size(); i++) { - points.write[i] = points_array[i]; + points.resize(points_array.size() / 2); + for (int i = 0; i < points_array.size(); i += 2) { + points.write[i / 2] = points_array[i]; } points = Box2DShapeConvexPolygon::make_sure_polygon_is_counterclockwise(points); points = Box2DShapeConvexPolygon::remove_points_that_are_too_close(points); @@ -26,9 +26,14 @@ void Box2DShapeConcavePolygon::set_data(const Variant &p_data) { Variant Box2DShapeConcavePolygon::get_data() const { Array points_array; - points_array.resize(points.size()); - for (int i = 0; i < points.size(); i++) { + points_array.resize(points.size() * 2); + for (int i = 1; i < points.size(); i++) { points_array[i] = points[i]; + points_array[i + 1] = points[i]; + } + if (points.size() > 0) { + points_array[0] = points[0]; + points_array[points_array.size() - 1] = points[0]; } return points_array; } @@ -42,17 +47,17 @@ b2Shape *Box2DShapeConcavePolygon::_get_transformed_b2Shape(ShapeInfo shape_info // make a chain shape if it's static if (shape_info.is_static) { ERR_FAIL_INDEX_V(shape_info.index, 1, nullptr); - b2ChainShape *shape = memnew(b2ChainShape); - b2Vec2 box2d_points[b2_maxPolygonVertices]; - for (int i = 0; i < points.size(); i++) { - box2d_points[i] = godot_to_box2d(shape_info.transform.xform(points[i])); - } int points_count = points.size(); if (points_count < 3) { - memdelete(shape); ERR_FAIL_COND_V(points_count < 3, nullptr); } + b2ChainShape *shape = memnew(b2ChainShape); + b2Vec2 *box2d_points = new b2Vec2[points_count]; + for (int i = 0; i < points.size(); i++) { + box2d_points[i] = godot_to_box2d(shape_info.transform.xform(points[i])); + } shape->CreateLoop(box2d_points, points_count); + delete[] box2d_points; return shape; } ERR_FAIL_COND_V(shape_info.index > points.size(), nullptr); diff --git a/src/shapes/box2d_shape_convex_polygon.cpp b/src/shapes/box2d_shape_convex_polygon.cpp index d9faf98..5854074 100644 --- a/src/shapes/box2d_shape_convex_polygon.cpp +++ b/src/shapes/box2d_shape_convex_polygon.cpp @@ -50,6 +50,9 @@ Variant Box2DShapeConvexPolygon::get_data() const { } int Box2DShapeConvexPolygon::get_b2Shape_count(bool is_static) const { + if (is_static) { + return 1; + } return polygons.size(); } @@ -193,24 +196,28 @@ int Box2DShapeConvexPolygon::remove_bad_points(b2Vec2 *vertices, int32 count) { b2Shape *Box2DShapeConvexPolygon::_get_transformed_b2Shape(ShapeInfo shape_info, Box2DCollisionObject *body) { ERR_FAIL_COND_V(shape_info.index >= polygons.size(), nullptr); Vector polygon = polygons[shape_info.index]; - ERR_FAIL_COND_V(polygon.size() > b2_maxPolygonVertices, nullptr); ERR_FAIL_COND_V(polygon.size() < 3, nullptr); - b2Vec2 b2_points[b2_maxPolygonVertices]; - for (int i = 0; i < polygon.size(); i++) { - b2_points[i] = godot_to_box2d(shape_info.transform.xform(polygon[i])); - } // make a chain shape if it's static if (shape_info.is_static) { ERR_FAIL_INDEX_V(shape_info.index, 1, nullptr); - b2ChainShape *shape = memnew(b2ChainShape); int points_count = points.size(); if (points_count < 3) { - memdelete(shape); ERR_FAIL_COND_V(points_count < 3, nullptr); } + b2ChainShape *shape = memnew(b2ChainShape); + b2Vec2 *b2_points = new b2Vec2[points_count]; + for (int i = 0; i < polygon.size(); i++) { + b2_points[i] = godot_to_box2d(shape_info.transform.xform(polygon[i])); + } shape->CreateLoop(b2_points, points_count); + delete[] b2_points; return shape; } + ERR_FAIL_COND_V(polygon.size() > b2_maxPolygonVertices, nullptr); + b2Vec2 b2_points[b2_maxPolygonVertices]; + for (int i = 0; i < polygon.size(); i++) { + b2_points[i] = godot_to_box2d(shape_info.transform.xform(polygon[i])); + } b2PolygonShape *shape = memnew(b2PolygonShape); int new_size = remove_bad_points(b2_points, polygon.size()); bool result = shape->Set(b2_points, new_size); diff --git a/src/spaces/box2d_direct_space_state.cpp b/src/spaces/box2d_direct_space_state.cpp index dafcf0f..01c237e 100644 --- a/src/spaces/box2d_direct_space_state.cpp +++ b/src/spaces/box2d_direct_space_state.cpp @@ -102,8 +102,8 @@ int32_t Box2DDirectSpaceState::_intersect_shape(const RID &shape_rid, const Tran return 0; } Box2DShape *shape = const_cast(const_shape); - Vector query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, this); - Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, max_results, query_result, this); + Vector query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, 0, collision_mask, collide_with_bodies, collide_with_areas, this); + Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, motion, margin, collide_with_bodies, collide_with_areas, max_results, query_result, this); if (sweep_test_results.is_empty()) { return 0; } @@ -130,12 +130,12 @@ bool Box2DDirectSpaceState::_cast_motion(const RID &shape_rid, const Transform2D const Box2DShape *const_shape = server->shape_owner.get_or_null(shape_rid); ERR_FAIL_COND_V(!const_shape, 0); Box2DShape *shape = const_cast(const_shape); - Vector query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, this); - Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, 2048, query_result, this); + Vector query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, 0, collision_mask, collide_with_bodies, collide_with_areas, this); + Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, motion, margin, collide_with_bodies, collide_with_areas, 2048, query_result, this); SweepTestResult sweep_test_result = Box2DSweepTest::closest_result_in_cast(sweep_test_results); if (sweep_test_result.collision && closest_safe != nullptr && closest_unsafe != nullptr) { *closest_safe = sweep_test_result.safe_fraction(); - *closest_unsafe = sweep_test_result.unsafe_fraction(*closest_safe, margin); + *closest_unsafe = sweep_test_result.unsafe_fraction(*closest_safe); } return true; } @@ -143,8 +143,8 @@ bool Box2DDirectSpaceState::_collide_shape(const RID &shape_rid, const Transform const Box2DShape *const_shape = server->shape_owner.get_or_null(shape_rid); ERR_FAIL_COND_V(!const_shape, 0); Box2DShape *shape = const_cast(const_shape); - Vector query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, this); - Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, max_results, query_result, this); + Vector query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, 0, collision_mask, collide_with_bodies, collide_with_areas, this); + Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, motion, margin, collide_with_bodies, collide_with_areas, max_results, query_result, this); if (!results) { return sweep_test_results.size(); } @@ -163,8 +163,8 @@ bool Box2DDirectSpaceState::_rest_info(const RID &shape_rid, const Transform2D & const Box2DShape *const_shape = server->shape_owner.get_or_null(shape_rid); ERR_FAIL_COND_V(!const_shape, 0); Box2DShape *shape = const_cast(const_shape); - Vector query_result = Box2DSweepTest::query_aabb_motion(shape, transform, Vector2(), margin, collision_mask, collide_with_bodies, collide_with_areas, this); - Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, Vector2(), margin, collision_mask, collide_with_bodies, collide_with_areas, 2048, query_result, this); + Vector query_result = Box2DSweepTest::query_aabb_motion(shape, transform, Vector2(), margin, 0, collision_mask, collide_with_bodies, collide_with_areas, this); + Vector sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, Vector2(), margin, collide_with_bodies, collide_with_areas, 2048, query_result, this); SweepTestResult sweep_test_result = Box2DSweepTest::closest_result_in_cast(sweep_test_results); if (!sweep_test_result.collision) { return false; diff --git a/src/spaces/box2d_query_callback.cpp b/src/spaces/box2d_query_callback.cpp index 5a64bc3..1cc4e32 100644 --- a/src/spaces/box2d_query_callback.cpp +++ b/src/spaces/box2d_query_callback.cpp @@ -5,11 +5,13 @@ #define QUERY_MAX_SIZE 2048 Box2DQueryCallback::Box2DQueryCallback(Box2DDirectSpaceState *p_space_state, + uint32_t p_collision_layer, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { space_state = p_space_state; collision_mask = p_collision_mask; + collision_layer = p_collision_layer; collide_with_bodies = p_collide_with_bodies; collide_with_areas = p_collide_with_areas; } @@ -19,8 +21,9 @@ Vector Box2DQueryCallback::get_results() { } bool Box2DQueryCallback::ReportFixture(b2Fixture *fixture) { - if ( // collision mask - ((fixture->GetFilterData().categoryBits & collision_mask) != 0) && + if ( // collision mask or layer + ((fixture->GetFilterData().categoryBits & collision_mask) != 0 || + (fixture->GetFilterData().maskBits & collision_layer) != 0) && // collide with area or body bit ((fixture->IsSensor() && collide_with_areas) || (!fixture->IsSensor() && collide_with_bodies))) { diff --git a/src/spaces/box2d_query_callback.h b/src/spaces/box2d_query_callback.h index 4f0bdf2..0854440 100644 --- a/src/spaces/box2d_query_callback.h +++ b/src/spaces/box2d_query_callback.h @@ -14,6 +14,7 @@ class Box2DDirectSpaceState; class Box2DQueryCallback : public b2QueryCallback { Box2DDirectSpaceState *space_state; Vector results; + uint32_t collision_layer; uint32_t collision_mask; bool collide_with_bodies; bool collide_with_areas; @@ -21,6 +22,7 @@ class Box2DQueryCallback : public b2QueryCallback { public: Box2DQueryCallback(Box2DDirectSpaceState *space_state, + uint32_t collision_layer, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas); diff --git a/src/spaces/box2d_space_contact_listener.cpp b/src/spaces/box2d_space_contact_listener.cpp index 6558def..0787c60 100644 --- a/src/spaces/box2d_space_contact_listener.cpp +++ b/src/spaces/box2d_space_contact_listener.cpp @@ -31,14 +31,9 @@ void Box2DSpaceContactListener::EndContact(b2Contact *contact) { handle_contact(contact, PhysicsServer2D::AreaBodyStatus::AREA_BODY_REMOVED); } -void Box2DSpaceContactListener::handle_static_constant_linear_velocity(b2Body *b2_body_A, Box2DCollisionObject *bodyA, b2Body *b2_body_B, Box2DCollisionObject *bodyB, b2Contact *contact) { +void Box2DSpaceContactListener::handle_static_constant_linear_velocity(b2Body *b2_body_A, Box2DCollisionObject *bodyA, b2Body *b2_body_B, Box2DCollisionObject *bodyB, b2WorldManifold worldManifold, int points_count) { if (b2_body_A->GetType() != b2BodyType::b2_dynamicBody && b2_body_B->GetType() == b2BodyType::b2_dynamicBody) { - if (!world_manifold_computed) { - contact->GetWorldManifold(&worldManifold); - world_manifold_computed = true; - } - int point_count = contact->GetManifold()->pointCount; - if (point_count > 0) { + if (points_count > 0) { b2_body_B->ApplyLinearImpulse(b2_body_B->GetMass() * bodyA->get_constant_linear_velocity(), worldManifold.points[0], true); } float inertia = b2_body_B->GetInertia() - b2_body_B->GetMass() * b2Dot(b2_body_B->GetLocalCenter(), b2_body_B->GetLocalCenter()); @@ -46,24 +41,20 @@ void Box2DSpaceContactListener::handle_static_constant_linear_velocity(b2Body *b } } -void Box2DSpaceContactListener::handle_one_way_direction(b2Vec2 one_way_collision_direction_A, b2Body *b2_body_A, b2Body *b2_body_B, b2Contact *contact) { - //if (!world_manifold_computed) { - //contact->GetWorldManifold(&worldManifold); - //world_manifold_computed = true; - //} +bool Box2DSpaceContactListener::should_disable_collision_one_way_direction(b2Vec2 one_way_collision_direction_A, b2Body *b2_body_A, b2Body *b2_body_B, b2Vec2 body_B_velocity) { + ERR_FAIL_COND_V(!b2_body_A, false); + ERR_FAIL_COND_V(!b2_body_B, false); b2Vec2 normal = b2Mul(b2_body_A->GetTransform().q, one_way_collision_direction_A); b2Vec2 body_A_velocity = b2_body_A->GetLinearVelocity(); - b2Vec2 body_B_velocity = b2_body_B->GetLinearVelocity(); - //b2Vec2 local_normal = contact->GetManifold()->localNormal; - // relative velocity + body_B_velocity -= body_A_velocity; body_B_velocity.Normalize(); float dot_product = b2Dot(body_B_velocity, normal); float passThroughThreshold = -b2_epsilon * 10; if (dot_product >= passThroughThreshold) { - contact->SetEnabled(false); - return; + return true; } + return false; } void Box2DSpaceContactListener::PreSolve(b2Contact *contact, const b2Manifold *oldManifold) { @@ -79,15 +70,21 @@ void Box2DSpaceContactListener::PreSolve(b2Contact *contact, const b2Manifold *o bool one_way_collision_B = fixtureB_user_data.one_way_collision; b2Vec2 one_way_collision_direction_A(fixtureA_user_data.one_way_collision_direction_x, fixtureA_user_data.one_way_collision_direction_y); b2Vec2 one_way_collision_direction_B(fixtureB_user_data.one_way_collision_direction_x, fixtureB_user_data.one_way_collision_direction_y); - handle_static_constant_linear_velocity(b2_body_A, bodyA, b2_body_B, bodyB, contact); - handle_static_constant_linear_velocity(b2_body_B, bodyB, b2_body_A, bodyA, contact); + contact->GetWorldManifold(&world_manifold); + handle_static_constant_linear_velocity(b2_body_A, bodyA, b2_body_B, bodyB, world_manifold, contact->GetManifold()->pointCount); + handle_static_constant_linear_velocity(b2_body_B, bodyB, b2_body_A, bodyA, world_manifold, contact->GetManifold()->pointCount); b2Vec2 b2_body_A_position = b2_body_A->GetPosition(); b2Vec2 b2_body_B_position = b2_body_B->GetPosition(); if (one_way_collision_A) { - handle_one_way_direction(one_way_collision_direction_A, b2_body_A, b2_body_B, contact); + if (should_disable_collision_one_way_direction(one_way_collision_direction_A, b2_body_A, b2_body_B, b2_body_B->GetLinearVelocity())) { + contact->SetEnabled(false); + return; + } } if (one_way_collision_B) { - handle_one_way_direction(one_way_collision_direction_B, b2_body_B, b2_body_A, contact); + if (should_disable_collision_one_way_direction(one_way_collision_direction_B, b2_body_B, b2_body_A, b2_body_A->GetLinearVelocity())) { + contact->SetEnabled(false); + } } } diff --git a/src/spaces/box2d_space_contact_listener.h b/src/spaces/box2d_space_contact_listener.h index 19aa99c..2d64682 100644 --- a/src/spaces/box2d_space_contact_listener.h +++ b/src/spaces/box2d_space_contact_listener.h @@ -13,15 +13,15 @@ class Box2DCollisionObject; class Box2DSpaceContactListener : public b2ContactListener { Box2DSpace *space; - b2WorldManifold worldManifold; - bool world_manifold_computed; + b2WorldManifold world_manifold; void handle_contact(b2Contact *p_space, PhysicsServer2D::AreaBodyStatus status); - void handle_static_constant_linear_velocity(b2Body *b2_body_A, Box2DCollisionObject *bodyA, b2Body *b2_body_B, Box2DCollisionObject *bodyB, b2Contact *contact); - void handle_one_way_direction(b2Vec2 one_way_collision_direction_A, b2Body *b2_body_A, b2Body *b2_body_B, b2Contact *contact); public: Box2DSpaceContactListener(Box2DSpace *p_space) : - space(p_space), world_manifold_computed(false) {} + space(p_space) {} + + static void handle_static_constant_linear_velocity(b2Body *b2_body_A, Box2DCollisionObject *bodyA, b2Body *b2_body_B, Box2DCollisionObject *bodyB, b2WorldManifold world_manifold, int points_count); + static bool should_disable_collision_one_way_direction(b2Vec2 one_way_collision_direction_A, b2Body *b2_body_A, b2Body *b2_body_B, b2Vec2 body_B_velocity); /// Called when two fixtures begin to touch. virtual void BeginContact(b2Contact *contact) override; diff --git a/src/spaces/box2d_sweep_test.cpp b/src/spaces/box2d_sweep_test.cpp index 2e6d0c4..87d4f9a 100644 --- a/src/spaces/box2d_sweep_test.cpp +++ b/src/spaces/box2d_sweep_test.cpp @@ -32,13 +32,16 @@ real_t SweepTestResult::safe_fraction() { return 0; } float safe_fraction = safe_length / motion_length; + if (safe_fraction < 0) { + safe_fraction = 0; + } return safe_fraction; } -real_t SweepTestResult::unsafe_fraction(float safe_fraction, float margin) { +real_t SweepTestResult::unsafe_fraction(float safe_fraction) { if (is_zero(safe_fraction) || safe_fraction < 0) { return 0; } - float unsafe_fraction = safe_fraction + margin; + float unsafe_fraction = safe_fraction; if (unsafe_fraction >= 1) { unsafe_fraction = 1; } @@ -143,17 +146,22 @@ IntersectionManifoldResult _evaluate_intersection_manifold(const b2Shape *p_shap return IntersectionManifoldResult{ manifold, flipped }; } -b2DistanceOutput _call_b2_distance(b2Transform p_transformA, b2Shape *shapeA, int child_index_A, b2Transform p_transformB, b2Shape *shapeB, int child_index_B) { +b2DistanceOutput _call_b2_distance(b2Transform p_transformA, b2Shape *shapeA, int child_index_A, b2Transform p_transformB, b2Shape *shapeB, int child_index_B, float extra_margin) { b2DistanceOutput output; b2DistanceInput input; b2SimplexCache cache; cache.count = 0; + extra_margin = godot_to_box2d(extra_margin); + shapeA->m_radius += extra_margin; + shapeB->m_radius += extra_margin; input.proxyA.Set(shapeA, child_index_A); input.proxyB.Set(shapeB, child_index_B); input.transformA = p_transformA; input.transformB = p_transformB; input.useRadii = true; b2Distance(&output, &cache, &input); + shapeA->m_radius -= extra_margin; + shapeB->m_radius -= extra_margin; b2PolygonShape *polyA = (b2PolygonShape *)shapeA; b2PolygonShape *polyB = (b2PolygonShape *)shapeB; return output; @@ -189,7 +197,7 @@ b2AABB get_shape_aabb(Box2DShape *shape, const b2Transform &shape_transform) { return aabb_total; } -SweepTestResult Box2DSweepTest::shape_cast(SweepShape p_sweep_shape_A, b2Shape *shape_A, SweepShape p_sweep_shape_B, b2Shape *shape_B) { +SweepTestResult Box2DSweepTest::shape_cast(SweepShape p_sweep_shape_A, b2Shape *shape_A, SweepShape p_sweep_shape_B, b2Shape *shape_B, float extra_margin) { b2TOIInput toi_input; b2TOIOutput toi_output; b2Sweep sweep_A = p_sweep_shape_A.sweep; @@ -212,7 +220,7 @@ SweepTestResult Box2DSweepTest::shape_cast(SweepShape p_sweep_shape_A, b2Shape * // move transform_A and B to end transform; sweep_A.GetTransform(&p_sweep_shape_A.transform, toi_output.t); sweep_B.GetTransform(&p_sweep_shape_B.transform, toi_output.t); - b2DistanceOutput distance_output = _call_b2_distance(p_sweep_shape_A.transform, shape_A, i, p_sweep_shape_B.transform, shape_B, j); + b2DistanceOutput distance_output = _call_b2_distance(p_sweep_shape_A.transform, shape_A, i, p_sweep_shape_B.transform, shape_B, j, extra_margin); if (distance_output.distance > b2_epsilon) { break; } @@ -241,15 +249,16 @@ SweepTestResult Box2DSweepTest::shape_cast(SweepShape p_sweep_shape_A, b2Shape * } return SweepTestResult{ p_sweep_shape_A, p_sweep_shape_B, b2DistanceOutput(), toi_output, 0, manifold, false }; } -Vector Box2DSweepTest::query_aabb_motion(Box2DShape *p_shape, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, Box2DDirectSpaceState *space_state) { +Vector Box2DSweepTest::query_aabb_motion(Box2DShape *p_shape, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_layer, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, Box2DDirectSpaceState *space_state) { Vector shapes; shapes.append(p_shape); - return query_aabb_motion(shapes, p_transform, p_motion, p_margin, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, space_state); + return query_aabb_motion(shapes, p_transform, p_motion, p_margin, p_collision_layer, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, space_state); } -Vector Box2DSweepTest::query_aabb_motion(Vector p_shapes, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, Box2DDirectSpaceState *space_state) { +Vector Box2DSweepTest::query_aabb_motion(Vector p_shapes, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_layer, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, Box2DDirectSpaceState *space_state) { b2Vec2 motion = godot_to_box2d(p_motion); Box2DQueryCallback callback(space_state, + p_collision_layer, p_collision_mask, p_collide_with_bodies, p_collide_with_areas); @@ -265,12 +274,14 @@ Vector Box2DSweepTest::query_aabb_motion(Vector p_sha return shapes_result; } -Vector Box2DSweepTest::multiple_shapes_cast(Box2DShape *p_shape, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, int32_t p_max_results, Vector p_other_fixtures, Box2DDirectSpaceState *space_state) { - Vector shapes; - shapes.append(p_shape); - return multiple_shapes_cast(shapes, p_transform, p_motion, p_margin, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_max_results, p_other_fixtures, space_state); +Vector Box2DSweepTest::multiple_shapes_cast(Box2DShape *p_shape, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, bool p_collide_with_bodies, bool p_collide_with_areas, int32_t p_max_results, Vector p_other_fixtures, Box2DDirectSpaceState *space_state) { + Vector shapes; + Box2DCollisionObject::Shape shape; + shape.shape = p_shape; + shapes.append(shape); + return multiple_shapes_cast(shapes, p_transform, p_motion, p_margin, p_collide_with_bodies, p_collide_with_areas, p_max_results, p_other_fixtures, space_state); } -Vector Box2DSweepTest::multiple_shapes_cast(Vector p_shapes, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, int32_t p_max_results, Vector p_other_fixtures, Box2DDirectSpaceState *space_state) { +Vector Box2DSweepTest::multiple_shapes_cast(Vector p_shapes, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, bool p_collide_with_bodies, bool p_collide_with_areas, int32_t p_max_results, Vector p_other_fixtures, Box2DDirectSpaceState *space_state) { Vector results; if (p_max_results == 0) { return results; @@ -292,18 +303,33 @@ Vector Box2DSweepTest::multiple_shapes_cast(VectorGetTransform(), body_B->GetLocalCenter(), b2Vec2_zero); - for (Box2DShape *box2d_shape_A : p_shapes) { + for (Box2DCollisionObject::Shape body_shape_A : p_shapes) { + Box2DShape *box2d_shape_A = body_shape_A.shape; for (int i = 0; i < box2d_shape_A->get_b2Shape_count(false); i++) { Box2DShape::ShapeInfo shape_info{ i, identity, false, false }; - b2Shape *shape_A = box2d_shape_A->get_transformed_b2Shape(shape_info, nullptr); + b2Shape *shape_A; + if (!body_shape_A.fixtures.is_empty()) { + shape_A = body_shape_A.fixtures[i]->GetShape(); + // check if shape body is same as one we are checking + if (body_shape_A.fixtures[i]->GetBody() == body_B) { + continue; + } + } else { + shape_A = box2d_shape_A->get_transformed_b2Shape(shape_info, nullptr); + } SweepShape sweep_shape_A{ box2d_shape_A, sweepA, nullptr, shape_A_transform }; + if (!body_shape_A.fixtures.is_empty()) { + sweep_shape_A.fixture = body_shape_A.fixtures[i]; + } SweepShape sweep_shape_B{ box2d_shape_B, sweepB, fixture_B, body_B->GetTransform() }; - SweepTestResult output = Box2DSweepTest::shape_cast(sweep_shape_A, shape_A, sweep_shape_B, shape_B); + SweepTestResult output = Box2DSweepTest::shape_cast(sweep_shape_A, shape_A, sweep_shape_B, shape_B, p_margin); if (output.collision) { results.append(output); } - box2d_shape_A->erase_shape(shape_A); - memdelete(shape_A); + if (body_shape_A.fixtures.is_empty()) { + box2d_shape_A->erase_shape(shape_A); + memdelete(shape_A); + } } } } diff --git a/src/spaces/box2d_sweep_test.h b/src/spaces/box2d_sweep_test.h index bdfeba9..5f81c14 100644 --- a/src/spaces/box2d_sweep_test.h +++ b/src/spaces/box2d_sweep_test.h @@ -1,5 +1,7 @@ #pragma once +#include "../bodies/box2d_collision_object.h" + #include #include #include @@ -35,16 +37,16 @@ struct SweepTestResult { b2WorldManifold manifold; bool collision = false; real_t safe_fraction(); - real_t unsafe_fraction(float safe_fraction, float margin); + real_t unsafe_fraction(float safe_fraction); }; class Box2DSweepTest { public: static b2Sweep create_b2_sweep(b2Transform p_transform, b2Vec2 p_center, b2Vec2 p_motion); - static SweepTestResult shape_cast(SweepShape p_sweep_shape_A, b2Shape *shape_A, SweepShape p_sweep_shape_B, b2Shape *shape_B); - static Vector query_aabb_motion(Box2DShape *p_shape, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, Box2DDirectSpaceState *space_state); - static Vector query_aabb_motion(Vector p_shapes, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, Box2DDirectSpaceState *space_state); - static Vector multiple_shapes_cast(Vector p_shapes, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, int32_t p_max_results, Vector p_other_fixtures, Box2DDirectSpaceState *space_state); - static Vector multiple_shapes_cast(Box2DShape *p_shape, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, int32_t p_max_results, Vector p_other_fixtures, Box2DDirectSpaceState *space_state); + static SweepTestResult shape_cast(SweepShape p_sweep_shape_A, b2Shape *shape_A, SweepShape p_sweep_shape_B, b2Shape *shape_B, float margin); + static Vector query_aabb_motion(Box2DShape *p_shape, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_layer, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, Box2DDirectSpaceState *space_state); + static Vector query_aabb_motion(Vector p_shapes, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, uint32_t p_collision_layer, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, Box2DDirectSpaceState *space_state); + static Vector multiple_shapes_cast(Vector p_shapes, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, bool p_collide_with_bodies, bool p_collide_with_areas, int32_t p_max_results, Vector p_other_fixtures, Box2DDirectSpaceState *space_state); + static Vector multiple_shapes_cast(Box2DShape *p_shape, const Transform2D &p_transform, const Vector2 &p_motion, double p_margin, bool p_collide_with_bodies, bool p_collide_with_areas, int32_t p_max_results, Vector p_other_fixtures, Box2DDirectSpaceState *space_state); static SweepTestResult closest_result_in_cast(Vector p_results); };