initial impl for character body one way collision (#34)

* initial impl

* fix all character one way tests

* fix moving of character body.

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)

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:
 

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);

src/bodies/box2d_collision_object.cpp

@@ -718,6 +718,9 @@ void Box2DCollisionObject::_set_space(Box2DSpace *p_space) {
 		_update_shapes();
 	}
 }
+Vector<Box2DCollisionObject::Shape> Box2DCollisionObject::get_shapes() {
+	return shapes;
+}
 
 int Box2DCollisionObject::get_shape_count() const { return shapes.size(); }
 Box2DShape *Box2DCollisionObject::get_shape(int p_index) const {

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<b2Shape *> shapes;
+		Vector<b2Fixture *> fixtures;
+		bool disabled = false;
+		bool one_way_collision = false;
+	};
 
 protected:
 	Type type;
@@ -42,14 +50,6 @@ class Box2DCollisionObject {
 	HashSet<Box2DCollisionObject *> collision_exception;
 	Vector<Box2DArea *> areas;
 
-	struct Shape {
-		Transform2D xform;
-		Box2DShape *shape = nullptr;
-		Vector<b2Shape *> shapes;
-		Vector<b2Fixture *> fixtures;
-		bool disabled = false;
-		bool one_way_collision = false;
-	};
 	Vector<Shape> 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<Shape> get_shapes();
 	void add_collision_exception(Box2DCollisionObject *excepted_body);
 	void remove_collision_exception(Box2DCollisionObject *excepted_body);
 	bool is_body_collision_excepted(Box2DCollisionObject *excepted_body);

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<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shapes, p_from, p_motion, p_margin, 0xff, true, false, (Box2DDirectSpaceState *)body->get_space_state());
-	Vector<SweepTestResult> 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<b2Fixture *> 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<SweepTestResult> 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;

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;

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);

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<Vector2> 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);

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<Box2DShape *>(const_shape);
-	Vector<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, this);
-	Vector<SweepTestResult> 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<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, 0, collision_mask, collide_with_bodies, collide_with_areas, this);
+	Vector<SweepTestResult> 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,21 +130,21 @@ 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<Box2DShape *>(const_shape);
-	Vector<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, this);
-	Vector<SweepTestResult> sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, 2048, query_result, this);
+	Vector<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, 0, collision_mask, collide_with_bodies, collide_with_areas, this);
+	Vector<SweepTestResult> 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;
 }
 bool Box2DDirectSpaceState::_collide_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, void *results, int32_t max_results, int32_t *result_count) {
 	const Box2DShape *const_shape = server->shape_owner.get_or_null(shape_rid);
 	ERR_FAIL_COND_V(!const_shape, 0);
 	Box2DShape *shape = const_cast<Box2DShape *>(const_shape);
-	Vector<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, collision_mask, collide_with_bodies, collide_with_areas, this);
-	Vector<SweepTestResult> 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<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shape, transform, motion, margin, 0, collision_mask, collide_with_bodies, collide_with_areas, this);
+	Vector<SweepTestResult> 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<Box2DShape *>(const_shape);
-	Vector<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shape, transform, Vector2(), margin, collision_mask, collide_with_bodies, collide_with_areas, this);
-	Vector<SweepTestResult> sweep_test_results = Box2DSweepTest::multiple_shapes_cast(shape, transform, Vector2(), margin, collision_mask, collide_with_bodies, collide_with_areas, 2048, query_result, this);
+	Vector<b2Fixture *> query_result = Box2DSweepTest::query_aabb_motion(shape, transform, Vector2(), margin, 0, collision_mask, collide_with_bodies, collide_with_areas, this);
+	Vector<SweepTestResult> 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;

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<b2Fixture *> 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))) {

src/spaces/box2d_query_callback.h

@@ -14,13 +14,15 @@ class Box2DDirectSpaceState;
 class Box2DQueryCallback : public b2QueryCallback {
 	Box2DDirectSpaceState *space_state;
 	Vector<b2Fixture *> results;
+	uint32_t collision_layer;
 	uint32_t collision_mask;
 	bool collide_with_bodies;
 	bool collide_with_areas;
 	int hit_count = 0;
 
 public:
 	Box2DQueryCallback(Box2DDirectSpaceState *space_state,
+			uint32_t collision_layer,
 			uint32_t collision_mask,
 			bool collide_with_bodies,
 			bool collide_with_areas);