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/servers/physics_server_box2d.cpp

@@ -145,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);
 			}
@@ -1033,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

@@ -135,7 +135,7 @@ bool Box2DDirectSpaceState::_cast_motion(const RID &shape_rid, const Transform2D
 	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;
 }

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;
 					}
@@ -302,6 +310,10 @@ Vector<SweepTestResult> Box2DSweepTest::multiple_shapes_cast(Vector<Box2DCollisi
 				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);
 				}
@@ -310,7 +322,7 @@ Vector<SweepTestResult> Box2DSweepTest::multiple_shapes_cast(Vector<Box2DCollisi
 					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);
 				}

src/spaces/box2d_sweep_test.h

@@ -37,13 +37,13 @@ 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 SweepTestResult shape_cast(SweepShape p_sweep_shape_A, b2Shape *shape_A, SweepShape p_sweep_shape_B, b2Shape *shape_B, float margin);
 	static Vector<b2Fixture *> 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<b2Fixture *> query_aabb_motion(Vector<Box2DShape *> 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<SweepTestResult> multiple_shapes_cast(Vector<Box2DCollisionObject::Shape> 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<b2Fixture *> p_other_fixtures, Box2DDirectSpaceState *space_state);