add position and velocity iteration. Also fix one way direction for r…

…igidbodies. (#32)

* add position and velocity iteration

* lint

* fix windows err

* upd

* upd

* upd

* upd

.gitmodules

@@ -4,7 +4,7 @@
 	branch = 4.1
 [submodule "box2d"]
 	path = box2d
-	url = https://github.com/godot-box2d/box2d
+	url = https://github.com/appsinacup/box2d
 	branch = common-assert-noop
 [submodule "Godot-Physics-Tests"]
 	path = Godot-Physics-Tests

CONTRIBUTING.md

@@ -46,6 +46,14 @@ NOTE: the simulation for box2d goes slower (eg. 30 fps), while for godot physics
 
 *Note*: The `template_debug` target can also be loaded in the Godot editor.
 
+## How to update submodule
+
+```
+git submodule sync
+git submodule update
+git submodule foreach git pull
+```
+
 ## Lint
 
 Run `scripts/clang-tidy.sh` in order to lint.

src/register_types.cpp

@@ -17,16 +17,22 @@ using namespace godot;
 static PhysicsServerBox2DFactory *box2d_factory = nullptr;
 
 void initialize_physics_server_box2d_module(ModuleInitializationLevel p_level) {
-	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
-		return;
+	switch (p_level) {
+		case MODULE_INITIALIZATION_LEVEL_SERVERS: {
+			ClassDB::register_class<Box2DDirectSpaceState>();
+			ClassDB::register_class<Box2DDirectBodyState>();
+			ClassDB::register_class<PhysicsServerBox2D>();
+			ClassDB::register_class<PhysicsServerBox2DFactory>();
+
+			box2d_factory = memnew(PhysicsServerBox2DFactory());
+			PhysicsServer2DManager::get_singleton()->register_server("Box2D", Callable(box2d_factory, "create_box2d_callback"));
+		} break;
+		case MODULE_INITIALIZATION_LEVEL_SCENE: {
+			Box2DProjectSettings::register_settings();
+		} break;
+		default: {
+		} break;
 	}
-	ClassDB::register_class<Box2DDirectSpaceState>();
-	ClassDB::register_class<Box2DDirectBodyState>();
-	ClassDB::register_class<PhysicsServerBox2D>();
-	ClassDB::register_class<PhysicsServerBox2DFactory>();
-
-	box2d_factory = memnew(PhysicsServerBox2DFactory());
-	PhysicsServer2DManager::get_singleton()->register_server("Box2D", Callable(box2d_factory, "create_box2d_callback"));
 }
 
 void uninitialize_physics_server_box2d_module(ModuleInitializationLevel p_level) {

src/servers/box2d_project_settings.cpp

@@ -0,0 +1,97 @@
+#include "box2d_project_settings.h"
+
+#include <godot_cpp/classes/project_settings.hpp>
+
+using namespace godot;
+
+constexpr char RUN_ON_SEPARATE_THREAD[] = "physics/2d/run_on_separate_thread";
+constexpr char MAX_THREADS[] = "threading/worker_pool/max_threads";
+constexpr char POSITION_ITERATIONS[] = "physics/box_2d/solver/position_iterations";
+constexpr char VELOCITY_ITERATIONS[] = "physics/box_2d/solver/velocity_iterations";
+
+void register_setting(
+		const String &p_name,
+		const Variant &p_value,
+		bool p_needs_restart,
+		PropertyHint p_hint,
+		const String &p_hint_string) {
+	ProjectSettings *project_settings = ProjectSettings::get_singleton();
+
+	if (!project_settings->has_setting(p_name)) {
+		project_settings->set(p_name, p_value);
+	}
+
+	Dictionary property_info;
+	property_info["name"] = p_name;
+	property_info["type"] = p_value.get_type();
+	property_info["hint"] = p_hint;
+	property_info["hint_string"] = p_hint_string;
+
+	project_settings->add_property_info(property_info);
+	project_settings->set_initial_value(p_name, p_value);
+	project_settings->set_restart_if_changed(p_name, p_needs_restart);
+
+	// HACK(mihe): We want our settings to appear in the order we register them in, but if we start
+	// the order at 0 we end up moving the entire `physics/` group to the top of the tree view, so
+	// instead we give it a hefty starting order and increment from there, which seems to give us
+	// the desired effect.
+	static int32_t order = 1000000;
+
+	project_settings->set_order(p_name, order++);
+}
+
+void register_setting_plain(
+		const String &p_name,
+		const Variant &p_value,
+		bool p_needs_restart = false) {
+	register_setting(p_name, p_value, p_needs_restart, PROPERTY_HINT_NONE, {});
+}
+
+void register_setting_hinted(
+		const String &p_name,
+		const Variant &p_value,
+		const String &p_hint_string,
+		bool p_needs_restart = false) {
+	register_setting(p_name, p_value, p_needs_restart, PROPERTY_HINT_NONE, p_hint_string);
+}
+
+void register_setting_ranged(
+		const String &p_name,
+		const Variant &p_value,
+		const String &p_hint_string,
+		bool p_needs_restart = false) {
+	register_setting(p_name, p_value, p_needs_restart, PROPERTY_HINT_RANGE, p_hint_string);
+}
+
+void Box2DProjectSettings::register_settings() {
+	register_setting_ranged(VELOCITY_ITERATIONS, 8, U"2,16,or_greater");
+	register_setting_ranged(POSITION_ITERATIONS, 3, U"1,16,or_greater");
+}
+
+template <typename TType>
+TType get_setting(const char *p_setting) {
+	const ProjectSettings *project_settings = ProjectSettings::get_singleton();
+	const Variant setting_value = project_settings->get_setting_with_override(p_setting);
+	const Variant::Type setting_type = setting_value.get_type();
+	const Variant::Type expected_type = Variant(TType()).get_type();
+
+	ERR_FAIL_COND_V(setting_type != expected_type, Variant());
+
+	return setting_value;
+}
+
+bool Box2DProjectSettings::should_run_on_separate_thread() {
+	return get_setting<bool>(RUN_ON_SEPARATE_THREAD);
+}
+
+int Box2DProjectSettings::get_max_threads() {
+	return get_setting<int>(MAX_THREADS);
+}
+
+int Box2DProjectSettings::get_position_iterations() {
+	return get_setting<int>(POSITION_ITERATIONS);
+}
+
+int Box2DProjectSettings::get_velocity_iterations() {
+	return get_setting<int>(VELOCITY_ITERATIONS);
+}

src/servers/box2d_project_settings.h

@@ -0,0 +1,11 @@
+#pragma once
+
+class Box2DProjectSettings {
+public:
+	static void register_settings();
+
+	static bool should_run_on_separate_thread();
+	static int get_max_threads();
+	static int get_position_iterations();
+	static int get_velocity_iterations();
+};

src/servers/physics_server_box2d.cpp

@@ -195,29 +195,11 @@ bool PhysicsServerBox2D::_space_is_active(const RID &p_space) const {
 }
 
 void PhysicsServerBox2D::_space_set_param(const RID &p_space, PhysicsServer2D::SpaceParameter p_param, double p_value) {
-	const Box2DSpace *space_const = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND(!space_const);
-	switch (p_param) {
-		case SPACE_PARAM_SOLVER_ITERATIONS: {
-			Box2DSpace *space = const_cast<Box2DSpace *>(space_const);
-			space->set_solver_iterations((int32)p_value);
-		} break;
-		default: {
-			ERR_PRINT("Unsupported space property");
-		}
-	}
+	// UNUSED
 }
 
 double PhysicsServerBox2D::_space_get_param(const RID &p_space, PhysicsServer2D::SpaceParameter p_param) const {
-	const Box2DSpace *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND_V(!space, 0);
-	switch (p_param) {
-		case SPACE_PARAM_SOLVER_ITERATIONS:
-			return (double)space->get_solver_iterations();
-		default: {
-			ERR_PRINT("Unsupported space property");
-		}
-	}
+	// UNUSED
 	return 0;
 }
 

src/shapes/box2d_shape_capsule.cpp

@@ -35,7 +35,7 @@ int Box2DShapeCapsule::get_b2Shape_count(bool is_static) const {
 b2Shape *Box2DShapeCapsule::_get_transformed_b2Shape(ShapeInfo shape_info, Box2DCollisionObject *body) {
 	ERR_FAIL_INDEX_V(shape_info.index, 3, nullptr);
 	Vector2 scale = shape_info.transform.get_scale();
-	if (scale.x != scale.y) {
+	if (fabs(scale.x - scale.y) > b2_epsilon) {
 		ERR_PRINT("Capsules don't support non uniform scale.");
 	}
 	float radius_scaled = godot_to_box2d(radius * scale.x);

src/shapes/box2d_shape_circle.cpp

@@ -21,7 +21,7 @@ b2Shape *Box2DShapeCircle::_get_transformed_b2Shape(ShapeInfo shape_info, Box2DC
 	ERR_FAIL_INDEX_V(shape_info.index, 1, nullptr);
 	b2CircleShape *shape = memnew(b2CircleShape);
 	Vector2 scale = shape_info.transform.get_scale();
-	if (scale.x != scale.y) {
+	if (fabs(scale.x - scale.y) > b2_epsilon) {
 		ERR_PRINT("Circles don't support non uniform scale.");
 	}
 	shape->m_radius = godot_to_box2d(radius * scale.x);

src/spaces/box2d_space.cpp

@@ -59,17 +59,9 @@ void Box2DSpace::set_debug_contacts(int32_t p_max_contacts) {
 	max_debug_contacts = p_max_contacts;
 }
 
-void Box2DSpace::set_solver_iterations(int32 p_iterations) {
-	solver_iterations = p_iterations;
-}
-
-int32 Box2DSpace::get_solver_iterations() const {
-	return solver_iterations;
-}
-
 void Box2DSpace::step(double p_step) {
-	const int32 velocityIterations = solver_iterations;
-	const int32 positionIterations = solver_iterations;
+	const int32 velocityIterations = Box2DProjectSettings::get_velocity_iterations();
+	const int32 positionIterations = Box2DProjectSettings::get_position_iterations();
 
 	const SelfList<Box2DBody>::List *body_list = &get_active_body_list();
 	const SelfList<Box2DBody> *b = body_list->first();

src/spaces/box2d_space.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "../servers/box2d_project_settings.h"
+
 #include <godot_cpp/core/defs.hpp>
 #include <godot_cpp/templates/self_list.hpp>
 #include <godot_cpp/variant/packed_vector2_array.hpp>
@@ -34,7 +36,6 @@ class Box2DSpace {
 	int max_debug_contacts = 0;
 	ProcessInfo process_info;
 	bool locked = false;
-	double solver_iterations = 8;
 
 	Box2DDirectSpaceState *direct_state = nullptr;
 	Box2DSpaceContactFilter *contact_filter;
@@ -53,9 +54,6 @@ class Box2DSpace {
 
 	void set_debug_contacts(int32_t max_contacts);
 
-	void set_solver_iterations(int32 iterations);
-	int32 get_solver_iterations() const;
-
 	void step(double p_step);
 
 	void call_queries();

src/spaces/box2d_space_contact_listener.cpp

@@ -47,14 +47,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;
-	}
+	//if (!world_manifold_computed) {
+	//contact->GetWorldManifold(&worldManifold);
+	//world_manifold_computed = true;
+	//}
 	b2Vec2 normal = b2Mul(b2_body_A->GetTransform().q, one_way_collision_direction_A);
-	b2Vec2 vel = b2_body_B->GetLinearVelocity();
-	vel.Normalize();
-	if (b2Dot(vel, normal) > 0.9f) {
+	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;
 	}
@@ -79,11 +85,9 @@ void Box2DSpaceContactListener::PreSolve(b2Contact *contact, const b2Manifold *o
 	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);
-		return;
 	}
 	if (one_way_collision_B) {
 		handle_one_way_direction(one_way_collision_direction_B, b2_body_B, b2_body_A, contact);
-		return;
 	}
 }