Move Godot Physics 2D into a module; add dummy 2D physics server

If the module is enabled (default), 2D physics works as it did before.

If the module is disabled and no other 2D physics server is registered
(via a module or GDExtension), then we fall back to a dummy
implementation which effectively disables 2D physics functionality (and
a warning is printed).

The dummy 2D physics server can also be selected explicitly, in which
case no warning is printed.

35 files changed, 11537 insertions, 0 deletions

diff --git a/modules/godot_physics_2d/SCsub b/modules/godot_physics_2d/SCsub
new file mode 100644
index 0000000000..5d93da5ecf
--- /dev/null
+++ b/modules/godot_physics_2d/SCsub
@@ -0,0 +1,5 @@
+#!/usr/bin/env python
+
+Import('env')
+
+env.add_source_files(env.modules_sources, "*.cpp")
diff --git a/modules/godot_physics_2d/config.py b/modules/godot_physics_2d/config.py
new file mode 100644
index 0000000000..d22f9454ed
--- /dev/null
+++ b/modules/godot_physics_2d/config.py
@@ -0,0 +1,6 @@
+def can_build(env, platform):
+    return True
+
+
+def configure(env):
+    pass
diff --git a/modules/godot_physics_2d/godot_area_2d.cpp b/modules/godot_physics_2d/godot_area_2d.cpp
new file mode 100644
index 0000000000..d6c786706c
--- /dev/null
+++ b/modules/godot_physics_2d/godot_area_2d.cpp
@@ -0,0 +1,314 @@
+/**************************************************************************/
+/*  godot_area_2d.cpp                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_area_2d.h"
+#include "godot_body_2d.h"
+#include "godot_space_2d.h"
+
+GodotArea2D::BodyKey::BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	rid = p_body->get_self();
+	instance_id = p_body->get_instance_id();
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+}
+
+GodotArea2D::BodyKey::BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	rid = p_body->get_self();
+	instance_id = p_body->get_instance_id();
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+}
+
+void GodotArea2D::_shapes_changed() {
+	if (!moved_list.in_list() && get_space()) {
+		get_space()->area_add_to_moved_list(&moved_list);
+	}
+}
+
+void GodotArea2D::set_transform(const Transform2D &p_transform) {
+	if (!moved_list.in_list() && get_space()) {
+		get_space()->area_add_to_moved_list(&moved_list);
+	}
+
+	_set_transform(p_transform);
+	_set_inv_transform(p_transform.affine_inverse());
+}
+
+void GodotArea2D::set_space(GodotSpace2D *p_space) {
+	if (get_space()) {
+		if (monitor_query_list.in_list()) {
+			get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+		}
+		if (moved_list.in_list()) {
+			get_space()->area_remove_from_moved_list(&moved_list);
+		}
+	}
+
+	monitored_bodies.clear();
+	monitored_areas.clear();
+
+	_set_space(p_space);
+}
+
+void GodotArea2D::set_monitor_callback(const Callable &p_callback) {
+	_unregister_shapes();
+
+	monitor_callback = p_callback;
+
+	monitored_bodies.clear();
+	monitored_areas.clear();
+
+	_shape_changed();
+
+	if (!moved_list.in_list() && get_space()) {
+		get_space()->area_add_to_moved_list(&moved_list);
+	}
+}
+
+void GodotArea2D::set_area_monitor_callback(const Callable &p_callback) {
+	_unregister_shapes();
+
+	area_monitor_callback = p_callback;
+
+	monitored_bodies.clear();
+	monitored_areas.clear();
+
+	_shape_changed();
+
+	if (!moved_list.in_list() && get_space()) {
+		get_space()->area_add_to_moved_list(&moved_list);
+	}
+}
+
+void GodotArea2D::_set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode) {
+	bool do_override = p_new_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override == (r_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) {
+		return;
+	}
+	_unregister_shapes();
+	r_mode = p_new_mode;
+	_shape_changed();
+}
+
+void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) {
+	switch (p_param) {
+		case PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE:
+			_set_space_override_mode(gravity_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value);
+			break;
+		case PhysicsServer2D::AREA_PARAM_GRAVITY:
+			gravity = p_value;
+			break;
+		case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR:
+			gravity_vector = p_value;
+			break;
+		case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT:
+			gravity_is_point = p_value;
+			break;
+		case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE:
+			gravity_point_unit_distance = p_value;
+			break;
+		case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE:
+			_set_space_override_mode(linear_damping_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value);
+			break;
+		case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP:
+			linear_damp = p_value;
+			break;
+		case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE:
+			_set_space_override_mode(angular_damping_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value);
+			break;
+		case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP:
+			angular_damp = p_value;
+			break;
+		case PhysicsServer2D::AREA_PARAM_PRIORITY:
+			priority = p_value;
+			break;
+	}
+}
+
+Variant GodotArea2D::get_param(PhysicsServer2D::AreaParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE:
+			return gravity_override_mode;
+		case PhysicsServer2D::AREA_PARAM_GRAVITY:
+			return gravity;
+		case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR:
+			return gravity_vector;
+		case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT:
+			return gravity_is_point;
+		case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE:
+			return gravity_point_unit_distance;
+		case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE:
+			return linear_damping_override_mode;
+		case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP:
+			return linear_damp;
+		case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE:
+			return angular_damping_override_mode;
+		case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP:
+			return angular_damp;
+		case PhysicsServer2D::AREA_PARAM_PRIORITY:
+			return priority;
+	}
+
+	return Variant();
+}
+
+void GodotArea2D::_queue_monitor_update() {
+	ERR_FAIL_NULL(get_space());
+
+	if (!monitor_query_list.in_list()) {
+		get_space()->area_add_to_monitor_query_list(&monitor_query_list);
+	}
+}
+
+void GodotArea2D::set_monitorable(bool p_monitorable) {
+	if (monitorable == p_monitorable) {
+		return;
+	}
+
+	monitorable = p_monitorable;
+	_set_static(!monitorable);
+	_shapes_changed();
+}
+
+void GodotArea2D::call_queries() {
+	if (!monitor_callback.is_null() && !monitored_bodies.is_empty()) {
+		if (monitor_callback.is_valid()) {
+			Variant res[5];
+			Variant *resptr[5];
+			for (int i = 0; i < 5; i++) {
+				resptr[i] = &res[i];
+			}
+
+			for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) {
+				if (E->value.state == 0) { // Nothing happened
+					HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
+					++next;
+					monitored_bodies.remove(E);
+					E = next;
+					continue;
+				}
+
+				res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
+				res[1] = E->key.rid;
+				res[2] = E->key.instance_id;
+				res[3] = E->key.body_shape;
+				res[4] = E->key.area_shape;
+
+				HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
+				++next;
+				monitored_bodies.remove(E);
+				E = next;
+
+				Callable::CallError ce;
+				Variant ret;
+				monitor_callback.callp((const Variant **)resptr, 5, ret, ce);
+
+				if (ce.error != Callable::CallError::CALL_OK) {
+					ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(monitor_callback, (const Variant **)resptr, 5, ce));
+				}
+			}
+		} else {
+			monitored_bodies.clear();
+			monitor_callback = Callable();
+		}
+	}
+
+	if (!area_monitor_callback.is_null() && !monitored_areas.is_empty()) {
+		if (area_monitor_callback.is_valid()) {
+			Variant res[5];
+			Variant *resptr[5];
+			for (int i = 0; i < 5; i++) {
+				resptr[i] = &res[i];
+			}
+
+			for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) {
+				if (E->value.state == 0) { // Nothing happened
+					HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
+					++next;
+					monitored_areas.remove(E);
+					E = next;
+					continue;
+				}
+
+				res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED;
+				res[1] = E->key.rid;
+				res[2] = E->key.instance_id;
+				res[3] = E->key.body_shape;
+				res[4] = E->key.area_shape;
+
+				HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E;
+				++next;
+				monitored_areas.remove(E);
+				E = next;
+
+				Callable::CallError ce;
+				Variant ret;
+				area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce);
+
+				if (ce.error != Callable::CallError::CALL_OK) {
+					ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce));
+				}
+			}
+		} else {
+			monitored_areas.clear();
+			area_monitor_callback = Callable();
+		}
+	}
+}
+
+void GodotArea2D::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const {
+	if (is_gravity_point()) {
+		const real_t gr_unit_dist = get_gravity_point_unit_distance();
+		Vector2 v = get_transform().xform(get_gravity_vector()) - p_position;
+		if (gr_unit_dist > 0) {
+			const real_t v_length_sq = v.length_squared();
+			if (v_length_sq > 0) {
+				const real_t gravity_strength = get_gravity() * gr_unit_dist * gr_unit_dist / v_length_sq;
+				r_gravity = v.normalized() * gravity_strength;
+			} else {
+				r_gravity = Vector2();
+			}
+		} else {
+			r_gravity = v.normalized() * get_gravity();
+		}
+	} else {
+		r_gravity = get_gravity_vector() * get_gravity();
+	}
+}
+
+GodotArea2D::GodotArea2D() :
+		GodotCollisionObject2D(TYPE_AREA),
+		monitor_query_list(this),
+		moved_list(this) {
+	_set_static(true); //areas are not active by default
+}
+
+GodotArea2D::~GodotArea2D() {
+}
diff --git a/modules/godot_physics_2d/godot_area_2d.h b/modules/godot_physics_2d/godot_area_2d.h
new file mode 100644
index 0000000000..e6c3b45d6c
--- /dev/null
+++ b/modules/godot_physics_2d/godot_area_2d.h
@@ -0,0 +1,191 @@
+/**************************************************************************/
+/*  godot_area_2d.h                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_AREA_2D_H
+#define GODOT_AREA_2D_H
+
+#include "godot_collision_object_2d.h"
+
+#include "core/templates/self_list.h"
+#include "servers/physics_server_2d.h"
+
+class GodotSpace2D;
+class GodotBody2D;
+class GodotConstraint2D;
+
+class GodotArea2D : public GodotCollisionObject2D {
+	PhysicsServer2D::AreaSpaceOverrideMode gravity_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
+	PhysicsServer2D::AreaSpaceOverrideMode linear_damping_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
+	PhysicsServer2D::AreaSpaceOverrideMode angular_damping_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
+
+	real_t gravity = 9.80665;
+	Vector2 gravity_vector = Vector2(0, -1);
+	bool gravity_is_point = false;
+	real_t gravity_point_unit_distance = 0.0;
+	real_t linear_damp = 0.1;
+	real_t angular_damp = 1.0;
+	int priority = 0;
+	bool monitorable = false;
+
+	Callable monitor_callback;
+
+	Callable area_monitor_callback;
+
+	SelfList<GodotArea2D> monitor_query_list;
+	SelfList<GodotArea2D> moved_list;
+
+	struct BodyKey {
+		RID rid;
+		ObjectID instance_id;
+		uint32_t body_shape = 0;
+		uint32_t area_shape = 0;
+
+		static uint32_t hash(const BodyKey &p_key) {
+			uint32_t h = hash_one_uint64(p_key.rid.get_id());
+			h = hash_murmur3_one_64(p_key.instance_id, h);
+			h = hash_murmur3_one_32(p_key.area_shape, h);
+			return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h));
+		}
+
+		_FORCE_INLINE_ bool operator==(const BodyKey &p_key) const {
+			return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape;
+		}
+
+		_FORCE_INLINE_ BodyKey() {}
+		BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+		BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+	};
+
+	struct BodyState {
+		int state = 0;
+		_FORCE_INLINE_ void inc() { state++; }
+		_FORCE_INLINE_ void dec() { state--; }
+	};
+
+	HashMap<BodyKey, BodyState, BodyKey> monitored_bodies;
+	HashMap<BodyKey, BodyState, BodyKey> monitored_areas;
+
+	HashSet<GodotConstraint2D *> constraints;
+
+	virtual void _shapes_changed() override;
+	void _queue_monitor_update();
+
+	void _set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode);
+
+public:
+	void set_monitor_callback(const Callable &p_callback);
+	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback.is_valid(); }
+
+	void set_area_monitor_callback(const Callable &p_callback);
+	_FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback.is_valid(); }
+
+	_FORCE_INLINE_ void add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+
+	_FORCE_INLINE_ void add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
+	_FORCE_INLINE_ void remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
+
+	void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value);
+	Variant get_param(PhysicsServer2D::AreaParameter p_param) const;
+
+	_FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; }
+	_FORCE_INLINE_ real_t get_gravity() const { return gravity; }
+
+	_FORCE_INLINE_ void set_gravity_vector(const Vector2 &p_gravity) { gravity_vector = p_gravity; }
+	_FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; }
+
+	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; }
+	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
+
+	_FORCE_INLINE_ void set_gravity_point_unit_distance(real_t scale) { gravity_point_unit_distance = scale; }
+	_FORCE_INLINE_ real_t get_gravity_point_unit_distance() const { return gravity_point_unit_distance; }
+
+	_FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; }
+	_FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; }
+
+	_FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; }
+	_FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; }
+
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
+	_FORCE_INLINE_ int get_priority() const { return priority; }
+
+	_FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const HashSet<GodotConstraint2D *> &get_constraints() const { return constraints; }
+	_FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
+
+	void set_monitorable(bool p_monitorable);
+	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
+
+	void set_transform(const Transform2D &p_transform);
+
+	void set_space(GodotSpace2D *p_space) override;
+
+	void call_queries();
+
+	void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const;
+
+	GodotArea2D();
+	~GodotArea2D();
+};
+
+void GodotArea2D::add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	BodyKey bk(p_body, p_body_shape, p_area_shape);
+	monitored_bodies[bk].inc();
+	if (!monitor_query_list.in_list()) {
+		_queue_monitor_update();
+	}
+}
+
+void GodotArea2D::remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	BodyKey bk(p_body, p_body_shape, p_area_shape);
+	monitored_bodies[bk].dec();
+	if (get_space() && !monitor_query_list.in_list()) {
+		_queue_monitor_update();
+	}
+}
+
+void GodotArea2D::add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
+	BodyKey bk(p_area, p_area_shape, p_self_shape);
+	monitored_areas[bk].inc();
+	if (!monitor_query_list.in_list()) {
+		_queue_monitor_update();
+	}
+}
+
+void GodotArea2D::remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
+	BodyKey bk(p_area, p_area_shape, p_self_shape);
+	monitored_areas[bk].dec();
+	if (get_space() && !monitor_query_list.in_list()) {
+		_queue_monitor_update();
+	}
+}
+
+#endif // GODOT_AREA_2D_H
diff --git a/modules/godot_physics_2d/godot_area_pair_2d.cpp b/modules/godot_physics_2d/godot_area_pair_2d.cpp
new file mode 100644
index 0000000000..ca12e30c29
--- /dev/null
+++ b/modules/godot_physics_2d/godot_area_pair_2d.cpp
@@ -0,0 +1,203 @@
+/**************************************************************************/
+/*  godot_area_pair_2d.cpp                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_area_pair_2d.h"
+#include "godot_collision_solver_2d.h"
+
+bool GodotAreaPair2D::setup(real_t p_step) {
+	bool result = false;
+	if (area->collides_with(body) && GodotCollisionSolver2D::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;
+	}
+
+	process_collision = false;
+	has_space_override = false;
+	if (result != colliding) {
+		if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		} else if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		} else if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		}
+		process_collision = has_space_override;
+
+		if (area->has_monitor_callback()) {
+			process_collision = true;
+		}
+
+		colliding = result;
+	}
+
+	return process_collision;
+}
+
+bool GodotAreaPair2D::pre_solve(real_t p_step) {
+	if (!process_collision) {
+		return false;
+	}
+
+	if (colliding) {
+		if (has_space_override) {
+			body_has_attached_area = true;
+			body->add_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->add_body_to_query(body, body_shape, area_shape);
+		}
+	} else {
+		if (has_space_override) {
+			body_has_attached_area = false;
+			body->remove_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->remove_body_from_query(body, body_shape, area_shape);
+		}
+	}
+
+	return false; // Never do any post solving.
+}
+
+void GodotAreaPair2D::solve(real_t p_step) {
+	// Nothing to do.
+}
+
+GodotAreaPair2D::GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape) {
+	body = p_body;
+	area = p_area;
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+	body->add_constraint(this, 0);
+	area->add_constraint(this);
+	if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair
+		p_body->set_active(true);
+	}
+}
+
+GodotAreaPair2D::~GodotAreaPair2D() {
+	if (colliding) {
+		if (body_has_attached_area) {
+			body_has_attached_area = false;
+			body->remove_area(area);
+		}
+		if (area->has_monitor_callback()) {
+			area->remove_body_from_query(body, body_shape, area_shape);
+		}
+	}
+	body->remove_constraint(this, 0);
+	area->remove_constraint(this);
+}
+
+//////////////////////////////////
+
+bool GodotArea2Pair2D::setup(real_t p_step) {
+	bool result_a = area_a->collides_with(area_b);
+	bool result_b = area_b->collides_with(area_a);
+	if ((result_a || result_b) && !GodotCollisionSolver2D::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_a = false;
+		result_b = false;
+	}
+
+	bool process_collision = false;
+
+	process_collision_a = false;
+	if (result_a != colliding_a) {
+		if (area_a->has_area_monitor_callback() && area_b_monitorable) {
+			process_collision_a = true;
+			process_collision = true;
+		}
+		colliding_a = result_a;
+	}
+
+	process_collision_b = false;
+	if (result_b != colliding_b) {
+		if (area_b->has_area_monitor_callback() && area_a_monitorable) {
+			process_collision_b = true;
+			process_collision = true;
+		}
+		colliding_b = result_b;
+	}
+
+	return process_collision;
+}
+
+bool GodotArea2Pair2D::pre_solve(real_t p_step) {
+	if (process_collision_a) {
+		if (colliding_a) {
+			area_a->add_area_to_query(area_b, shape_b, shape_a);
+		} else {
+			area_a->remove_area_from_query(area_b, shape_b, shape_a);
+		}
+	}
+
+	if (process_collision_b) {
+		if (colliding_b) {
+			area_b->add_area_to_query(area_a, shape_a, shape_b);
+		} else {
+			area_b->remove_area_from_query(area_a, shape_a, shape_b);
+		}
+	}
+
+	return false; // Never do any post solving.
+}
+
+void GodotArea2Pair2D::solve(real_t p_step) {
+	// Nothing to do.
+}
+
+GodotArea2Pair2D::GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b) {
+	area_a = p_area_a;
+	area_b = p_area_b;
+	shape_a = p_shape_a;
+	shape_b = p_shape_b;
+	area_a_monitorable = area_a->is_monitorable();
+	area_b_monitorable = area_b->is_monitorable();
+	area_a->add_constraint(this);
+	area_b->add_constraint(this);
+}
+
+GodotArea2Pair2D::~GodotArea2Pair2D() {
+	if (colliding_a) {
+		if (area_a->has_area_monitor_callback() && area_b_monitorable) {
+			area_a->remove_area_from_query(area_b, shape_b, shape_a);
+		}
+	}
+
+	if (colliding_b) {
+		if (area_b->has_area_monitor_callback() && area_a_monitorable) {
+			area_b->remove_area_from_query(area_a, shape_a, shape_b);
+		}
+	}
+
+	area_a->remove_constraint(this);
+	area_b->remove_constraint(this);
+}
diff --git a/modules/godot_physics_2d/godot_area_pair_2d.h b/modules/godot_physics_2d/godot_area_pair_2d.h
new file mode 100644
index 0000000000..eb091288a9
--- /dev/null
+++ b/modules/godot_physics_2d/godot_area_pair_2d.h
@@ -0,0 +1,78 @@
+/**************************************************************************/
+/*  godot_area_pair_2d.h                                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_AREA_PAIR_2D_H
+#define GODOT_AREA_PAIR_2D_H
+
+#include "godot_area_2d.h"
+#include "godot_body_2d.h"
+#include "godot_constraint_2d.h"
+
+class GodotAreaPair2D : public GodotConstraint2D {
+	GodotBody2D *body = nullptr;
+	GodotArea2D *area = nullptr;
+	int body_shape = 0;
+	int area_shape = 0;
+	bool colliding = false;
+	bool has_space_override = false;
+	bool process_collision = false;
+	bool body_has_attached_area = false;
+
+public:
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape);
+	~GodotAreaPair2D();
+};
+
+class GodotArea2Pair2D : public GodotConstraint2D {
+	GodotArea2D *area_a = nullptr;
+	GodotArea2D *area_b = nullptr;
+	int shape_a = 0;
+	int shape_b = 0;
+	bool colliding_a = false;
+	bool colliding_b = false;
+	bool process_collision_a = false;
+	bool process_collision_b = false;
+	bool area_a_monitorable;
+	bool area_b_monitorable;
+
+public:
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b);
+	~GodotArea2Pair2D();
+};
+
+#endif // GODOT_AREA_PAIR_2D_H
diff --git a/modules/godot_physics_2d/godot_body_2d.cpp b/modules/godot_physics_2d/godot_body_2d.cpp
new file mode 100644
index 0000000000..c401e6eee7
--- /dev/null
+++ b/modules/godot_physics_2d/godot_body_2d.cpp
@@ -0,0 +1,762 @@
+/**************************************************************************/
+/*  godot_body_2d.cpp                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_body_2d.h"
+
+#include "godot_area_2d.h"
+#include "godot_body_direct_state_2d.h"
+#include "godot_space_2d.h"
+
+void GodotBody2D::_mass_properties_changed() {
+	if (get_space() && !mass_properties_update_list.in_list()) {
+		get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
+	}
+}
+
+void GodotBody2D::update_mass_properties() {
+	//update shapes and motions
+
+	switch (mode) {
+		case PhysicsServer2D::BODY_MODE_RIGID: {
+			real_t total_area = 0;
+			for (int i = 0; i < get_shape_count(); i++) {
+				if (is_shape_disabled(i)) {
+					continue;
+				}
+				total_area += get_shape_aabb(i).get_area();
+			}
+
+			if (calculate_center_of_mass) {
+				// We have to recompute the center of mass.
+				center_of_mass_local = Vector2();
+
+				if (total_area != 0.0) {
+					for (int i = 0; i < get_shape_count(); i++) {
+						if (is_shape_disabled(i)) {
+							continue;
+						}
+
+						real_t area = get_shape_aabb(i).get_area();
+
+						real_t mass_new = area * mass / total_area;
+
+						// NOTE: we assume that the shape origin is also its center of mass.
+						center_of_mass_local += mass_new * get_shape_transform(i).get_origin();
+					}
+
+					center_of_mass_local /= mass;
+				}
+			}
+
+			if (calculate_inertia) {
+				inertia = 0;
+
+				for (int i = 0; i < get_shape_count(); i++) {
+					if (is_shape_disabled(i)) {
+						continue;
+					}
+
+					const GodotShape2D *shape = get_shape(i);
+
+					real_t area = get_shape_aabb(i).get_area();
+					if (area == 0.0) {
+						continue;
+					}
+
+					real_t mass_new = area * mass / total_area;
+
+					Transform2D mtx = get_shape_transform(i);
+					Vector2 scale = mtx.get_scale();
+					Vector2 shape_origin = mtx.get_origin() - center_of_mass_local;
+					inertia += shape->get_moment_of_inertia(mass_new, scale) + mass_new * shape_origin.length_squared();
+				}
+			}
+
+			_inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0;
+
+			if (mass) {
+				_inv_mass = 1.0 / mass;
+			} else {
+				_inv_mass = 0;
+			}
+
+		} break;
+		case PhysicsServer2D::BODY_MODE_KINEMATIC:
+		case PhysicsServer2D::BODY_MODE_STATIC: {
+			_inv_inertia = 0;
+			_inv_mass = 0;
+		} break;
+		case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: {
+			_inv_inertia = 0;
+			_inv_mass = 1.0 / mass;
+
+		} break;
+	}
+
+	_update_transform_dependent();
+}
+
+void GodotBody2D::reset_mass_properties() {
+	calculate_inertia = true;
+	calculate_center_of_mass = true;
+	_mass_properties_changed();
+}
+
+void GodotBody2D::set_active(bool p_active) {
+	if (active == p_active) {
+		return;
+	}
+
+	active = p_active;
+
+	if (active) {
+		if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
+			// Static bodies can't be active.
+			active = false;
+		} else if (get_space()) {
+			get_space()->body_add_to_active_list(&active_list);
+		}
+	} else if (get_space()) {
+		get_space()->body_remove_from_active_list(&active_list);
+	}
+}
+
+void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) {
+	switch (p_param) {
+		case PhysicsServer2D::BODY_PARAM_BOUNCE: {
+			bounce = p_value;
+		} break;
+		case PhysicsServer2D::BODY_PARAM_FRICTION: {
+			friction = p_value;
+		} break;
+		case PhysicsServer2D::BODY_PARAM_MASS: {
+			real_t mass_value = p_value;
+			ERR_FAIL_COND(mass_value <= 0);
+			mass = mass_value;
+			if (mode >= PhysicsServer2D::BODY_MODE_RIGID) {
+				_mass_properties_changed();
+			}
+		} break;
+		case PhysicsServer2D::BODY_PARAM_INERTIA: {
+			real_t inertia_value = p_value;
+			if (inertia_value <= 0.0) {
+				calculate_inertia = true;
+				if (mode == PhysicsServer2D::BODY_MODE_RIGID) {
+					_mass_properties_changed();
+				}
+			} else {
+				calculate_inertia = false;
+				inertia = inertia_value;
+				if (mode == PhysicsServer2D::BODY_MODE_RIGID) {
+					_inv_inertia = 1.0 / inertia;
+				}
+			}
+		} break;
+		case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
+			calculate_center_of_mass = false;
+			center_of_mass_local = p_value;
+			_update_transform_dependent();
+		} break;
+		case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
+			if (Math::is_zero_approx(gravity_scale)) {
+				wakeup();
+			}
+			gravity_scale = p_value;
+		} break;
+		case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: {
+			int mode_value = p_value;
+			linear_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value;
+		} break;
+		case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: {
+			int mode_value = p_value;
+			angular_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value;
+		} break;
+		case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: {
+			linear_damp = p_value;
+		} break;
+		case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: {
+			angular_damp = p_value;
+		} break;
+		default: {
+		}
+	}
+}
+
+Variant GodotBody2D::get_param(PhysicsServer2D::BodyParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer2D::BODY_PARAM_BOUNCE: {
+			return bounce;
+		}
+		case PhysicsServer2D::BODY_PARAM_FRICTION: {
+			return friction;
+		}
+		case PhysicsServer2D::BODY_PARAM_MASS: {
+			return mass;
+		}
+		case PhysicsServer2D::BODY_PARAM_INERTIA: {
+			return inertia;
+		}
+		case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
+			return center_of_mass_local;
+		}
+		case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
+			return gravity_scale;
+		}
+		case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: {
+			return linear_damp_mode;
+		}
+		case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: {
+			return angular_damp_mode;
+		}
+		case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: {
+			return linear_damp;
+		}
+		case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: {
+			return angular_damp;
+		}
+		default: {
+		}
+	}
+
+	return 0;
+}
+
+void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) {
+	PhysicsServer2D::BodyMode prev = mode;
+	mode = p_mode;
+
+	switch (p_mode) {
+		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
+		case PhysicsServer2D::BODY_MODE_STATIC:
+		case PhysicsServer2D::BODY_MODE_KINEMATIC: {
+			_set_inv_transform(get_transform().affine_inverse());
+			_inv_mass = 0;
+			_inv_inertia = 0;
+			_set_static(p_mode == PhysicsServer2D::BODY_MODE_STATIC);
+			set_active(p_mode == PhysicsServer2D::BODY_MODE_KINEMATIC && contacts.size());
+			linear_velocity = Vector2();
+			angular_velocity = 0;
+			if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && prev != mode) {
+				first_time_kinematic = true;
+			}
+		} break;
+		case PhysicsServer2D::BODY_MODE_RIGID: {
+			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			if (!calculate_inertia) {
+				_inv_inertia = 1.0 / inertia;
+			}
+			_mass_properties_changed();
+			_set_static(false);
+			set_active(true);
+
+		} break;
+		case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: {
+			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			_inv_inertia = 0;
+			angular_velocity = 0;
+			_set_static(false);
+			set_active(true);
+		}
+	}
+}
+
+PhysicsServer2D::BodyMode GodotBody2D::get_mode() const {
+	return mode;
+}
+
+void GodotBody2D::_shapes_changed() {
+	_mass_properties_changed();
+	wakeup();
+	wakeup_neighbours();
+}
+
+void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) {
+	switch (p_state) {
+		case PhysicsServer2D::BODY_STATE_TRANSFORM: {
+			if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
+				new_transform = p_variant;
+				//wakeup_neighbours();
+				set_active(true);
+				if (first_time_kinematic) {
+					_set_transform(p_variant);
+					_set_inv_transform(get_transform().affine_inverse());
+					first_time_kinematic = false;
+				}
+			} else if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
+				_set_transform(p_variant);
+				_set_inv_transform(get_transform().affine_inverse());
+				wakeup_neighbours();
+			} else {
+				Transform2D t = p_variant;
+				t.orthonormalize();
+				new_transform = get_transform(); //used as old to compute motion
+				if (t == new_transform) {
+					break;
+				}
+				_set_transform(t);
+				_set_inv_transform(get_transform().inverse());
+				_update_transform_dependent();
+			}
+			wakeup();
+
+		} break;
+		case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: {
+			linear_velocity = p_variant;
+			constant_linear_velocity = linear_velocity;
+			wakeup();
+
+		} break;
+		case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: {
+			angular_velocity = p_variant;
+			constant_angular_velocity = angular_velocity;
+			wakeup();
+
+		} break;
+		case PhysicsServer2D::BODY_STATE_SLEEPING: {
+			if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
+				break;
+			}
+			bool do_sleep = p_variant;
+			if (do_sleep) {
+				linear_velocity = Vector2();
+				//biased_linear_velocity=Vector3();
+				angular_velocity = 0;
+				//biased_angular_velocity=Vector3();
+				set_active(false);
+			} else {
+				if (mode != PhysicsServer2D::BODY_MODE_STATIC) {
+					set_active(true);
+				}
+			}
+		} break;
+		case PhysicsServer2D::BODY_STATE_CAN_SLEEP: {
+			can_sleep = p_variant;
+			if (mode >= PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) {
+				set_active(true);
+			}
+
+		} break;
+	}
+}
+
+Variant GodotBody2D::get_state(PhysicsServer2D::BodyState p_state) const {
+	switch (p_state) {
+		case PhysicsServer2D::BODY_STATE_TRANSFORM: {
+			return get_transform();
+		}
+		case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: {
+			return linear_velocity;
+		}
+		case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: {
+			return angular_velocity;
+		}
+		case PhysicsServer2D::BODY_STATE_SLEEPING: {
+			return !is_active();
+		}
+		case PhysicsServer2D::BODY_STATE_CAN_SLEEP: {
+			return can_sleep;
+		}
+	}
+
+	return Variant();
+}
+
+void GodotBody2D::set_space(GodotSpace2D *p_space) {
+	if (get_space()) {
+		wakeup_neighbours();
+
+		if (mass_properties_update_list.in_list()) {
+			get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
+		}
+		if (active_list.in_list()) {
+			get_space()->body_remove_from_active_list(&active_list);
+		}
+		if (direct_state_query_list.in_list()) {
+			get_space()->body_remove_from_state_query_list(&direct_state_query_list);
+		}
+	}
+
+	_set_space(p_space);
+
+	if (get_space()) {
+		_mass_properties_changed();
+
+		if (active && !active_list.in_list()) {
+			get_space()->body_add_to_active_list(&active_list);
+		}
+	}
+}
+
+void GodotBody2D::_update_transform_dependent() {
+	center_of_mass = get_transform().basis_xform(center_of_mass_local);
+}
+
+void GodotBody2D::integrate_forces(real_t p_step) {
+	if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
+		return;
+	}
+
+	ERR_FAIL_NULL(get_space());
+
+	int ac = areas.size();
+
+	bool gravity_done = false;
+	bool linear_damp_done = false;
+	bool angular_damp_done = false;
+
+	bool stopped = false;
+
+	gravity = Vector2(0, 0);
+
+	total_linear_damp = 0.0;
+	total_angular_damp = 0.0;
+
+	// Combine gravity and damping from overlapping areas in priority order.
+	if (ac) {
+		areas.sort();
+		const AreaCMP *aa = &areas[0];
+		for (int i = ac - 1; i >= 0 && !stopped; i--) {
+			if (!gravity_done) {
+				PhysicsServer2D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE);
+				if (area_gravity_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+					Vector2 area_gravity;
+					aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity);
+					switch (area_gravity_mode) {
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							gravity += area_gravity;
+							gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							gravity = area_gravity;
+							gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
+				}
+			}
+			if (!linear_damp_done) {
+				PhysicsServer2D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE);
+				if (area_linear_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+					real_t area_linear_damp = aa[i].area->get_linear_damp();
+					switch (area_linear_damp_mode) {
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							total_linear_damp += area_linear_damp;
+							linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							total_linear_damp = area_linear_damp;
+							linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
+				}
+			}
+			if (!angular_damp_done) {
+				PhysicsServer2D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE);
+				if (area_angular_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
+					real_t area_angular_damp = aa[i].area->get_angular_damp();
+					switch (area_angular_damp_mode) {
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							total_angular_damp += area_angular_damp;
+							angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							total_angular_damp = area_angular_damp;
+							angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
+				}
+			}
+			stopped = gravity_done && linear_damp_done && angular_damp_done;
+		}
+	}
+
+	// Add default gravity and damping from space area.
+	if (!stopped) {
+		GodotArea2D *default_area = get_space()->get_default_area();
+		ERR_FAIL_NULL(default_area);
+
+		if (!gravity_done) {
+			Vector2 default_gravity;
+			default_area->compute_gravity(get_transform().get_origin(), default_gravity);
+			gravity += default_gravity;
+		}
+
+		if (!linear_damp_done) {
+			total_linear_damp += default_area->get_linear_damp();
+		}
+
+		if (!angular_damp_done) {
+			total_angular_damp += default_area->get_angular_damp();
+		}
+	}
+
+	// Override linear damping with body's value.
+	switch (linear_damp_mode) {
+		case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: {
+			total_linear_damp += linear_damp;
+		} break;
+		case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: {
+			total_linear_damp = linear_damp;
+		} break;
+	}
+
+	// Override angular damping with body's value.
+	switch (angular_damp_mode) {
+		case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: {
+			total_angular_damp += angular_damp;
+		} break;
+		case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: {
+			total_angular_damp = angular_damp;
+		} break;
+	}
+
+	gravity *= gravity_scale;
+
+	prev_linear_velocity = linear_velocity;
+	prev_angular_velocity = angular_velocity;
+
+	Vector2 motion;
+	bool do_motion = false;
+
+	if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
+		//compute motion, angular and etc. velocities from prev transform
+		motion = new_transform.get_origin() - get_transform().get_origin();
+		linear_velocity = constant_linear_velocity + motion / p_step;
+
+		real_t rot = new_transform.get_rotation() - get_transform().get_rotation();
+		angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step;
+
+		do_motion = true;
+
+	} else {
+		if (!omit_force_integration) {
+			//overridden by direct state query
+
+			Vector2 force = gravity * mass + applied_force + constant_force;
+			real_t torque = applied_torque + constant_torque;
+
+			real_t damp = 1.0 - p_step * total_linear_damp;
+
+			if (damp < 0) { // reached zero in the given time
+				damp = 0;
+			}
+
+			real_t angular_damp_new = 1.0 - p_step * total_angular_damp;
+
+			if (angular_damp_new < 0) { // reached zero in the given time
+				angular_damp_new = 0;
+			}
+
+			linear_velocity *= damp;
+			angular_velocity *= angular_damp_new;
+
+			linear_velocity += _inv_mass * force * p_step;
+			angular_velocity += _inv_inertia * torque * p_step;
+		}
+
+		if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
+			motion = linear_velocity * p_step;
+			do_motion = true;
+		}
+	}
+
+	applied_force = Vector2();
+	applied_torque = 0.0;
+
+	biased_angular_velocity = 0.0;
+	biased_linear_velocity = Vector2();
+
+	if (do_motion) { //shapes temporarily extend for raycast
+		_update_shapes_with_motion(motion);
+	}
+
+	contact_count = 0;
+}
+
+void GodotBody2D::integrate_velocities(real_t p_step) {
+	if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
+		return;
+	}
+
+	ERR_FAIL_NULL(get_space());
+
+	if (fi_callback_data || body_state_callback.is_valid()) {
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	}
+
+	if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
+		_set_transform(new_transform, false);
+		_set_inv_transform(new_transform.affine_inverse());
+		if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) {
+			set_active(false); //stopped moving, deactivate
+		}
+		return;
+	}
+
+	real_t total_angular_velocity = angular_velocity + biased_angular_velocity;
+	Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity;
+
+	real_t angle_delta = total_angular_velocity * p_step;
+	real_t angle = get_transform().get_rotation() + angle_delta;
+	Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
+
+	if (center_of_mass.length_squared() > CMP_EPSILON2) {
+		// Calculate displacement due to center of mass offset.
+		pos += center_of_mass - center_of_mass.rotated(angle_delta);
+	}
+
+	_set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED);
+	_set_inv_transform(get_transform().inverse());
+
+	if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
+		new_transform = get_transform();
+	}
+
+	_update_transform_dependent();
+}
+
+void GodotBody2D::wakeup_neighbours() {
+	for (const Pair<GodotConstraint2D *, int> &E : constraint_list) {
+		const GodotConstraint2D *c = E.first;
+		GodotBody2D **n = c->get_body_ptr();
+		int bc = c->get_body_count();
+
+		for (int i = 0; i < bc; i++) {
+			if (i == E.second) {
+				continue;
+			}
+			GodotBody2D *b = n[i];
+			if (b->mode < PhysicsServer2D::BODY_MODE_RIGID) {
+				continue;
+			}
+
+			if (!b->is_active()) {
+				b->set_active(true);
+			}
+		}
+	}
+}
+
+void GodotBody2D::call_queries() {
+	Variant direct_state_variant = get_direct_state();
+
+	if (fi_callback_data) {
+		if (!fi_callback_data->callable.is_valid()) {
+			set_force_integration_callback(Callable());
+		} else {
+			const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
+
+			Callable::CallError ce;
+			Variant rv;
+			if (fi_callback_data->udata.get_type() != Variant::NIL) {
+				fi_callback_data->callable.callp(vp, 2, rv, ce);
+
+			} else {
+				fi_callback_data->callable.callp(vp, 1, rv, ce);
+			}
+		}
+	}
+
+	if (body_state_callback.is_valid()) {
+		body_state_callback.call(direct_state_variant);
+	}
+}
+
+bool GodotBody2D::sleep_test(real_t p_step) {
+	if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
+		return true;
+	} else if (!can_sleep) {
+		return false;
+	}
+
+	ERR_FAIL_NULL_V(get_space(), true);
+
+	if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) {
+		still_time += p_step;
+
+		return still_time > get_space()->get_body_time_to_sleep();
+	} else {
+		still_time = 0; //maybe this should be set to 0 on set_active?
+		return false;
+	}
+}
+
+void GodotBody2D::set_state_sync_callback(const Callable &p_callable) {
+	body_state_callback = p_callable;
+}
+
+void GodotBody2D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
+	if (p_callable.is_valid()) {
+		if (!fi_callback_data) {
+			fi_callback_data = memnew(ForceIntegrationCallbackData);
+		}
+		fi_callback_data->callable = p_callable;
+		fi_callback_data->udata = p_udata;
+	} else if (fi_callback_data) {
+		memdelete(fi_callback_data);
+		fi_callback_data = nullptr;
+	}
+}
+
+GodotPhysicsDirectBodyState2D *GodotBody2D::get_direct_state() {
+	if (!direct_state) {
+		direct_state = memnew(GodotPhysicsDirectBodyState2D);
+		direct_state->body = this;
+	}
+	return direct_state;
+}
+
+GodotBody2D::GodotBody2D() :
+		GodotCollisionObject2D(TYPE_BODY),
+		active_list(this),
+		mass_properties_update_list(this),
+		direct_state_query_list(this) {
+	_set_static(false);
+}
+
+GodotBody2D::~GodotBody2D() {
+	if (fi_callback_data) {
+		memdelete(fi_callback_data);
+	}
+	if (direct_state) {
+		memdelete(direct_state);
+	}
+}
diff --git a/modules/godot_physics_2d/godot_body_2d.h b/modules/godot_physics_2d/godot_body_2d.h
new file mode 100644
index 0000000000..529305dbb2
--- /dev/null
+++ b/modules/godot_physics_2d/godot_body_2d.h
@@ -0,0 +1,389 @@
+/**************************************************************************/
+/*  godot_body_2d.h                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_BODY_2D_H
+#define GODOT_BODY_2D_H
+
+#include "godot_area_2d.h"
+#include "godot_collision_object_2d.h"
+
+#include "core/templates/list.h"
+#include "core/templates/pair.h"
+#include "core/templates/vset.h"
+
+class GodotConstraint2D;
+class GodotPhysicsDirectBodyState2D;
+
+class GodotBody2D : public GodotCollisionObject2D {
+	PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_RIGID;
+
+	Vector2 biased_linear_velocity;
+	real_t biased_angular_velocity = 0.0;
+
+	Vector2 linear_velocity;
+	real_t angular_velocity = 0.0;
+
+	Vector2 prev_linear_velocity;
+	real_t prev_angular_velocity = 0.0;
+
+	Vector2 constant_linear_velocity;
+	real_t constant_angular_velocity = 0.0;
+
+	PhysicsServer2D::BodyDampMode linear_damp_mode = PhysicsServer2D::BODY_DAMP_MODE_COMBINE;
+	PhysicsServer2D::BodyDampMode angular_damp_mode = PhysicsServer2D::BODY_DAMP_MODE_COMBINE;
+
+	real_t linear_damp = 0.0;
+	real_t angular_damp = 0.0;
+
+	real_t total_linear_damp = 0.0;
+	real_t total_angular_damp = 0.0;
+
+	real_t gravity_scale = 1.0;
+
+	real_t bounce = 0.0;
+	real_t friction = 1.0;
+
+	real_t mass = 1.0;
+	real_t _inv_mass = 1.0;
+
+	real_t inertia = 0.0;
+	real_t _inv_inertia = 0.0;
+
+	Vector2 center_of_mass_local;
+	Vector2 center_of_mass;
+
+	bool calculate_inertia = true;
+	bool calculate_center_of_mass = true;
+
+	Vector2 gravity;
+
+	real_t still_time = 0.0;
+
+	Vector2 applied_force;
+	real_t applied_torque = 0.0;
+
+	Vector2 constant_force;
+	real_t constant_torque = 0.0;
+
+	SelfList<GodotBody2D> active_list;
+	SelfList<GodotBody2D> mass_properties_update_list;
+	SelfList<GodotBody2D> direct_state_query_list;
+
+	VSet<RID> exceptions;
+	PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
+	bool omit_force_integration = false;
+	bool active = true;
+	bool can_sleep = true;
+	bool first_time_kinematic = false;
+	void _mass_properties_changed();
+	virtual void _shapes_changed() override;
+	Transform2D new_transform;
+
+	List<Pair<GodotConstraint2D *, int>> constraint_list;
+
+	struct AreaCMP {
+		GodotArea2D *area = nullptr;
+		int refCount = 0;
+		_FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
+		_FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
+		_FORCE_INLINE_ AreaCMP() {}
+		_FORCE_INLINE_ AreaCMP(GodotArea2D *p_area) {
+			area = p_area;
+			refCount = 1;
+		}
+	};
+
+	Vector<AreaCMP> areas;
+
+	struct Contact {
+		Vector2 local_pos;
+		Vector2 local_normal;
+		Vector2 local_velocity_at_pos;
+		real_t depth = 0.0;
+		int local_shape = 0;
+		Vector2 collider_pos;
+		int collider_shape = 0;
+		ObjectID collider_instance_id;
+		RID collider;
+		Vector2 collider_velocity_at_pos;
+		Vector2 impulse;
+	};
+
+	Vector<Contact> contacts; //no contacts by default
+	int contact_count = 0;
+
+	Callable body_state_callback;
+
+	struct ForceIntegrationCallbackData {
+		Callable callable;
+		Variant udata;
+	};
+
+	ForceIntegrationCallbackData *fi_callback_data = nullptr;
+
+	GodotPhysicsDirectBodyState2D *direct_state = nullptr;
+
+	uint64_t island_step = 0;
+
+	void _update_transform_dependent();
+
+	friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose
+
+public:
+	void set_state_sync_callback(const Callable &p_callable);
+	void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
+
+	GodotPhysicsDirectBodyState2D *get_direct_state();
+
+	_FORCE_INLINE_ void add_area(GodotArea2D *p_area) {
+		int index = areas.find(AreaCMP(p_area));
+		if (index > -1) {
+			areas.write[index].refCount += 1;
+		} else {
+			areas.ordered_insert(AreaCMP(p_area));
+		}
+	}
+
+	_FORCE_INLINE_ void remove_area(GodotArea2D *p_area) {
+		int index = areas.find(AreaCMP(p_area));
+		if (index > -1) {
+			areas.write[index].refCount -= 1;
+			if (areas[index].refCount < 1) {
+				areas.remove_at(index);
+			}
+		}
+	}
+
+	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) {
+		contacts.resize(p_size);
+		contact_count = 0;
+		if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) {
+			set_active(true);
+		}
+	}
+
+	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
+
+	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); }
+	_FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_local_velocity_at_pos, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &p_impulse);
+
+	_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
+	_FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
+	_FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
+	_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
+
+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
+
+	_FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); }
+	_FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); }
+	const List<Pair<GodotConstraint2D *, int>> &get_constraint_list() const { return constraint_list; }
+	_FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); }
+
+	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; }
+	_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
+
+	_FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; }
+	_FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
+
+	_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; }
+	_FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
+
+	_FORCE_INLINE_ Vector2 get_prev_linear_velocity() const { return prev_linear_velocity; }
+	_FORCE_INLINE_ real_t get_prev_angular_velocity() const { return prev_angular_velocity; }
+
+	_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; }
+	_FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
+
+	_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; }
+	_FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
+
+	_FORCE_INLINE_ void apply_central_impulse(const Vector2 &p_impulse) {
+		linear_velocity += p_impulse * _inv_mass;
+	}
+
+	_FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) {
+		linear_velocity += p_impulse * _inv_mass;
+		angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
+	}
+
+	_FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) {
+		angular_velocity += _inv_inertia * p_torque;
+	}
+
+	_FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2(), real_t p_max_delta_av = -1.0) {
+		biased_linear_velocity += p_impulse * _inv_mass;
+		if (p_max_delta_av != 0.0) {
+			real_t delta_av = _inv_inertia * (p_position - center_of_mass).cross(p_impulse);
+			if (p_max_delta_av > 0 && delta_av > p_max_delta_av) {
+				delta_av = p_max_delta_av;
+			}
+			biased_angular_velocity += delta_av;
+		}
+	}
+
+	_FORCE_INLINE_ void apply_central_force(const Vector2 &p_force) {
+		applied_force += p_force;
+	}
+
+	_FORCE_INLINE_ void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
+		applied_force += p_force;
+		applied_torque += (p_position - center_of_mass).cross(p_force);
+	}
+
+	_FORCE_INLINE_ void apply_torque(real_t p_torque) {
+		applied_torque += p_torque;
+	}
+
+	_FORCE_INLINE_ void add_constant_central_force(const Vector2 &p_force) {
+		constant_force += p_force;
+	}
+
+	_FORCE_INLINE_ void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) {
+		constant_force += p_force;
+		constant_torque += (p_position - center_of_mass).cross(p_force);
+	}
+
+	_FORCE_INLINE_ void add_constant_torque(real_t p_torque) {
+		constant_torque += p_torque;
+	}
+
+	void set_constant_force(const Vector2 &p_force) { constant_force = p_force; }
+	Vector2 get_constant_force() const { return constant_force; }
+
+	void set_constant_torque(real_t p_torque) { constant_torque = p_torque; }
+	real_t get_constant_torque() const { return constant_torque; }
+
+	void set_active(bool p_active);
+	_FORCE_INLINE_ bool is_active() const { return active; }
+
+	_FORCE_INLINE_ void wakeup() {
+		if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
+			return;
+		}
+		set_active(true);
+	}
+
+	void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value);
+	Variant get_param(PhysicsServer2D::BodyParameter p_param) const;
+
+	void set_mode(PhysicsServer2D::BodyMode p_mode);
+	PhysicsServer2D::BodyMode get_mode() const;
+
+	void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant);
+	Variant get_state(PhysicsServer2D::BodyState p_state) const;
+
+	_FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; }
+	_FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
+
+	void set_space(GodotSpace2D *p_space) override;
+
+	void update_mass_properties();
+	void reset_mass_properties();
+
+	_FORCE_INLINE_ const Vector2 &get_center_of_mass() const { return center_of_mass; }
+	_FORCE_INLINE_ const Vector2 &get_center_of_mass_local() const { return center_of_mass_local; }
+	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
+	_FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
+	_FORCE_INLINE_ real_t get_friction() const { return friction; }
+	_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
+
+	void integrate_forces(real_t p_step);
+	void integrate_velocities(real_t p_step);
+
+	_FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const {
+		return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x);
+	}
+
+	_FORCE_INLINE_ Vector2 get_motion() const {
+		if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+			return new_transform.get_origin() - get_transform().get_origin();
+		} else if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
+			return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward
+		}
+		return Vector2();
+	}
+
+	void call_queries();
+	void wakeup_neighbours();
+
+	bool sleep_test(real_t p_step);
+
+	GodotBody2D();
+	~GodotBody2D();
+};
+
+//add contact inline
+
+void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_local_velocity_at_pos, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &p_impulse) {
+	int c_max = contacts.size();
+
+	if (c_max == 0) {
+		return;
+	}
+
+	Contact *c = contacts.ptrw();
+
+	int idx = -1;
+
+	if (contact_count < c_max) {
+		idx = contact_count++;
+	} else {
+		real_t least_depth = 1e20;
+		int least_deep = -1;
+		for (int i = 0; i < c_max; i++) {
+			if (i == 0 || c[i].depth < least_depth) {
+				least_deep = i;
+				least_depth = c[i].depth;
+			}
+		}
+
+		if (least_deep >= 0 && least_depth < p_depth) {
+			idx = least_deep;
+		}
+		if (idx == -1) {
+			return; //none least deepe than this
+		}
+	}
+
+	c[idx].local_pos = p_local_pos;
+	c[idx].local_normal = p_local_normal;
+	c[idx].local_velocity_at_pos = p_local_velocity_at_pos;
+	c[idx].depth = p_depth;
+	c[idx].local_shape = p_local_shape;
+	c[idx].collider_pos = p_collider_pos;
+	c[idx].collider_shape = p_collider_shape;
+	c[idx].collider_instance_id = p_collider_instance_id;
+	c[idx].collider = p_collider;
+	c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
+	c[idx].impulse = p_impulse;
+}
+
+#endif // GODOT_BODY_2D_H
diff --git a/modules/godot_physics_2d/godot_body_direct_state_2d.cpp b/modules/godot_physics_2d/godot_body_direct_state_2d.cpp
new file mode 100644
index 0000000000..b34c70831d
--- /dev/null
+++ b/modules/godot_physics_2d/godot_body_direct_state_2d.cpp
@@ -0,0 +1,229 @@
+/**************************************************************************/
+/*  godot_body_direct_state_2d.cpp                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_body_direct_state_2d.h"
+
+#include "godot_body_2d.h"
+#include "godot_physics_server_2d.h"
+#include "godot_space_2d.h"
+
+Vector2 GodotPhysicsDirectBodyState2D::get_total_gravity() const {
+	return body->gravity;
+}
+
+real_t GodotPhysicsDirectBodyState2D::get_total_angular_damp() const {
+	return body->total_angular_damp;
+}
+
+real_t GodotPhysicsDirectBodyState2D::get_total_linear_damp() const {
+	return body->total_linear_damp;
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass() const {
+	return body->get_center_of_mass();
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass_local() const {
+	return body->get_center_of_mass_local();
+}
+
+real_t GodotPhysicsDirectBodyState2D::get_inverse_mass() const {
+	return body->get_inv_mass();
+}
+
+real_t GodotPhysicsDirectBodyState2D::get_inverse_inertia() const {
+	return body->get_inv_inertia();
+}
+
+void GodotPhysicsDirectBodyState2D::set_linear_velocity(const Vector2 &p_velocity) {
+	body->wakeup();
+	body->set_linear_velocity(p_velocity);
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_linear_velocity() const {
+	return body->get_linear_velocity();
+}
+
+void GodotPhysicsDirectBodyState2D::set_angular_velocity(real_t p_velocity) {
+	body->wakeup();
+	body->set_angular_velocity(p_velocity);
+}
+
+real_t GodotPhysicsDirectBodyState2D::get_angular_velocity() const {
+	return body->get_angular_velocity();
+}
+
+void GodotPhysicsDirectBodyState2D::set_transform(const Transform2D &p_transform) {
+	body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform);
+}
+
+Transform2D GodotPhysicsDirectBodyState2D::get_transform() const {
+	return body->get_transform();
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_velocity_at_local_position(const Vector2 &p_position) const {
+	return body->get_velocity_in_local_point(p_position);
+}
+
+void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) {
+	body->wakeup();
+	body->apply_central_impulse(p_impulse);
+}
+
+void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) {
+	body->wakeup();
+	body->apply_impulse(p_impulse, p_position);
+}
+
+void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) {
+	body->wakeup();
+	body->apply_torque_impulse(p_torque);
+}
+
+void GodotPhysicsDirectBodyState2D::apply_central_force(const Vector2 &p_force) {
+	body->wakeup();
+	body->apply_central_force(p_force);
+}
+
+void GodotPhysicsDirectBodyState2D::apply_force(const Vector2 &p_force, const Vector2 &p_position) {
+	body->wakeup();
+	body->apply_force(p_force, p_position);
+}
+
+void GodotPhysicsDirectBodyState2D::apply_torque(real_t p_torque) {
+	body->wakeup();
+	body->apply_torque(p_torque);
+}
+
+void GodotPhysicsDirectBodyState2D::add_constant_central_force(const Vector2 &p_force) {
+	body->wakeup();
+	body->add_constant_central_force(p_force);
+}
+
+void GodotPhysicsDirectBodyState2D::add_constant_force(const Vector2 &p_force, const Vector2 &p_position) {
+	body->wakeup();
+	body->add_constant_force(p_force, p_position);
+}
+
+void GodotPhysicsDirectBodyState2D::add_constant_torque(real_t p_torque) {
+	body->wakeup();
+	body->add_constant_torque(p_torque);
+}
+
+void GodotPhysicsDirectBodyState2D::set_constant_force(const Vector2 &p_force) {
+	if (!p_force.is_zero_approx()) {
+		body->wakeup();
+	}
+	body->set_constant_force(p_force);
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_constant_force() const {
+	return body->get_constant_force();
+}
+
+void GodotPhysicsDirectBodyState2D::set_constant_torque(real_t p_torque) {
+	if (!Math::is_zero_approx(p_torque)) {
+		body->wakeup();
+	}
+	body->set_constant_torque(p_torque);
+}
+
+real_t GodotPhysicsDirectBodyState2D::get_constant_torque() const {
+	return body->get_constant_torque();
+}
+
+void GodotPhysicsDirectBodyState2D::set_sleep_state(bool p_enable) {
+	body->set_active(!p_enable);
+}
+
+bool GodotPhysicsDirectBodyState2D::is_sleeping() const {
+	return !body->is_active();
+}
+
+int GodotPhysicsDirectBodyState2D::get_contact_count() const {
+	return body->contact_count;
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].local_pos;
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_normal(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].local_normal;
+}
+
+int GodotPhysicsDirectBodyState2D::get_contact_local_shape(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
+	return body->contacts[p_contact_idx].local_shape;
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].local_velocity_at_pos;
+}
+
+RID GodotPhysicsDirectBodyState2D::get_contact_collider(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
+	return body->contacts[p_contact_idx].collider;
+}
+Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].collider_pos;
+}
+
+ObjectID GodotPhysicsDirectBodyState2D::get_contact_collider_id(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
+	return body->contacts[p_contact_idx].collider_instance_id;
+}
+
+int GodotPhysicsDirectBodyState2D::get_contact_collider_shape(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
+	return body->contacts[p_contact_idx].collider_shape;
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].collider_velocity_at_pos;
+}
+
+Vector2 GodotPhysicsDirectBodyState2D::get_contact_impulse(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].impulse;
+}
+
+PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() {
+	return body->get_space()->get_direct_state();
+}
+
+real_t GodotPhysicsDirectBodyState2D::get_step() const {
+	return body->get_space()->get_last_step();
+}
diff --git a/modules/godot_physics_2d/godot_body_direct_state_2d.h b/modules/godot_physics_2d/godot_body_direct_state_2d.h
new file mode 100644
index 0000000000..90b7c1d369
--- /dev/null
+++ b/modules/godot_physics_2d/godot_body_direct_state_2d.h
@@ -0,0 +1,104 @@
+/**************************************************************************/
+/*  godot_body_direct_state_2d.h                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_BODY_DIRECT_STATE_2D_H
+#define GODOT_BODY_DIRECT_STATE_2D_H
+
+#include "servers/physics_server_2d.h"
+
+class GodotBody2D;
+
+class GodotPhysicsDirectBodyState2D : public PhysicsDirectBodyState2D {
+	GDCLASS(GodotPhysicsDirectBodyState2D, PhysicsDirectBodyState2D);
+
+public:
+	GodotBody2D *body = nullptr;
+
+	virtual Vector2 get_total_gravity() const override;
+	virtual real_t get_total_angular_damp() const override;
+	virtual real_t get_total_linear_damp() const override;
+
+	virtual Vector2 get_center_of_mass() const override;
+	virtual Vector2 get_center_of_mass_local() const override;
+	virtual real_t get_inverse_mass() const override;
+	virtual real_t get_inverse_inertia() const override;
+
+	virtual void set_linear_velocity(const Vector2 &p_velocity) override;
+	virtual Vector2 get_linear_velocity() const override;
+
+	virtual void set_angular_velocity(real_t p_velocity) override;
+	virtual real_t get_angular_velocity() const override;
+
+	virtual void set_transform(const Transform2D &p_transform) override;
+	virtual Transform2D get_transform() const override;
+
+	virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override;
+
+	virtual void apply_central_impulse(const Vector2 &p_impulse) override;
+	virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override;
+	virtual void apply_torque_impulse(real_t p_torque) override;
+
+	virtual void apply_central_force(const Vector2 &p_force) override;
+	virtual void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void apply_torque(real_t p_torque) override;
+
+	virtual void add_constant_central_force(const Vector2 &p_force) override;
+	virtual void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void add_constant_torque(real_t p_torque) override;
+
+	virtual void set_constant_force(const Vector2 &p_force) override;
+	virtual Vector2 get_constant_force() const override;
+
+	virtual void set_constant_torque(real_t p_torque) override;
+	virtual real_t get_constant_torque() const override;
+
+	virtual void set_sleep_state(bool p_enable) override;
+	virtual bool is_sleeping() const override;
+
+	virtual int get_contact_count() const override;
+
+	virtual Vector2 get_contact_local_position(int p_contact_idx) const override;
+	virtual Vector2 get_contact_local_normal(int p_contact_idx) const override;
+	virtual int get_contact_local_shape(int p_contact_idx) const override;
+	virtual Vector2 get_contact_local_velocity_at_position(int p_contact_idx) const override;
+
+	virtual RID get_contact_collider(int p_contact_idx) const override;
+	virtual Vector2 get_contact_collider_position(int p_contact_idx) const override;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
+	virtual int get_contact_collider_shape(int p_contact_idx) const override;
+	virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
+	virtual Vector2 get_contact_impulse(int p_contact_idx) const override;
+
+	virtual PhysicsDirectSpaceState2D *get_space_state() override;
+
+	virtual real_t get_step() const override;
+};
+
+#endif // GODOT_BODY_DIRECT_STATE_2D_H
diff --git a/modules/godot_physics_2d/godot_body_pair_2d.cpp b/modules/godot_physics_2d/godot_body_pair_2d.cpp
new file mode 100644
index 0000000000..6c2d28dc92
--- /dev/null
+++ b/modules/godot_physics_2d/godot_body_pair_2d.cpp
@@ -0,0 +1,608 @@
+/**************************************************************************/
+/*  godot_body_pair_2d.cpp                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_body_pair_2d.h"
+
+#include "godot_collision_solver_2d.h"
+#include "godot_space_2d.h"
+
+#define ACCUMULATE_IMPULSES
+
+#define MIN_VELOCITY 0.001
+#define MAX_BIAS_ROTATION (Math_PI / 8)
+
+void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
+	GodotBodyPair2D *self = static_cast<GodotBodyPair2D *>(p_self);
+
+	self->_contact_added_callback(p_point_A, p_point_B);
+}
+
+void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) {
+	Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
+	Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B);
+
+	int new_index = contact_count;
+
+	ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
+
+	Contact contact;
+	contact.local_A = local_A;
+	contact.local_B = local_B;
+	contact.normal = (p_point_A - p_point_B).normalized();
+	contact.used = true;
+
+	// Attempt to determine if the contact will be reused.
+	real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius();
+
+	for (int i = 0; i < contact_count; i++) {
+		Contact &c = contacts[i];
+		if (c.local_A.distance_squared_to(local_A) < (recycle_radius_2) &&
+				c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) {
+			contact.acc_normal_impulse = c.acc_normal_impulse;
+			contact.acc_tangent_impulse = c.acc_tangent_impulse;
+			contact.acc_bias_impulse = c.acc_bias_impulse;
+			contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
+			c = contact;
+			return;
+		}
+	}
+
+	// Figure out if the contact amount must be reduced to fit the new contact.
+	if (new_index == MAX_CONTACTS) {
+		// Remove the contact with the minimum depth.
+
+		const Transform2D &transform_A = A->get_transform();
+		const Transform2D &transform_B = B->get_transform();
+
+		int least_deep = -1;
+		real_t min_depth;
+
+		// Start with depth for new contact.
+		{
+			Vector2 global_A = transform_A.basis_xform(contact.local_A);
+			Vector2 global_B = transform_B.basis_xform(contact.local_B) + offset_B;
+
+			Vector2 axis = global_A - global_B;
+			min_depth = axis.dot(contact.normal);
+		}
+
+		for (int i = 0; i < contact_count; i++) {
+			const Contact &c = contacts[i];
+			Vector2 global_A = transform_A.basis_xform(c.local_A);
+			Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
+
+			Vector2 axis = global_A - global_B;
+			real_t depth = axis.dot(c.normal);
+
+			if (depth < min_depth) {
+				min_depth = depth;
+				least_deep = i;
+			}
+		}
+
+		if (least_deep > -1) {
+			// Replace the least deep contact by the new one.
+			contacts[least_deep] = contact;
+		}
+
+		return;
+	}
+
+	contacts[new_index] = contact;
+	contact_count++;
+}
+
+void GodotBodyPair2D::_validate_contacts() {
+	// Make sure to erase contacts that are no longer valid.
+	real_t max_separation = space->get_contact_max_separation();
+	real_t max_separation2 = max_separation * max_separation;
+
+	const Transform2D &transform_A = A->get_transform();
+	const Transform2D &transform_B = B->get_transform();
+
+	for (int i = 0; i < contact_count; i++) {
+		Contact &c = contacts[i];
+
+		bool erase = false;
+		if (!c.used) {
+			// Was left behind in previous frame.
+			erase = true;
+		} else {
+			c.used = false;
+
+			Vector2 global_A = transform_A.basis_xform(c.local_A);
+			Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
+			Vector2 axis = global_A - global_B;
+			real_t depth = axis.dot(c.normal);
+
+			if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
+				erase = true;
+			}
+		}
+
+		if (erase) {
+			// Contact no longer needed, remove.
+
+			if ((i + 1) < contact_count) {
+				// Swap with the last one.
+				SWAP(contacts[i], contacts[contact_count - 1]);
+			}
+
+			i--;
+			contact_count--;
+		}
+	}
+}
+
+// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
+// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
+// Process: only proceed if body A's motion is high relative to its size.
+// cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does.
+// adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it.
+bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) {
+	Vector2 motion = p_A->get_linear_velocity() * p_step;
+	real_t mlen = motion.length();
+	if (mlen < CMP_EPSILON) {
+		return false;
+	}
+
+	Vector2 mnormal = motion / mlen;
+
+	real_t min = 0.0, max = 0.0;
+	p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max);
+
+	// Did it move enough in this direction to even attempt raycast?
+	// Let's say it should move more than 1/3 the size of the object in that axis.
+	bool fast_object = mlen > (max - min) * 0.3;
+	if (!fast_object) {
+		return false;
+	}
+
+	// A is moving fast enough that tunneling might occur. See if it's really about to collide.
+
+	// Roughly predict body B's position in the next frame (ignoring collisions).
+	Transform2D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step);
+
+	// Cast a segment from support in motion normal, in the same direction of motion by motion length.
+	// Support point will the farthest forward collision point along the movement vector.
+	// i.e. the point that should hit B first if any collision does occur.
+
+	// convert mnormal into body A's local xform because get_support requires (and returns) local coordinates.
+	int a;
+	Vector2 s[2];
+	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform_inv(mnormal).normalized(), s, a);
+	Vector2 from = p_xform_A.xform(s[0]);
+	// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
+	// This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close.
+	Vector2 to = from + motion;
+
+	Transform2D from_inv = predicted_xform_B.affine_inverse();
+
+	// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
+	// At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd. But it still works out.
+	Vector2 local_from = from_inv.xform(from - motion * 0.1);
+	Vector2 local_to = from_inv.xform(to);
+
+	Vector2 rpos, rnorm;
+	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
+		// there was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
+		// actually collide yet on next frame. We'll probably check again next frame once they're closer.
+		return false;
+	}
+
+	// Check one-way collision based on motion direction.
+	if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) {
+		Vector2 direction = predicted_xform_B.columns[1].normalized();
+		if (direction.dot(mnormal) < CMP_EPSILON) {
+			collided = false;
+			oneway_disabled = true;
+			return false;
+		}
+	}
+
+	// Shorten the linear velocity so it does not hit, but gets close enough,
+	// next frame will hit softly or soft enough.
+	Vector2 hitpos = predicted_xform_B.xform(rpos);
+
+	real_t newlen = hitpos.distance_to(from) + (max - min) * 0.01; // adding 1% of body length to the distance between collision and support point should cause body A's support point to arrive just within B's collider next frame.
+	p_A->set_linear_velocity(mnormal * (newlen / p_step));
+
+	return true;
+}
+
+real_t combine_bounce(GodotBody2D *A, GodotBody2D *B) {
+	return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1);
+}
+
+real_t combine_friction(GodotBody2D *A, GodotBody2D *B) {
+	return ABS(MIN(A->get_friction(), B->get_friction()));
+}
+
+bool GodotBodyPair2D::setup(real_t p_step) {
+	check_ccd = false;
+
+	if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
+		collided = false;
+		return false;
+	}
+
+	collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B);
+	collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A);
+
+	report_contacts_only = false;
+	if (!collide_A && !collide_B) {
+		if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
+			report_contacts_only = true;
+		} else {
+			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();
+
+	_validate_contacts();
+
+	const Vector2 &offset_A = A->get_transform().get_origin();
+	Transform2D xform_Au = A->get_transform().untranslated();
+	Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
+
+	Transform2D xform_Bu = B->get_transform();
+	xform_Bu.columns[2] -= offset_A;
+	Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
+
+	GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
+	GodotShape2D *shape_B_ptr = B->get_shape(shape_B);
+
+	Vector2 motion_A, motion_B;
+
+	if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) {
+		motion_A = A->get_motion();
+	}
+	if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) {
+		motion_B = B->get_motion();
+	}
+
+	bool prev_collided = collided;
+
+	collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
+	if (!collided) {
+		oneway_disabled = false;
+
+		if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
+			check_ccd = true;
+			return true;
+		}
+
+		if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
+			check_ccd = true;
+			return true;
+		}
+
+		return false;
+	}
+
+	if (oneway_disabled) {
+		return false;
+	}
+
+	if (!prev_collided) {
+		if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) {
+			Vector2 direction = xform_A.columns[1].normalized();
+			bool valid = false;
+			for (int i = 0; i < contact_count; i++) {
+				Contact &c = contacts[i];
+				if (c.normal.dot(direction) > -CMP_EPSILON) { // Greater (normal inverted).
+					continue;
+				}
+				valid = true;
+				break;
+			}
+			if (!valid) {
+				collided = false;
+				oneway_disabled = true;
+				return false;
+			}
+		}
+
+		if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) {
+			Vector2 direction = xform_B.columns[1].normalized();
+			bool valid = false;
+			for (int i = 0; i < contact_count; i++) {
+				Contact &c = contacts[i];
+				if (c.normal.dot(direction) < CMP_EPSILON) { // Less (normal ok).
+					continue;
+				}
+				valid = true;
+				break;
+			}
+			if (!valid) {
+				collided = false;
+				oneway_disabled = true;
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+
+bool GodotBodyPair2D::pre_solve(real_t p_step) {
+	if (oneway_disabled) {
+		return false;
+	}
+
+	if (!collided) {
+		if (check_ccd) {
+			const Vector2 &offset_A = A->get_transform().get_origin();
+			Transform2D xform_Au = A->get_transform().untranslated();
+			Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
+
+			Transform2D xform_Bu = B->get_transform();
+			xform_Bu.columns[2] -= offset_A;
+			Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
+
+			if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
+				_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
+			}
+
+			if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
+				_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
+			}
+		}
+
+		return false;
+	}
+
+	real_t max_penetration = space->get_contact_max_allowed_penetration();
+
+	real_t bias = space->get_contact_bias();
+
+	GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
+	GodotShape2D *shape_B_ptr = B->get_shape(shape_B);
+
+	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
+		if (shape_A_ptr->get_custom_bias() == 0) {
+			bias = shape_B_ptr->get_custom_bias();
+		} else if (shape_B_ptr->get_custom_bias() == 0) {
+			bias = shape_A_ptr->get_custom_bias();
+		} else {
+			bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
+		}
+	}
+
+	real_t inv_dt = 1.0 / p_step;
+
+	bool do_process = false;
+
+	const Vector2 &offset_A = A->get_transform().get_origin();
+	const Transform2D &transform_A = A->get_transform();
+	const Transform2D &transform_B = B->get_transform();
+
+	real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0;
+	real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0;
+
+	real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
+	real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
+
+	for (int i = 0; i < contact_count; i++) {
+		Contact &c = contacts[i];
+		c.active = false;
+
+		Vector2 global_A = transform_A.basis_xform(c.local_A);
+		Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
+
+		Vector2 axis = global_A - global_B;
+		real_t depth = axis.dot(c.normal);
+
+		if (depth <= 0.0) {
+			continue;
+		}
+
+#ifdef DEBUG_ENABLED
+		if (space->is_debugging_contacts()) {
+			space->add_debug_contact(global_A + offset_A);
+			space->add_debug_contact(global_B + offset_A);
+		}
+#endif
+
+		c.rA = global_A - A->get_center_of_mass();
+		c.rB = global_B - B->get_center_of_mass() - offset_B;
+
+		// Precompute normal mass, tangent mass, and bias.
+		real_t rnA = c.rA.dot(c.normal);
+		real_t rnB = c.rB.dot(c.normal);
+		real_t kNormal = inv_mass_A + inv_mass_B;
+		kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB);
+		c.mass_normal = 1.0f / kNormal;
+
+		Vector2 tangent = c.normal.orthogonal();
+		real_t rtA = c.rA.dot(tangent);
+		real_t rtB = c.rB.dot(tangent);
+		real_t kTangent = inv_mass_A + inv_mass_B;
+		kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB);
+		c.mass_tangent = 1.0f / kTangent;
+
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+		c.depth = depth;
+
+		Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
+
+		c.acc_impulse -= P;
+
+		if (A->can_report_contacts() || B->can_report_contacts()) {
+			Vector2 crB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity();
+			Vector2 crA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity();
+			if (A->can_report_contacts()) {
+				A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse);
+			}
+			if (B->can_report_contacts()) {
+				B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, c.acc_impulse);
+			}
+		}
+
+		if (report_contacts_only) {
+			collided = false;
+			continue;
+		}
+
+#ifdef ACCUMULATE_IMPULSES
+		{
+			// Apply normal + friction impulse
+			if (collide_A) {
+				A->apply_impulse(-P, c.rA + A->get_center_of_mass());
+			}
+			if (collide_B) {
+				B->apply_impulse(P, c.rB + B->get_center_of_mass());
+			}
+		}
+#endif
+
+		c.bounce = combine_bounce(A, B);
+		if (c.bounce) {
+			Vector2 crA(-A->get_prev_angular_velocity() * c.rA.y, A->get_prev_angular_velocity() * c.rA.x);
+			Vector2 crB(-B->get_prev_angular_velocity() * c.rB.y, B->get_prev_angular_velocity() * c.rB.x);
+			Vector2 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA;
+			c.bounce = c.bounce * dv.dot(c.normal);
+		}
+
+		c.active = true;
+		do_process = true;
+	}
+
+	return do_process;
+}
+
+void GodotBodyPair2D::solve(real_t p_step) {
+	if (!collided || oneway_disabled) {
+		return;
+	}
+
+	const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
+
+	real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
+	real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
+
+	for (int i = 0; i < contact_count; ++i) {
+		Contact &c = contacts[i];
+
+		if (!c.active) {
+			continue;
+		}
+
+		// Relative velocity at contact
+
+		Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+		Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
+		Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
+
+		Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
+		Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
+		Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
+
+		real_t vn = dv.dot(c.normal);
+		real_t vbn = dbv.dot(c.normal);
+
+		Vector2 tangent = c.normal.orthogonal();
+		real_t vt = dv.dot(tangent);
+
+		real_t jbn = (c.bias - vbn) * c.mass_normal;
+		real_t jbnOld = c.acc_bias_impulse;
+		c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
+
+		Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
+
+		if (collide_A) {
+			A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av);
+		}
+		if (collide_B) {
+			B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av);
+		}
+
+		crbA = Vector2(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
+		crbB = Vector2(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
+		dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
+
+		vbn = dbv.dot(c.normal);
+
+		if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
+			real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B);
+			real_t jbnOld_com = c.acc_bias_impulse_center_of_mass;
+			c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f);
+
+			Vector2 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
+
+			if (collide_A) {
+				A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
+			}
+			if (collide_B) {
+				B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
+			}
+		}
+
+		real_t jn = -(c.bounce + vn) * c.mass_normal;
+		real_t jnOld = c.acc_normal_impulse;
+		c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
+
+		real_t friction = combine_friction(A, B);
+
+		real_t jtMax = friction * c.acc_normal_impulse;
+		real_t jt = -vt * c.mass_tangent;
+		real_t jtOld = c.acc_tangent_impulse;
+		c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax);
+
+		Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
+
+		if (collide_A) {
+			A->apply_impulse(-j, c.rA + A->get_center_of_mass());
+		}
+		if (collide_B) {
+			B->apply_impulse(j, c.rB + B->get_center_of_mass());
+		}
+		c.acc_impulse -= j;
+	}
+}
+
+GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) :
+		GodotConstraint2D(_arr, 2) {
+	A = p_A;
+	B = p_B;
+	shape_A = p_shape_A;
+	shape_B = p_shape_B;
+	space = A->get_space();
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+GodotBodyPair2D::~GodotBodyPair2D() {
+	A->remove_constraint(this, 0);
+	B->remove_constraint(this, 1);
+}
diff --git a/modules/godot_physics_2d/godot_body_pair_2d.h b/modules/godot_physics_2d/godot_body_pair_2d.h
new file mode 100644
index 0000000000..4e9bfa6022
--- /dev/null
+++ b/modules/godot_physics_2d/godot_body_pair_2d.h
@@ -0,0 +1,101 @@
+/**************************************************************************/
+/*  godot_body_pair_2d.h                                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_BODY_PAIR_2D_H
+#define GODOT_BODY_PAIR_2D_H
+
+#include "godot_body_2d.h"
+#include "godot_constraint_2d.h"
+
+class GodotBodyPair2D : public GodotConstraint2D {
+	enum {
+		MAX_CONTACTS = 2
+	};
+	union {
+		struct {
+			GodotBody2D *A;
+			GodotBody2D *B;
+		};
+
+		GodotBody2D *_arr[2] = { nullptr, nullptr };
+	};
+
+	int shape_A = 0;
+	int shape_B = 0;
+
+	bool collide_A = false;
+	bool collide_B = false;
+
+	GodotSpace2D *space = nullptr;
+
+	struct Contact {
+		Vector2 position;
+		Vector2 normal;
+		Vector2 local_A, local_B;
+		Vector2 acc_impulse; // accumulated impulse
+		real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
+		real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt)
+		real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb)
+		real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com
+		real_t mass_normal, mass_tangent = 0.0;
+		real_t bias = 0.0;
+
+		real_t depth = 0.0;
+		bool active = false;
+		bool used = false;
+		Vector2 rA, rB;
+		real_t bounce = 0.0;
+	};
+
+	Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection
+
+	Vector2 sep_axis;
+	Contact contacts[MAX_CONTACTS];
+	int contact_count = 0;
+	bool collided = false;
+	bool check_ccd = false;
+	bool oneway_disabled = false;
+	bool report_contacts_only = false;
+
+	bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B);
+	void _validate_contacts();
+	static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self);
+	_FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
+
+public:
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B);
+	~GodotBodyPair2D();
+};
+
+#endif // GODOT_BODY_PAIR_2D_H
diff --git a/modules/godot_physics_2d/godot_broad_phase_2d.cpp b/modules/godot_physics_2d/godot_broad_phase_2d.cpp
new file mode 100644
index 0000000000..eb6bc21d60
--- /dev/null
+++ b/modules/godot_physics_2d/godot_broad_phase_2d.cpp
@@ -0,0 +1,36 @@
+/**************************************************************************/
+/*  godot_broad_phase_2d.cpp                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_broad_phase_2d.h"
+
+GodotBroadPhase2D::CreateFunction GodotBroadPhase2D::create_func = nullptr;
+
+GodotBroadPhase2D::~GodotBroadPhase2D() {
+}
diff --git a/modules/godot_physics_2d/godot_broad_phase_2d.h b/modules/godot_physics_2d/godot_broad_phase_2d.h
new file mode 100644
index 0000000000..f3c07a69bb
--- /dev/null
+++ b/modules/godot_physics_2d/godot_broad_phase_2d.h
@@ -0,0 +1,71 @@
+/**************************************************************************/
+/*  godot_broad_phase_2d.h                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_BROAD_PHASE_2D_H
+#define GODOT_BROAD_PHASE_2D_H
+
+#include "core/math/math_funcs.h"
+#include "core/math/rect2.h"
+
+class GodotCollisionObject2D;
+
+class GodotBroadPhase2D {
+public:
+	typedef GodotBroadPhase2D *(*CreateFunction)();
+
+	static CreateFunction create_func;
+
+	typedef uint32_t ID;
+
+	typedef void *(*PairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_userdata);
+	typedef void (*UnpairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_userdata);
+
+	// 0 is an invalid ID
+	virtual ID create(GodotCollisionObject2D *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0;
+	virtual void move(ID p_id, const Rect2 &p_aabb) = 0;
+	virtual void set_static(ID p_id, bool p_static) = 0;
+	virtual void remove(ID p_id) = 0;
+
+	virtual GodotCollisionObject2D *get_object(ID p_id) const = 0;
+	virtual bool is_static(ID p_id) const = 0;
+	virtual int get_subindex(ID p_id) const = 0;
+
+	virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
+	virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
+
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0;
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0;
+
+	virtual void update() = 0;
+
+	virtual ~GodotBroadPhase2D();
+};
+
+#endif // GODOT_BROAD_PHASE_2D_H
diff --git a/modules/godot_physics_2d/godot_broad_phase_2d_bvh.cpp b/modules/godot_physics_2d/godot_broad_phase_2d_bvh.cpp
new file mode 100644
index 0000000000..59623a2667
--- /dev/null
+++ b/modules/godot_physics_2d/godot_broad_phase_2d_bvh.cpp
@@ -0,0 +1,123 @@
+/**************************************************************************/
+/*  godot_broad_phase_2d_bvh.cpp                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_broad_phase_2d_bvh.h"
+#include "godot_collision_object_2d.h"
+
+GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) {
+	uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
+	uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
+	ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care?
+	return oid + 1;
+}
+
+void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
+	ERR_FAIL_COND(!p_id);
+	bvh.move(p_id - 1, p_aabb);
+}
+
+void GodotBroadPhase2DBVH::set_static(ID p_id, bool p_static) {
+	ERR_FAIL_COND(!p_id);
+	uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
+	uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
+	bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false);
+}
+
+void GodotBroadPhase2DBVH::remove(ID p_id) {
+	ERR_FAIL_COND(!p_id);
+	bvh.erase(p_id - 1);
+}
+
+GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, nullptr);
+	GodotCollisionObject2D *it = bvh.get(p_id - 1);
+	ERR_FAIL_NULL_V(it, nullptr);
+	return it;
+}
+
+bool GodotBroadPhase2DBVH::is_static(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, false);
+	uint32_t tree_id = bvh.get_tree_id(p_id - 1);
+	return tree_id == 0;
+}
+
+int GodotBroadPhase2DBVH::get_subindex(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, 0);
+	return bvh.get_subindex(p_id - 1);
+}
+
+int GodotBroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) {
+	return bvh.cull_segment(p_from, p_to, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
+}
+
+int GodotBroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) {
+	return bvh.cull_aabb(p_aabb, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
+}
+
+void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B) {
+	GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self);
+	if (!bpo->pair_callback) {
+		return nullptr;
+	}
+
+	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
+}
+
+void GodotBroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B, void *pairdata) {
+	GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self);
+	if (!bpo->unpair_callback) {
+		return;
+	}
+
+	bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
+}
+
+void GodotBroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
+	pair_callback = p_pair_callback;
+	pair_userdata = p_userdata;
+}
+
+void GodotBroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
+	unpair_callback = p_unpair_callback;
+	unpair_userdata = p_userdata;
+}
+
+void GodotBroadPhase2DBVH::update() {
+	bvh.update();
+}
+
+GodotBroadPhase2D *GodotBroadPhase2DBVH::_create() {
+	return memnew(GodotBroadPhase2DBVH);
+}
+
+GodotBroadPhase2DBVH::GodotBroadPhase2DBVH() {
+	bvh.set_pair_callback(_pair_callback, this);
+	bvh.set_unpair_callback(_unpair_callback, this);
+}
diff --git a/modules/godot_physics_2d/godot_broad_phase_2d_bvh.h b/modules/godot_physics_2d/godot_broad_phase_2d_bvh.h
new file mode 100644
index 0000000000..6c1fae5cb2
--- /dev/null
+++ b/modules/godot_physics_2d/godot_broad_phase_2d_bvh.h
@@ -0,0 +1,101 @@
+/**************************************************************************/
+/*  godot_broad_phase_2d_bvh.h                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_BROAD_PHASE_2D_BVH_H
+#define GODOT_BROAD_PHASE_2D_BVH_H
+
+#include "godot_broad_phase_2d.h"
+
+#include "core/math/bvh.h"
+#include "core/math/rect2.h"
+#include "core/math/vector2.h"
+
+class GodotBroadPhase2DBVH : public GodotBroadPhase2D {
+	template <typename T>
+	class UserPairTestFunction {
+	public:
+		static bool user_pair_check(const T *p_a, const T *p_b) {
+			// return false if no collision, decided by masks etc
+			return p_a->interacts_with(p_b);
+		}
+	};
+
+	template <typename T>
+	class UserCullTestFunction {
+	public:
+		static bool user_cull_check(const T *p_a, const T *p_b) {
+			return true;
+		}
+	};
+
+	enum Tree {
+		TREE_STATIC = 0,
+		TREE_DYNAMIC = 1,
+	};
+
+	enum TreeFlag {
+		TREE_FLAG_STATIC = 1 << TREE_STATIC,
+		TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC,
+	};
+
+	BVH_Manager<GodotCollisionObject2D, 2, true, 128, UserPairTestFunction<GodotCollisionObject2D>, UserCullTestFunction<GodotCollisionObject2D>, Rect2, Vector2> bvh;
+
+	static void *_pair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int);
+	static void _unpair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int, void *);
+
+	PairCallback pair_callback = nullptr;
+	void *pair_userdata = nullptr;
+	UnpairCallback unpair_callback = nullptr;
+	void *unpair_userdata = nullptr;
+
+public:
+	// 0 is an invalid ID
+	virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) override;
+	virtual void move(ID p_id, const Rect2 &p_aabb) override;
+	virtual void set_static(ID p_id, bool p_static) override;
+	virtual void remove(ID p_id) override;
+
+	virtual GodotCollisionObject2D *get_object(ID p_id) const override;
+	virtual bool is_static(ID p_id) const override;
+	virtual int get_subindex(ID p_id) const override;
+
+	virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override;
+	virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override;
+
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) override;
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) override;
+
+	virtual void update() override;
+
+	static GodotBroadPhase2D *_create();
+	GodotBroadPhase2DBVH();
+};
+
+#endif // GODOT_BROAD_PHASE_2D_BVH_H
diff --git a/modules/godot_physics_2d/godot_collision_object_2d.cpp b/modules/godot_physics_2d/godot_collision_object_2d.cpp
new file mode 100644
index 0000000000..9851cac140
--- /dev/null
+++ b/modules/godot_physics_2d/godot_collision_object_2d.cpp
@@ -0,0 +1,244 @@
+/**************************************************************************/
+/*  godot_collision_object_2d.cpp                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_collision_object_2d.h"
+#include "godot_physics_server_2d.h"
+#include "godot_space_2d.h"
+
+void GodotCollisionObject2D::add_shape(GodotShape2D *p_shape, const Transform2D &p_transform, bool p_disabled) {
+	Shape s;
+	s.shape = p_shape;
+	s.xform = p_transform;
+	s.xform_inv = s.xform.affine_inverse();
+	s.bpid = 0; //needs update
+	s.disabled = p_disabled;
+	s.one_way_collision = false;
+	s.one_way_collision_margin = 0;
+	shapes.push_back(s);
+	p_shape->add_owner(this);
+
+	if (!pending_shape_update_list.in_list()) {
+		GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+}
+
+void GodotCollisionObject2D::set_shape(int p_index, GodotShape2D *p_shape) {
+	ERR_FAIL_INDEX(p_index, shapes.size());
+	shapes[p_index].shape->remove_owner(this);
+	shapes.write[p_index].shape = p_shape;
+
+	p_shape->add_owner(this);
+
+	if (!pending_shape_update_list.in_list()) {
+		GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+}
+
+void GodotCollisionObject2D::set_shape_transform(int p_index, const Transform2D &p_transform) {
+	ERR_FAIL_INDEX(p_index, shapes.size());
+
+	shapes.write[p_index].xform = p_transform;
+	shapes.write[p_index].xform_inv = p_transform.affine_inverse();
+
+	if (!pending_shape_update_list.in_list()) {
+		GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+}
+
+void GodotCollisionObject2D::set_shape_disabled(int p_idx, bool p_disabled) {
+	ERR_FAIL_INDEX(p_idx, shapes.size());
+
+	GodotCollisionObject2D::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()) {
+			GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+		}
+	} else if (!p_disabled && shape.bpid == 0) {
+		if (!pending_shape_update_list.in_list()) {
+			GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+		}
+	}
+}
+
+void GodotCollisionObject2D::remove_shape(GodotShape2D *p_shape) {
+	//remove a shape, all the times it appears
+	for (int i = 0; i < shapes.size(); i++) {
+		if (shapes[i].shape == p_shape) {
+			remove_shape(i);
+			i--;
+		}
+	}
+}
+
+void GodotCollisionObject2D::remove_shape(int p_index) {
+	//remove anything from shape to be erased to end, so subindices don't change
+	ERR_FAIL_INDEX(p_index, shapes.size());
+	for (int i = p_index; i < shapes.size(); i++) {
+		if (shapes[i].bpid == 0) {
+			continue;
+		}
+		//should never get here with a null owner
+		space->get_broadphase()->remove(shapes[i].bpid);
+		shapes.write[i].bpid = 0;
+	}
+	shapes[p_index].shape->remove_owner(this);
+	shapes.remove_at(p_index);
+
+	if (!pending_shape_update_list.in_list()) {
+		GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+	// _update_shapes();
+	// _shapes_changed();
+}
+
+void GodotCollisionObject2D::_set_static(bool p_static) {
+	if (_static == p_static) {
+		return;
+	}
+	_static = p_static;
+
+	if (!space) {
+		return;
+	}
+	for (int i = 0; i < get_shape_count(); i++) {
+		const Shape &s = shapes[i];
+		if (s.bpid > 0) {
+			space->get_broadphase()->set_static(s.bpid, _static);
+		}
+	}
+}
+
+void GodotCollisionObject2D::_unregister_shapes() {
+	for (int i = 0; i < shapes.size(); i++) {
+		Shape &s = shapes.write[i];
+		if (s.bpid > 0) {
+			space->get_broadphase()->remove(s.bpid);
+			s.bpid = 0;
+		}
+	}
+}
+
+void GodotCollisionObject2D::_update_shapes() {
+	if (!space) {
+		return;
+	}
+
+	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..
+		Rect2 shape_aabb = s.shape->get_aabb();
+		Transform2D xform = transform * s.xform;
+		shape_aabb = xform.xform(shape_aabb);
+		shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
+		s.aabb_cache = shape_aabb;
+
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
+			space->get_broadphase()->set_static(s.bpid, _static);
+		}
+
+		space->get_broadphase()->move(s.bpid, shape_aabb);
+	}
+}
+
+void GodotCollisionObject2D::_update_shapes_with_motion(const Vector2 &p_motion) {
+	if (!space) {
+		return;
+	}
+
+	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..
+		Rect2 shape_aabb = s.shape->get_aabb();
+		Transform2D xform = transform * s.xform;
+		shape_aabb = xform.xform(shape_aabb);
+		shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
+		s.aabb_cache = shape_aabb;
+
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
+			space->get_broadphase()->set_static(s.bpid, _static);
+		}
+
+		space->get_broadphase()->move(s.bpid, shape_aabb);
+	}
+}
+
+void GodotCollisionObject2D::_set_space(GodotSpace2D *p_space) {
+	GodotSpace2D *old_space = space;
+	space = p_space;
+
+	if (old_space) {
+		old_space->remove_object(this);
+
+		for (int i = 0; i < shapes.size(); i++) {
+			Shape &s = shapes.write[i];
+			if (s.bpid) {
+				old_space->get_broadphase()->remove(s.bpid);
+				s.bpid = 0;
+			}
+		}
+	}
+
+	if (space) {
+		space->add_object(this);
+		_update_shapes();
+	}
+}
+
+void GodotCollisionObject2D::_shape_changed() {
+	_update_shapes();
+	_shapes_changed();
+}
+
+GodotCollisionObject2D::GodotCollisionObject2D(Type p_type) :
+		pending_shape_update_list(this) {
+	type = p_type;
+}
diff --git a/modules/godot_physics_2d/godot_collision_object_2d.h b/modules/godot_physics_2d/godot_collision_object_2d.h
new file mode 100644
index 0000000000..129fa27ff3
--- /dev/null
+++ b/modules/godot_physics_2d/godot_collision_object_2d.h
@@ -0,0 +1,198 @@
+/**************************************************************************/
+/*  godot_collision_object_2d.h                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_COLLISION_OBJECT_2D_H
+#define GODOT_COLLISION_OBJECT_2D_H
+
+#include "godot_broad_phase_2d.h"
+#include "godot_shape_2d.h"
+
+#include "core/templates/self_list.h"
+#include "servers/physics_server_2d.h"
+
+class GodotSpace2D;
+
+class GodotCollisionObject2D : public GodotShapeOwner2D {
+public:
+	enum Type {
+		TYPE_AREA,
+		TYPE_BODY
+	};
+
+private:
+	Type type;
+	RID self;
+	ObjectID instance_id;
+	ObjectID canvas_instance_id;
+	bool pickable = true;
+
+	struct Shape {
+		Transform2D xform;
+		Transform2D xform_inv;
+		GodotBroadPhase2D::ID bpid = 0;
+		Rect2 aabb_cache; //for rayqueries
+		GodotShape2D *shape = nullptr;
+		bool disabled = false;
+		bool one_way_collision = false;
+		real_t one_way_collision_margin = 0.0;
+	};
+
+	Vector<Shape> shapes;
+	GodotSpace2D *space = nullptr;
+	Transform2D transform;
+	Transform2D inv_transform;
+	uint32_t collision_mask = 1;
+	uint32_t collision_layer = 1;
+	real_t collision_priority = 1.0;
+	bool _static = true;
+
+	SelfList<GodotCollisionObject2D> pending_shape_update_list;
+
+	void _update_shapes();
+
+protected:
+	void _update_shapes_with_motion(const Vector2 &p_motion);
+	void _unregister_shapes();
+
+	_FORCE_INLINE_ void _set_transform(const Transform2D &p_transform, bool p_update_shapes = true) {
+		transform = p_transform;
+		if (p_update_shapes) {
+			_update_shapes();
+		}
+	}
+	_FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; }
+	void _set_static(bool p_static);
+
+	virtual void _shapes_changed() = 0;
+	void _set_space(GodotSpace2D *p_space);
+
+	GodotCollisionObject2D(Type p_type);
+
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; }
+	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
+
+	_FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; }
+	_FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; }
+
+	void _shape_changed() override;
+
+	_FORCE_INLINE_ Type get_type() const { return type; }
+	void add_shape(GodotShape2D *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false);
+	void set_shape(int p_index, GodotShape2D *p_shape);
+	void set_shape_transform(int p_index, const Transform2D &p_transform);
+
+	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
+	_FORCE_INLINE_ GodotShape2D *get_shape(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].shape;
+	}
+	_FORCE_INLINE_ const Transform2D &get_shape_transform(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].xform;
+	}
+	_FORCE_INLINE_ const Transform2D &get_shape_inv_transform(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].xform_inv;
+	}
+	_FORCE_INLINE_ const Rect2 &get_shape_aabb(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].aabb_cache;
+	}
+
+	_FORCE_INLINE_ const Transform2D &get_transform() const { return transform; }
+	_FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; }
+	_FORCE_INLINE_ GodotSpace2D *get_space() const { return space; }
+
+	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;
+	}
+
+	_FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) {
+		CRASH_BAD_INDEX(p_idx, shapes.size());
+		shapes.write[p_idx].one_way_collision = p_one_way_collision;
+		shapes.write[p_idx].one_way_collision_margin = p_margin;
+	}
+	_FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const {
+		CRASH_BAD_INDEX(p_idx, shapes.size());
+		return shapes[p_idx].one_way_collision;
+	}
+
+	_FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const {
+		CRASH_BAD_INDEX(p_idx, shapes.size());
+		return shapes[p_idx].one_way_collision_margin;
+	}
+
+	void set_collision_mask(uint32_t p_mask) {
+		collision_mask = p_mask;
+		_shape_changed();
+	}
+	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
+
+	void set_collision_layer(uint32_t p_layer) {
+		collision_layer = p_layer;
+		_shape_changed();
+	}
+	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
+
+	_FORCE_INLINE_ void set_collision_priority(real_t p_priority) {
+		ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0.");
+		collision_priority = p_priority;
+		_shape_changed();
+	}
+	_FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; }
+
+	void remove_shape(GodotShape2D *p_shape) override;
+	void remove_shape(int p_index);
+
+	virtual void set_space(GodotSpace2D *p_space) = 0;
+
+	_FORCE_INLINE_ bool is_static() const { return _static; }
+
+	void set_pickable(bool p_pickable) { pickable = p_pickable; }
+	_FORCE_INLINE_ bool is_pickable() const { return pickable; }
+
+	_FORCE_INLINE_ bool collides_with(GodotCollisionObject2D *p_other) const {
+		return p_other->collision_layer & collision_mask;
+	}
+
+	_FORCE_INLINE_ bool interacts_with(const GodotCollisionObject2D *p_other) const {
+		return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
+	}
+
+	virtual ~GodotCollisionObject2D() {}
+};
+
+#endif // GODOT_COLLISION_OBJECT_2D_H
diff --git a/modules/godot_physics_2d/godot_collision_solver_2d.cpp b/modules/godot_physics_2d/godot_collision_solver_2d.cpp
new file mode 100644
index 0000000000..a1acbe9cf0
--- /dev/null
+++ b/modules/godot_physics_2d/godot_collision_solver_2d.cpp
@@ -0,0 +1,274 @@
+/**************************************************************************/
+/*  godot_collision_solver_2d.cpp                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_collision_solver_2d.h"
+#include "godot_collision_solver_2d_sat.h"
+
+#define collision_solver sat_2d_calculate_penetration
+//#define collision_solver gjk_epa_calculate_penetration
+
+bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
+	const GodotWorldBoundaryShape2D *world_boundary = static_cast<const GodotWorldBoundaryShape2D *>(p_shape_A);
+	if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) {
+		return false;
+	}
+
+	Vector2 n = p_transform_A.basis_xform(world_boundary->get_normal()).normalized();
+	Vector2 p = p_transform_A.xform(world_boundary->get_normal() * world_boundary->get_d());
+	real_t d = n.dot(p);
+
+	Vector2 supports[2];
+	int support_count;
+
+	p_shape_B->get_supports(p_transform_B.affine_inverse().basis_xform(-n).normalized(), supports, support_count);
+
+	bool found = false;
+
+	for (int i = 0; i < support_count; i++) {
+		supports[i] += p_margin * supports[i].normalized();
+		supports[i] = p_transform_B.xform(supports[i]);
+		supports[i] += p_motion_B;
+		real_t pd = n.dot(supports[i]);
+		if (pd >= d) {
+			continue;
+		}
+		found = true;
+
+		Vector2 support_A = supports[i] - n * (pd - d);
+
+		if (p_result_callback) {
+			if (p_swap_result) {
+				p_result_callback(supports[i], support_A, p_userdata);
+			} else {
+				p_result_callback(support_A, supports[i], p_userdata);
+			}
+		}
+	}
+
+	return found;
+}
+
+bool GodotCollisionSolver2D::solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin) {
+	const GodotSeparationRayShape2D *ray = static_cast<const GodotSeparationRayShape2D *>(p_shape_A);
+	if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) {
+		return false;
+	}
+
+	Vector2 from = p_transform_A.get_origin();
+	Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin);
+	if (p_motion_A != Vector2()) {
+		//not the best but should be enough
+		Vector2 normal = (to - from).normalized();
+		to += normal * MAX(0.0, normal.dot(p_motion_A));
+	}
+	Vector2 support_A = to;
+
+	Transform2D invb = p_transform_B.affine_inverse();
+	from = invb.xform(from);
+	to = invb.xform(to);
+
+	Vector2 p, n;
+	if (!p_shape_B->intersect_segment(from, to, p, n)) {
+		if (r_sep_axis) {
+			*r_sep_axis = p_transform_A[1].normalized();
+		}
+		return false;
+	}
+
+	// Discard contacts when the ray is fully contained inside the shape.
+	if (n == Vector2()) {
+		if (r_sep_axis) {
+			*r_sep_axis = p_transform_A[1].normalized();
+		}
+		return false;
+	}
+
+	// Discard contacts in the wrong direction.
+	if (n.dot(from - to) < CMP_EPSILON) {
+		if (r_sep_axis) {
+			*r_sep_axis = p_transform_A[1].normalized();
+		}
+		return false;
+	}
+
+	Vector2 support_B = p_transform_B.xform(p);
+	if (ray->get_slide_on_slope()) {
+		Vector2 global_n = invb.basis_xform_inv(n).normalized();
+		support_B = support_A + (support_B - support_A).length() * global_n;
+	}
+
+	if (p_result_callback) {
+		if (p_swap_result) {
+			p_result_callback(support_B, support_A, p_userdata);
+		} else {
+			p_result_callback(support_A, support_B, p_userdata);
+		}
+	}
+	return true;
+}
+
+struct _ConcaveCollisionInfo2D {
+	const Transform2D *transform_A = nullptr;
+	const GodotShape2D *shape_A = nullptr;
+	const Transform2D *transform_B = nullptr;
+	Vector2 motion_A;
+	Vector2 motion_B;
+	real_t margin_A = 0.0;
+	real_t margin_B = 0.0;
+	GodotCollisionSolver2D::CallbackResult result_callback = nullptr;
+	void *userdata = nullptr;
+	bool swap_result = false;
+	bool collided = false;
+	int aabb_tests = 0;
+	int collisions = 0;
+	Vector2 *sep_axis = nullptr;
+};
+
+bool GodotCollisionSolver2D::concave_callback(void *p_userdata, GodotShape2D *p_convex) {
+	_ConcaveCollisionInfo2D &cinfo = *(static_cast<_ConcaveCollisionInfo2D *>(p_userdata));
+	cinfo.aabb_tests++;
+
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B);
+	if (!collided) {
+		return false;
+	}
+
+	cinfo.collided = true;
+	cinfo.collisions++;
+
+	// Stop at first collision if contacts are not needed.
+	return !cinfo.result_callback;
+}
+
+bool GodotCollisionSolver2D::solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
+	const GodotConcaveShape2D *concave_B = static_cast<const GodotConcaveShape2D *>(p_shape_B);
+
+	_ConcaveCollisionInfo2D cinfo;
+	cinfo.transform_A = &p_transform_A;
+	cinfo.shape_A = p_shape_A;
+	cinfo.transform_B = &p_transform_B;
+	cinfo.motion_A = p_motion_A;
+	cinfo.result_callback = p_result_callback;
+	cinfo.userdata = p_userdata;
+	cinfo.swap_result = p_swap_result;
+	cinfo.collided = false;
+	cinfo.collisions = 0;
+	cinfo.sep_axis = r_sep_axis;
+	cinfo.margin_A = p_margin_A;
+	cinfo.margin_B = p_margin_B;
+
+	cinfo.aabb_tests = 0;
+
+	Transform2D rel_transform = p_transform_A;
+	rel_transform.columns[2] -= p_transform_B.get_origin();
+
+	// Quickly compute a local Rect2.
+	Rect2 local_aabb;
+	for (int i = 0; i < 2; i++) {
+		Vector2 axis(p_transform_B.columns[i]);
+		real_t axis_scale = 1.0 / axis.length();
+		axis *= axis_scale;
+
+		real_t smin = 0.0, smax = 0.0;
+		p_shape_A->project_rangev(axis, rel_transform, smin, smax);
+		smin *= axis_scale;
+		smax *= axis_scale;
+
+		local_aabb.position[i] = smin;
+		local_aabb.size[i] = smax - smin;
+	}
+	// In case of motion, expand the Rect2 in the motion direction.
+	if (p_motion_A != Vector2()) {
+		Rect2 moved_aabb = local_aabb;
+		moved_aabb.position += p_motion_A;
+		local_aabb = local_aabb.merge(moved_aabb);
+	}
+
+	concave_B->cull(local_aabb, concave_callback, &cinfo);
+
+	return cinfo.collided;
+}
+
+bool GodotCollisionSolver2D::solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
+	PhysicsServer2D::ShapeType type_A = p_shape_A->get_type();
+	PhysicsServer2D::ShapeType type_B = p_shape_B->get_type();
+	bool concave_A = p_shape_A->is_concave();
+	bool concave_B = p_shape_B->is_concave();
+	real_t margin_A = p_margin_A, margin_B = p_margin_B;
+
+	bool swap = false;
+
+	if (type_A > type_B) {
+		SWAP(type_A, type_B);
+		SWAP(concave_A, concave_B);
+		SWAP(margin_A, margin_B);
+		swap = true;
+	}
+
+	if (type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) {
+		if (type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) {
+			WARN_PRINT_ONCE("Collisions between world boundaries are not supported.");
+			return false;
+		}
+
+		if (swap) {
+			return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, p_margin_A);
+		} else {
+			return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, p_margin_B);
+		}
+
+	} else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) {
+		if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) {
+			WARN_PRINT_ONCE("Collisions between two rays are not supported.");
+			return false; //no ray-ray
+		}
+
+		if (swap) {
+			return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B);
+		} else {
+			return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A);
+		}
+
+	} else if (concave_B) {
+		if (concave_A) {
+			WARN_PRINT_ONCE("Collisions between two concave shapes are not supported.");
+			return false;
+		}
+
+		if (!swap) {
+			return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B);
+		} else {
+			return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B);
+		}
+
+	} else {
+		return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B);
+	}
+}
diff --git a/modules/godot_physics_2d/godot_collision_solver_2d.h b/modules/godot_physics_2d/godot_collision_solver_2d.h
new file mode 100644
index 0000000000..1c09714f76
--- /dev/null
+++ b/modules/godot_physics_2d/godot_collision_solver_2d.h
@@ -0,0 +1,50 @@
+/**************************************************************************/
+/*  godot_collision_solver_2d.h                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_COLLISION_SOLVER_2D_H
+#define GODOT_COLLISION_SOLVER_2D_H
+
+#include "godot_shape_2d.h"
+
+class GodotCollisionSolver2D {
+public:
+	typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata);
+
+private:
+	static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
+	static bool concave_callback(void *p_userdata, GodotShape2D *p_convex);
+	static bool solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
+	static bool solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0);
+
+public:
+	static bool solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
+};
+
+#endif // GODOT_COLLISION_SOLVER_2D_H
diff --git a/modules/godot_physics_2d/godot_collision_solver_2d_sat.cpp b/modules/godot_physics_2d/godot_collision_solver_2d_sat.cpp
new file mode 100644
index 0000000000..daa9982b2e
--- /dev/null
+++ b/modules/godot_physics_2d/godot_collision_solver_2d_sat.cpp
@@ -0,0 +1,1404 @@
+/**************************************************************************/
+/*  godot_collision_solver_2d_sat.cpp                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_collision_solver_2d_sat.h"
+
+#include "core/math/geometry_2d.h"
+
+struct _CollectorCallback2D {
+	GodotCollisionSolver2D::CallbackResult callback = nullptr;
+	void *userdata = nullptr;
+	bool swap = false;
+	bool collided = false;
+	Vector2 normal;
+	Vector2 *sep_axis = nullptr;
+
+	_FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) {
+		if (swap) {
+			callback(p_point_B, p_point_A, userdata);
+		} else {
+			callback(p_point_A, p_point_B, userdata);
+		}
+	}
+};
+
+typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *);
+
+_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND(p_point_count_A != 1);
+	ERR_FAIL_COND(p_point_count_B != 1);
+#endif
+
+	p_collector->call(*p_points_A, *p_points_B);
+}
+
+_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND(p_point_count_A != 1);
+	ERR_FAIL_COND(p_point_count_B != 2);
+#endif
+
+	Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B);
+	p_collector->call(*p_points_A, closest_B);
+}
+
+struct _generate_contacts_Pair {
+	bool a = false;
+	int idx = 0;
+	real_t d = 0.0;
+	_FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; }
+};
+
+_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND(p_point_count_A != 2);
+	ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix
+#endif
+
+	Vector2 n = p_collector->normal;
+	Vector2 t = n.orthogonal();
+	real_t dA = n.dot(p_points_A[0]);
+	real_t dB = n.dot(p_points_B[0]);
+
+	_generate_contacts_Pair dvec[4];
+
+	dvec[0].d = t.dot(p_points_A[0]);
+	dvec[0].a = true;
+	dvec[0].idx = 0;
+	dvec[1].d = t.dot(p_points_A[1]);
+	dvec[1].a = true;
+	dvec[1].idx = 1;
+	dvec[2].d = t.dot(p_points_B[0]);
+	dvec[2].a = false;
+	dvec[2].idx = 0;
+	dvec[3].d = t.dot(p_points_B[1]);
+	dvec[3].a = false;
+	dvec[3].idx = 1;
+
+	SortArray<_generate_contacts_Pair> sa;
+	sa.sort(dvec, 4);
+
+	for (int i = 1; i <= 2; i++) {
+		if (dvec[i].a) {
+			Vector2 a = p_points_A[dvec[i].idx];
+			Vector2 b = n.plane_project(dB, a);
+			if (n.dot(a) > n.dot(b) - CMP_EPSILON) {
+				continue;
+			}
+			p_collector->call(a, b);
+		} else {
+			Vector2 b = p_points_B[dvec[i].idx];
+			Vector2 a = n.plane_project(dA, b);
+			if (n.dot(a) > n.dot(b) - CMP_EPSILON) {
+				continue;
+			}
+			p_collector->call(a, b);
+		}
+	}
+}
+
+static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND(p_point_count_A < 1);
+	ERR_FAIL_COND(p_point_count_B < 1);
+#endif
+
+	static const GenerateContactsFunc generate_contacts_func_table[2][2] = {
+		{
+				_generate_contacts_point_point,
+				_generate_contacts_point_edge,
+		},
+		{
+				nullptr,
+				_generate_contacts_edge_edge,
+		}
+	};
+
+	int pointcount_B = 0;
+	int pointcount_A = 0;
+	const Vector2 *points_A = nullptr;
+	const Vector2 *points_B = nullptr;
+
+	if (p_point_count_A > p_point_count_B) {
+		//swap
+		p_collector->swap = !p_collector->swap;
+		p_collector->normal = -p_collector->normal;
+
+		pointcount_B = p_point_count_A;
+		pointcount_A = p_point_count_B;
+		points_A = p_points_B;
+		points_B = p_points_A;
+	} else {
+		pointcount_B = p_point_count_B;
+		pointcount_A = p_point_count_A;
+		points_A = p_points_A;
+		points_B = p_points_B;
+	}
+
+	int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1;
+	int version_B = (pointcount_B > 2 ? 2 : pointcount_B) - 1;
+
+	GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
+	ERR_FAIL_NULL(contacts_func);
+	contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector);
+}
+
+template <typename ShapeA, typename ShapeB, bool castA = false, bool castB = false, bool withMargin = false>
+class SeparatorAxisTest2D {
+	const ShapeA *shape_A = nullptr;
+	const ShapeB *shape_B = nullptr;
+	const Transform2D *transform_A = nullptr;
+	const Transform2D *transform_B = nullptr;
+	real_t best_depth = 1e15;
+	Vector2 best_axis;
+#ifdef DEBUG_ENABLED
+	int best_axis_count = 0;
+	int best_axis_index = -1;
+#endif
+	Vector2 motion_A;
+	Vector2 motion_B;
+	real_t margin_A = 0.0;
+	real_t margin_B = 0.0;
+	_CollectorCallback2D *callback;
+
+public:
+	_FORCE_INLINE_ bool test_previous_axis() {
+		if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) {
+			return test_axis(*callback->sep_axis);
+		} else {
+#ifdef DEBUG_ENABLED
+			best_axis_count++;
+#endif
+		}
+		return true;
+	}
+
+	_FORCE_INLINE_ bool test_cast() {
+		if (castA) {
+			Vector2 na = motion_A.normalized();
+			if (!test_axis(na)) {
+				return false;
+			}
+			if (!test_axis(na.orthogonal())) {
+				return false;
+			}
+		}
+
+		if (castB) {
+			Vector2 nb = motion_B.normalized();
+			if (!test_axis(nb)) {
+				return false;
+			}
+			if (!test_axis(nb.orthogonal())) {
+				return false;
+			}
+		}
+
+		return true;
+	}
+
+	_FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) {
+		Vector2 axis = p_axis;
+
+		if (Math::is_zero_approx(axis.x) &&
+				Math::is_zero_approx(axis.y)) {
+			// strange case, try an upwards separator
+			axis = Vector2(0.0, 1.0);
+		}
+
+		real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0;
+
+		if (castA) {
+			shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A);
+		} else {
+			shape_A->project_range(axis, *transform_A, min_A, max_A);
+		}
+
+		if (castB) {
+			shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B);
+		} else {
+			shape_B->project_range(axis, *transform_B, min_B, max_B);
+		}
+
+		if (withMargin) {
+			min_A -= margin_A;
+			max_A += margin_A;
+			min_B -= margin_B;
+			max_B += margin_B;
+		}
+
+		min_B -= (max_A - min_A) * 0.5;
+		max_B += (max_A - min_A) * 0.5;
+
+		real_t dmin = min_B - (min_A + max_A) * 0.5;
+		real_t dmax = max_B - (min_A + max_A) * 0.5;
+
+		if (dmin > 0.0 || dmax < 0.0) {
+			if (callback && callback->sep_axis) {
+				*callback->sep_axis = axis;
+			}
+#ifdef DEBUG_ENABLED
+			best_axis_count++;
+#endif
+
+			return false; // doesn't contain 0
+		}
+
+		//use the smallest depth
+
+		dmin = Math::abs(dmin);
+
+		if (dmax < dmin) {
+			if (dmax < best_depth) {
+				best_depth = dmax;
+				best_axis = axis;
+#ifdef DEBUG_ENABLED
+				best_axis_index = best_axis_count;
+#endif
+			}
+		} else {
+			if (dmin < best_depth) {
+				best_depth = dmin;
+				best_axis = -axis; // keep it as A axis
+#ifdef DEBUG_ENABLED
+				best_axis_index = best_axis_count;
+#endif
+			}
+		}
+
+#ifdef DEBUG_ENABLED
+		best_axis_count++;
+#endif
+
+		return true;
+	}
+
+	_FORCE_INLINE_ void generate_contacts() {
+		// nothing to do, don't generate
+		if (best_axis == Vector2(0.0, 0.0)) {
+			return;
+		}
+
+		if (callback) {
+			callback->collided = true;
+
+			if (!callback->callback) {
+				return; //only collide, no callback
+			}
+		}
+		static const int max_supports = 2;
+
+		Vector2 supports_A[max_supports];
+		int support_count_A;
+		if (castA) {
+			shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A);
+		} else {
+			shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A);
+			for (int i = 0; i < support_count_A; i++) {
+				supports_A[i] = transform_A->xform(supports_A[i]);
+			}
+		}
+
+		if (withMargin) {
+			for (int i = 0; i < support_count_A; i++) {
+				supports_A[i] += -best_axis * margin_A;
+			}
+		}
+
+		Vector2 supports_B[max_supports];
+		int support_count_B;
+		if (castB) {
+			shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B);
+		} else {
+			shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B);
+			for (int i = 0; i < support_count_B; i++) {
+				supports_B[i] = transform_B->xform(supports_B[i]);
+			}
+		}
+
+		if (withMargin) {
+			for (int i = 0; i < support_count_B; i++) {
+				supports_B[i] += best_axis * margin_B;
+			}
+		}
+		if (callback) {
+			callback->normal = best_axis;
+			_generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
+
+			if (callback->sep_axis && *callback->sep_axis != Vector2()) {
+				*callback->sep_axis = Vector2(); //invalidate previous axis (no test)
+			}
+		}
+	}
+
+	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) {
+		margin_A = p_margin_A;
+		margin_B = p_margin_B;
+		shape_A = p_shape_A;
+		shape_B = p_shape_B;
+		transform_A = &p_transform_a;
+		transform_B = &p_transform_b;
+		motion_A = p_motion_A;
+		motion_B = p_motion_B;
+		callback = p_collector;
+	}
+};
+
+/****** SAT TESTS *******/
+
+#define TEST_POINT(m_a, m_b)                                                                \
+	((!separator.test_axis(((m_a) - (m_b)).normalized())) ||                                \
+			(castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) ||   \
+			(castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \
+			(castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized())))
+
+typedef void (*CollisionFunc)(const GodotShape2D *, const Transform2D &, const GodotShape2D *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t);
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_segment(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
+	const GodotSegmentShape2D *segment_B = static_cast<const GodotSegmentShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotSegmentShape2D, GodotSegmentShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+	//this collision is kind of pointless
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
+		return;
+	}
+	if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) {
+		return;
+	}
+
+	if (withMargin) {
+		//points grow to circles
+
+		if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) {
+			return;
+		}
+		if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) {
+			return;
+		}
+		if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) {
+			return;
+		}
+		if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) {
+			return;
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
+	const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotSegmentShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	//segment normal
+	if (!separator.test_axis(
+				(p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) {
+		return;
+	}
+
+	//endpoint a vs circle
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) {
+		return;
+	}
+	//endpoint b vs circle
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) {
+		return;
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
+	const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotSegmentShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
+		return;
+	}
+
+	if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis(p_transform_b.columns[1].normalized())) {
+		return;
+	}
+
+	if (withMargin) {
+		Transform2D inv = p_transform_b.affine_inverse();
+
+		Vector2 a = p_transform_a.xform(segment_A->get_a());
+		Vector2 b = p_transform_a.xform(segment_A->get_b());
+
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) {
+			return;
+		}
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) {
+			return;
+		}
+
+		if constexpr (castA) {
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) {
+				return;
+			}
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) {
+				return;
+			}
+		}
+
+		if constexpr (castB) {
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) {
+				return;
+			}
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) {
+				return;
+			}
+		}
+
+		if constexpr (castA && castB) {
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) {
+				return;
+			}
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) {
+				return;
+			}
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
+	const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotSegmentShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
+		return;
+	}
+
+	if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
+		return;
+	}
+
+	real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
+
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
+		return;
+	}
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
+		return;
+	}
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
+		return;
+	}
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
+		return;
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
+	const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotSegmentShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
+		return;
+	}
+
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
+			return;
+		}
+
+		if (withMargin) {
+			if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) {
+				return;
+			}
+			if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) {
+				return;
+			}
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+/////////
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_circle_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
+	const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotCircleShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) {
+		return;
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
+	const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotCircleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	const Vector2 &sphere = p_transform_a.columns[2];
+	const Vector2 *axis = &p_transform_b.columns[0];
+	//const Vector2& half_extents = rectangle_B->get_half_extents();
+
+	if (!separator.test_axis(axis[0].normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis(axis[1].normalized())) {
+		return;
+	}
+
+	Transform2D binv = p_transform_b.affine_inverse();
+	{
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) {
+			return;
+		}
+	}
+
+	if constexpr (castA) {
+		Vector2 sphereofs = sphere + p_motion_a;
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
+			return;
+		}
+	}
+
+	if constexpr (castB) {
+		Vector2 sphereofs = sphere - p_motion_b;
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
+			return;
+		}
+	}
+
+	if constexpr (castA && castB) {
+		Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
+			return;
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
+	const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotCircleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	//capsule axis
+	if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
+		return;
+	}
+
+	real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
+
+	//capsule endpoints
+	if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
+		return;
+	}
+	if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
+		return;
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_circle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
+	const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotCircleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	//poly faces and poly points vs circle
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
+		if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) {
+			return;
+		}
+
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
+			return;
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+/////////
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
+	const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotRectangleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	//box faces A
+	if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
+		return;
+	}
+
+	//box faces B
+	if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis(p_transform_b.columns[1].normalized())) {
+		return;
+	}
+
+	if constexpr (withMargin) {
+		Transform2D invA = p_transform_a.affine_inverse();
+		Transform2D invB = p_transform_b.affine_inverse();
+
+		if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) {
+			return;
+		}
+
+		if constexpr (castA || castB) {
+			Transform2D aofs = p_transform_a;
+			aofs.columns[2] += p_motion_a;
+
+			Transform2D bofs = p_transform_b;
+			bofs.columns[2] += p_motion_b;
+
+			[[maybe_unused]] Transform2D aofsinv = aofs.affine_inverse();
+			[[maybe_unused]] Transform2D bofsinv = bofs.affine_inverse();
+
+			if constexpr (castA) {
+				if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) {
+					return;
+				}
+			}
+
+			if constexpr (castB) {
+				if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) {
+					return;
+				}
+			}
+
+			if constexpr (castA && castB) {
+				if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) {
+					return;
+				}
+			}
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
+	const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotRectangleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	//box faces
+	if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
+		return;
+	}
+
+	//capsule axis
+	if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
+		return;
+	}
+
+	//box endpoints to capsule circles
+
+	Transform2D boxinv = p_transform_a.affine_inverse();
+
+	real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
+
+	for (int i = 0; i < 2; i++) {
+		{
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
+
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
+				return;
+			}
+		}
+
+		if constexpr (castA) {
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
+			capsule_endpoint -= p_motion_a;
+
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
+				return;
+			}
+		}
+
+		if constexpr (castB) {
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
+			capsule_endpoint += p_motion_b;
+
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
+				return;
+			}
+		}
+
+		if constexpr (castA && castB) {
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
+			capsule_endpoint -= p_motion_a;
+			capsule_endpoint += p_motion_b;
+
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
+				return;
+			}
+		}
+
+		capsule_dir *= -1.0;
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
+	const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotRectangleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	//box faces
+	if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
+		return;
+	}
+
+	//convex faces
+	Transform2D boxinv;
+	if constexpr (withMargin) {
+		boxinv = p_transform_a.affine_inverse();
+	}
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
+			return;
+		}
+
+		if constexpr (withMargin) {
+			//all points vs all points need to be tested if margin exist
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) {
+				return;
+			}
+			if constexpr (castA) {
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) {
+					return;
+				}
+			}
+			if constexpr (castB) {
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) {
+					return;
+				}
+			}
+			if constexpr (castA && castB) {
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) {
+					return;
+				}
+			}
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+/////////
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a);
+	const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotCapsuleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	//capsule axis
+
+	if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
+		return;
+	}
+
+	//capsule endpoints
+
+	real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
+	for (int i = 0; i < 2; i++) {
+		Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir_A;
+
+		real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
+		for (int j = 0; j < 2; j++) {
+			Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir_B;
+
+			if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) {
+				return;
+			}
+
+			capsule_dir_B *= -1.0;
+		}
+
+		capsule_dir_A *= -1.0;
+	}
+
+	separator.generate_contacts();
+}
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a);
+	const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotCapsuleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	//capsule axis
+
+	if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
+		return;
+	}
+
+	//poly vs capsule
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
+		Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i));
+
+		real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
+		for (int j = 0; j < 2; j++) {
+			Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir;
+
+			if (TEST_POINT(capsule_endpoint_A, cpoint)) {
+				return;
+			}
+
+			capsule_dir *= -1.0;
+		}
+
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
+			return;
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+/////////
+
+template <bool castA, bool castB, bool withMargin>
+static void _collision_convex_polygon_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
+	const GodotConvexPolygonShape2D *convex_A = static_cast<const GodotConvexPolygonShape2D *>(p_a);
+	const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
+
+	SeparatorAxisTest2D<GodotConvexPolygonShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	if (!separator.test_cast()) {
+		return;
+	}
+
+	for (int i = 0; i < convex_A->get_point_count(); i++) {
+		if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) {
+			return;
+		}
+	}
+
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
+			return;
+		}
+	}
+
+	if (withMargin) {
+		for (int i = 0; i < convex_A->get_point_count(); i++) {
+			for (int j = 0; j < convex_B->get_point_count(); j++) {
+				if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) {
+					return;
+				}
+			}
+		}
+	}
+
+	separator.generate_contacts();
+}
+
+////////
+
+bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) {
+	PhysicsServer2D::ShapeType type_A = p_shape_A->get_type();
+
+	ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false);
+	ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false);
+	ERR_FAIL_COND_V(p_shape_A->is_concave(), false);
+
+	PhysicsServer2D::ShapeType type_B = p_shape_B->get_type();
+
+	ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false);
+	ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false);
+	ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
+
+	static const CollisionFunc collision_table[5][5] = {
+		{ _collision_segment_segment<false, false, false>,
+				_collision_segment_circle<false, false, false>,
+				_collision_segment_rectangle<false, false, false>,
+				_collision_segment_capsule<false, false, false>,
+				_collision_segment_convex_polygon<false, false, false> },
+		{ nullptr,
+				_collision_circle_circle<false, false, false>,
+				_collision_circle_rectangle<false, false, false>,
+				_collision_circle_capsule<false, false, false>,
+				_collision_circle_convex_polygon<false, false, false> },
+		{ nullptr,
+				nullptr,
+				_collision_rectangle_rectangle<false, false, false>,
+				_collision_rectangle_capsule<false, false, false>,
+				_collision_rectangle_convex_polygon<false, false, false> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				_collision_capsule_capsule<false, false, false>,
+				_collision_capsule_convex_polygon<false, false, false> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				nullptr,
+				_collision_convex_polygon_convex_polygon<false, false, false> }
+
+	};
+
+	static const CollisionFunc collision_table_castA[5][5] = {
+		{ _collision_segment_segment<true, false, false>,
+				_collision_segment_circle<true, false, false>,
+				_collision_segment_rectangle<true, false, false>,
+				_collision_segment_capsule<true, false, false>,
+				_collision_segment_convex_polygon<true, false, false> },
+		{ nullptr,
+				_collision_circle_circle<true, false, false>,
+				_collision_circle_rectangle<true, false, false>,
+				_collision_circle_capsule<true, false, false>,
+				_collision_circle_convex_polygon<true, false, false> },
+		{ nullptr,
+				nullptr,
+				_collision_rectangle_rectangle<true, false, false>,
+				_collision_rectangle_capsule<true, false, false>,
+				_collision_rectangle_convex_polygon<true, false, false> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				_collision_capsule_capsule<true, false, false>,
+				_collision_capsule_convex_polygon<true, false, false> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				nullptr,
+				_collision_convex_polygon_convex_polygon<true, false, false> }
+
+	};
+
+	static const CollisionFunc collision_table_castB[5][5] = {
+		{ _collision_segment_segment<false, true, false>,
+				_collision_segment_circle<false, true, false>,
+				_collision_segment_rectangle<false, true, false>,
+				_collision_segment_capsule<false, true, false>,
+				_collision_segment_convex_polygon<false, true, false> },
+		{ nullptr,
+				_collision_circle_circle<false, true, false>,
+				_collision_circle_rectangle<false, true, false>,
+				_collision_circle_capsule<false, true, false>,
+				_collision_circle_convex_polygon<false, true, false> },
+		{ nullptr,
+				nullptr,
+				_collision_rectangle_rectangle<false, true, false>,
+				_collision_rectangle_capsule<false, true, false>,
+				_collision_rectangle_convex_polygon<false, true, false> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				_collision_capsule_capsule<false, true, false>,
+				_collision_capsule_convex_polygon<false, true, false> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				nullptr,
+				_collision_convex_polygon_convex_polygon<false, true, false> }
+
+	};
+
+	static const CollisionFunc collision_table_castA_castB[5][5] = {
+		{ _collision_segment_segment<true, true, false>,
+				_collision_segment_circle<true, true, false>,
+				_collision_segment_rectangle<true, true, false>,
+				_collision_segment_capsule<true, true, false>,
+				_collision_segment_convex_polygon<true, true, false> },
+		{ nullptr,
+				_collision_circle_circle<true, true, false>,
+				_collision_circle_rectangle<true, true, false>,
+				_collision_circle_capsule<true, true, false>,
+				_collision_circle_convex_polygon<true, true, false> },
+		{ nullptr,
+				nullptr,
+				_collision_rectangle_rectangle<true, true, false>,
+				_collision_rectangle_capsule<true, true, false>,
+				_collision_rectangle_convex_polygon<true, true, false> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				_collision_capsule_capsule<true, true, false>,
+				_collision_capsule_convex_polygon<true, true, false> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				nullptr,
+				_collision_convex_polygon_convex_polygon<true, true, false> }
+
+	};
+
+	static const CollisionFunc collision_table_margin[5][5] = {
+		{ _collision_segment_segment<false, false, true>,
+				_collision_segment_circle<false, false, true>,
+				_collision_segment_rectangle<false, false, true>,
+				_collision_segment_capsule<false, false, true>,
+				_collision_segment_convex_polygon<false, false, true> },
+		{ nullptr,
+				_collision_circle_circle<false, false, true>,
+				_collision_circle_rectangle<false, false, true>,
+				_collision_circle_capsule<false, false, true>,
+				_collision_circle_convex_polygon<false, false, true> },
+		{ nullptr,
+				nullptr,
+				_collision_rectangle_rectangle<false, false, true>,
+				_collision_rectangle_capsule<false, false, true>,
+				_collision_rectangle_convex_polygon<false, false, true> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				_collision_capsule_capsule<false, false, true>,
+				_collision_capsule_convex_polygon<false, false, true> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				nullptr,
+				_collision_convex_polygon_convex_polygon<false, false, true> }
+
+	};
+
+	static const CollisionFunc collision_table_castA_margin[5][5] = {
+		{ _collision_segment_segment<true, false, true>,
+				_collision_segment_circle<true, false, true>,
+				_collision_segment_rectangle<true, false, true>,
+				_collision_segment_capsule<true, false, true>,
+				_collision_segment_convex_polygon<true, false, true> },
+		{ nullptr,
+				_collision_circle_circle<true, false, true>,
+				_collision_circle_rectangle<true, false, true>,
+				_collision_circle_capsule<true, false, true>,
+				_collision_circle_convex_polygon<true, false, true> },
+		{ nullptr,
+				nullptr,
+				_collision_rectangle_rectangle<true, false, true>,
+				_collision_rectangle_capsule<true, false, true>,
+				_collision_rectangle_convex_polygon<true, false, true> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				_collision_capsule_capsule<true, false, true>,
+				_collision_capsule_convex_polygon<true, false, true> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				nullptr,
+				_collision_convex_polygon_convex_polygon<true, false, true> }
+
+	};
+
+	static const CollisionFunc collision_table_castB_margin[5][5] = {
+		{ _collision_segment_segment<false, true, true>,
+				_collision_segment_circle<false, true, true>,
+				_collision_segment_rectangle<false, true, true>,
+				_collision_segment_capsule<false, true, true>,
+				_collision_segment_convex_polygon<false, true, true> },
+		{ nullptr,
+				_collision_circle_circle<false, true, true>,
+				_collision_circle_rectangle<false, true, true>,
+				_collision_circle_capsule<false, true, true>,
+				_collision_circle_convex_polygon<false, true, true> },
+		{ nullptr,
+				nullptr,
+				_collision_rectangle_rectangle<false, true, true>,
+				_collision_rectangle_capsule<false, true, true>,
+				_collision_rectangle_convex_polygon<false, true, true> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				_collision_capsule_capsule<false, true, true>,
+				_collision_capsule_convex_polygon<false, true, true> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				nullptr,
+				_collision_convex_polygon_convex_polygon<false, true, true> }
+
+	};
+
+	static const CollisionFunc collision_table_castA_castB_margin[5][5] = {
+		{ _collision_segment_segment<true, true, true>,
+				_collision_segment_circle<true, true, true>,
+				_collision_segment_rectangle<true, true, true>,
+				_collision_segment_capsule<true, true, true>,
+				_collision_segment_convex_polygon<true, true, true> },
+		{ nullptr,
+				_collision_circle_circle<true, true, true>,
+				_collision_circle_rectangle<true, true, true>,
+				_collision_circle_capsule<true, true, true>,
+				_collision_circle_convex_polygon<true, true, true> },
+		{ nullptr,
+				nullptr,
+				_collision_rectangle_rectangle<true, true, true>,
+				_collision_rectangle_capsule<true, true, true>,
+				_collision_rectangle_convex_polygon<true, true, true> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				_collision_capsule_capsule<true, true, true>,
+				_collision_capsule_convex_polygon<true, true, true> },
+		{ nullptr,
+				nullptr,
+				nullptr,
+				nullptr,
+				_collision_convex_polygon_convex_polygon<true, true, true> }
+
+	};
+
+	_CollectorCallback2D callback;
+	callback.callback = p_result_callback;
+	callback.swap = p_swap;
+	callback.userdata = p_userdata;
+	callback.collided = false;
+	callback.sep_axis = sep_axis;
+
+	const GodotShape2D *A = p_shape_A;
+	const GodotShape2D *B = p_shape_B;
+	const Transform2D *transform_A = &p_transform_A;
+	const Transform2D *transform_B = &p_transform_B;
+	const Vector2 *motion_A = &p_motion_A;
+	const Vector2 *motion_B = &p_motion_B;
+	real_t margin_A = p_margin_A, margin_B = p_margin_B;
+
+	if (type_A > type_B) {
+		SWAP(A, B);
+		SWAP(transform_A, transform_B);
+		SWAP(type_A, type_B);
+		SWAP(motion_A, motion_B);
+		SWAP(margin_A, margin_B);
+		callback.swap = !callback.swap;
+	}
+
+	CollisionFunc collision_func;
+
+	if (p_margin_A || p_margin_B) {
+		if (*motion_A == Vector2() && *motion_B == Vector2()) {
+			collision_func = collision_table_margin[type_A - 2][type_B - 2];
+		} else if (*motion_A != Vector2() && *motion_B == Vector2()) {
+			collision_func = collision_table_castA_margin[type_A - 2][type_B - 2];
+		} else if (*motion_A == Vector2() && *motion_B != Vector2()) {
+			collision_func = collision_table_castB_margin[type_A - 2][type_B - 2];
+		} else {
+			collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2];
+		}
+	} else {
+		if (*motion_A == Vector2() && *motion_B == Vector2()) {
+			collision_func = collision_table[type_A - 2][type_B - 2];
+		} else if (*motion_A != Vector2() && *motion_B == Vector2()) {
+			collision_func = collision_table_castA[type_A - 2][type_B - 2];
+		} else if (*motion_A == Vector2() && *motion_B != Vector2()) {
+			collision_func = collision_table_castB[type_A - 2][type_B - 2];
+		} else {
+			collision_func = collision_table_castA_castB[type_A - 2][type_B - 2];
+		}
+	}
+
+	ERR_FAIL_NULL_V(collision_func, false);
+
+	collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B);
+
+	return callback.collided;
+}
diff --git a/modules/godot_physics_2d/godot_collision_solver_2d_sat.h b/modules/godot_physics_2d/godot_collision_solver_2d_sat.h
new file mode 100644
index 0000000000..c9183f7ecb
--- /dev/null
+++ b/modules/godot_physics_2d/godot_collision_solver_2d_sat.h
@@ -0,0 +1,38 @@
+/**************************************************************************/
+/*  godot_collision_solver_2d_sat.h                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_COLLISION_SOLVER_2D_SAT_H
+#define GODOT_COLLISION_SOLVER_2D_SAT_H
+
+#include "godot_collision_solver_2d.h"
+
+bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
+
+#endif // GODOT_COLLISION_SOLVER_2D_SAT_H
diff --git a/modules/godot_physics_2d/godot_constraint_2d.h b/modules/godot_physics_2d/godot_constraint_2d.h
new file mode 100644
index 0000000000..f4136f6643
--- /dev/null
+++ b/modules/godot_physics_2d/godot_constraint_2d.h
@@ -0,0 +1,70 @@
+/**************************************************************************/
+/*  godot_constraint_2d.h                                                 */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_CONSTRAINT_2D_H
+#define GODOT_CONSTRAINT_2D_H
+
+#include "godot_body_2d.h"
+
+class GodotConstraint2D {
+	GodotBody2D **_body_ptr;
+	int _body_count;
+	uint64_t island_step = 0;
+	bool disabled_collisions_between_bodies = true;
+
+	RID self;
+
+protected:
+	GodotConstraint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) {
+		_body_ptr = p_body_ptr;
+		_body_count = p_body_count;
+	}
+
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
+
+	_FORCE_INLINE_ GodotBody2D **get_body_ptr() const { return _body_ptr; }
+	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
+
+	_FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; }
+	_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
+
+	virtual bool setup(real_t p_step) = 0;
+	virtual bool pre_solve(real_t p_step) = 0;
+	virtual void solve(real_t p_step) = 0;
+
+	virtual ~GodotConstraint2D() {}
+};
+
+#endif // GODOT_CONSTRAINT_2D_H
diff --git a/modules/godot_physics_2d/godot_joints_2d.cpp b/modules/godot_physics_2d/godot_joints_2d.cpp
new file mode 100644
index 0000000000..5c76eb9dad
--- /dev/null
+++ b/modules/godot_physics_2d/godot_joints_2d.cpp
@@ -0,0 +1,595 @@
+/**************************************************************************/
+/*  godot_joints_2d.cpp                                                   */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_joints_2d.h"
+
+#include "godot_space_2d.h"
+
+//based on chipmunk joint constraints
+
+/* Copyright (c) 2007 Scott Lembcke
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+void GodotJoint2D::copy_settings_from(GodotJoint2D *p_joint) {
+	set_self(p_joint->get_self());
+	set_max_force(p_joint->get_max_force());
+	set_bias(p_joint->get_bias());
+	set_max_bias(p_joint->get_max_bias());
+	disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies());
+}
+
+static inline real_t k_scalar(GodotBody2D *a, GodotBody2D *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) {
+	real_t value = 0.0;
+
+	{
+		value += a->get_inv_mass();
+		real_t rcn = (rA - a->get_center_of_mass()).cross(n);
+		value += a->get_inv_inertia() * rcn * rcn;
+	}
+
+	if (b) {
+		value += b->get_inv_mass();
+		real_t rcn = (rB - b->get_center_of_mass()).cross(n);
+		value += b->get_inv_inertia() * rcn * rcn;
+	}
+
+	return value;
+}
+
+static inline Vector2
+relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB) {
+	Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity();
+	if (b) {
+		return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum;
+	} else {
+		return -sum;
+	}
+}
+
+static inline real_t
+normal_relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB, Vector2 n) {
+	return relative_velocity(a, b, rA, rB).dot(n);
+}
+
+bool GodotPinJoint2D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	GodotSpace2D *space = A->get_space();
+	ERR_FAIL_NULL_V(space, false);
+
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B;
+
+	real_t B_inv_mass = B ? B->get_inv_mass() : 0.0;
+
+	Transform2D K1;
+	K1[0].x = A->get_inv_mass() + B_inv_mass;
+	K1[1].x = 0.0f;
+	K1[0].y = 0.0f;
+	K1[1].y = A->get_inv_mass() + B_inv_mass;
+
+	Vector2 r1 = rA - A->get_center_of_mass();
+
+	Transform2D K2;
+	K2[0].x = A->get_inv_inertia() * r1.y * r1.y;
+	K2[1].x = -A->get_inv_inertia() * r1.x * r1.y;
+	K2[0].y = -A->get_inv_inertia() * r1.x * r1.y;
+	K2[1].y = A->get_inv_inertia() * r1.x * r1.x;
+
+	Transform2D K;
+	K[0] = K1[0] + K2[0];
+	K[1] = K1[1] + K2[1];
+
+	if (B) {
+		Vector2 r2 = rB - B->get_center_of_mass();
+
+		Transform2D K3;
+		K3[0].x = B->get_inv_inertia() * r2.y * r2.y;
+		K3[1].x = -B->get_inv_inertia() * r2.x * r2.y;
+		K3[0].y = -B->get_inv_inertia() * r2.x * r2.y;
+		K3[1].y = B->get_inv_inertia() * r2.x * r2.x;
+
+		K[0] += K3[0];
+		K[1] += K3[1];
+	}
+
+	K[0].x += softness;
+	K[1].y += softness;
+
+	M = K.affine_inverse();
+
+	Vector2 gA = rA + A->get_transform().get_origin();
+	Vector2 gB = B ? rB + B->get_transform().get_origin() : rB;
+
+	Vector2 delta = gB - gA;
+
+	bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step);
+
+	// Compute max impulse.
+	jn_max = get_max_force() * p_step;
+
+	return true;
+}
+
+inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) {
+	return Vector2(p_other * p_vec.y, -p_other * p_vec.x);
+}
+
+bool GodotPinJoint2D::pre_solve(real_t p_step) {
+	// Apply accumulated impulse.
+	if (dynamic_A) {
+		A->apply_impulse(-P, rA);
+	}
+	if (B && dynamic_B) {
+		B->apply_impulse(P, rB);
+	}
+	// Angle limits joint pre_solve step taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpRotaryLimitJoint.c
+	real_t i_sum_local = A->get_inv_inertia();
+	if (B) {
+		i_sum_local += B->get_inv_inertia();
+	}
+	i_sum = 1.0 / (i_sum_local);
+	if (angular_limit_enabled && B) {
+		Vector2 diff_vector = B->get_transform().get_origin() - A->get_transform().get_origin();
+		diff_vector = diff_vector.rotated(-initial_angle);
+		real_t dist = diff_vector.angle();
+		real_t pdist = 0.0;
+		if (dist > angular_limit_upper) {
+			pdist = dist - angular_limit_upper;
+		} else if (dist < angular_limit_lower) {
+			pdist = dist - angular_limit_lower;
+		}
+		real_t error_bias = Math::pow(1.0 - 0.15, 60.0);
+		// Calculate bias velocity.
+		bias_velocity = -CLAMP((-1.0 - Math::pow(error_bias, p_step)) * pdist / p_step, -get_max_bias(), get_max_bias());
+		// If the bias velocity is 0, the joint is not at a limit.
+		if (bias_velocity >= -CMP_EPSILON && bias_velocity <= CMP_EPSILON) {
+			j_acc = 0;
+			is_joint_at_limit = false;
+		} else {
+			is_joint_at_limit = true;
+		}
+	} else {
+		bias_velocity = 0.0;
+	}
+
+	return true;
+}
+
+void GodotPinJoint2D::solve(real_t p_step) {
+	// Compute relative velocity.
+	Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity());
+
+	Vector2 rel_vel;
+	if (B) {
+		rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA;
+	} else {
+		rel_vel = -vA;
+	}
+	// Angle limits joint solve step taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpRotaryLimitJoint.c
+	if ((angular_limit_enabled || motor_enabled) && B) {
+		// Compute relative rotational velocity.
+		real_t wr = B->get_angular_velocity() - A->get_angular_velocity();
+		// Motor solve part taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpSimpleMotor.c
+		if (motor_enabled) {
+			wr -= motor_target_velocity;
+		}
+		real_t j_max = jn_max;
+
+		// Compute normal impulse.
+		real_t j = -(bias_velocity + wr) * i_sum;
+		real_t j_old = j_acc;
+		// Only enable the limits if we have to.
+		if (angular_limit_enabled && is_joint_at_limit) {
+			if (bias_velocity < 0.0) {
+				j_acc = CLAMP(j_old + j, 0.0, j_max);
+			} else {
+				j_acc = CLAMP(j_old + j, -j_max, 0.0);
+			}
+		} else {
+			j_acc = CLAMP(j_old + j, -j_max, j_max);
+		}
+		j = j_acc - j_old;
+		A->apply_torque_impulse(-j * A->get_inv_inertia());
+		B->apply_torque_impulse(j * B->get_inv_inertia());
+	}
+
+	Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P);
+
+	if (dynamic_A) {
+		A->apply_impulse(-impulse, rA);
+	}
+	if (B && dynamic_B) {
+		B->apply_impulse(impulse, rB);
+	}
+
+	P += impulse;
+}
+
+void GodotPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer2D::PIN_JOINT_SOFTNESS: {
+			softness = p_value;
+		} break;
+		case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: {
+			angular_limit_upper = p_value;
+		} break;
+		case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: {
+			angular_limit_lower = p_value;
+		} break;
+		case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: {
+			motor_target_velocity = p_value;
+		} break;
+	}
+}
+
+real_t GodotPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer2D::PIN_JOINT_SOFTNESS: {
+			return softness;
+		}
+		case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: {
+			return angular_limit_upper;
+		}
+		case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: {
+			return angular_limit_lower;
+		}
+		case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: {
+			return motor_target_velocity;
+		}
+	}
+	ERR_FAIL_V(0);
+}
+
+void GodotPinJoint2D::set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled) {
+	switch (p_flag) {
+		case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: {
+			angular_limit_enabled = p_enabled;
+		} break;
+		case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: {
+			motor_enabled = p_enabled;
+		} break;
+	}
+}
+
+bool GodotPinJoint2D::get_flag(PhysicsServer2D::PinJointFlag p_flag) const {
+	switch (p_flag) {
+		case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: {
+			return angular_limit_enabled;
+		}
+		case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: {
+			return motor_enabled;
+		}
+	}
+	ERR_FAIL_V(0);
+}
+
+GodotPinJoint2D::GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
+		GodotJoint2D(_arr, p_body_b ? 2 : 1) {
+	A = p_body_a;
+	B = p_body_b;
+	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
+	anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos;
+
+	p_body_a->add_constraint(this, 0);
+	if (p_body_b) {
+		p_body_b->add_constraint(this, 1);
+		initial_angle = A->get_transform().get_origin().angle_to_point(B->get_transform().get_origin());
+	}
+}
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+static inline void
+k_tensor(GodotBody2D *a, GodotBody2D *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) {
+	// calculate mass matrix
+	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
+	real_t k11, k12, k21, k22;
+	real_t m_sum = a->get_inv_mass() + b->get_inv_mass();
+
+	// start with I*m_sum
+	k11 = m_sum;
+	k12 = 0.0f;
+	k21 = 0.0f;
+	k22 = m_sum;
+
+	r1 -= a->get_center_of_mass();
+	r2 -= b->get_center_of_mass();
+
+	// add the influence from r1
+	real_t a_i_inv = a->get_inv_inertia();
+	real_t r1xsq = r1.x * r1.x * a_i_inv;
+	real_t r1ysq = r1.y * r1.y * a_i_inv;
+	real_t r1nxy = -r1.x * r1.y * a_i_inv;
+	k11 += r1ysq;
+	k12 += r1nxy;
+	k21 += r1nxy;
+	k22 += r1xsq;
+
+	// add the influnce from r2
+	real_t b_i_inv = b->get_inv_inertia();
+	real_t r2xsq = r2.x * r2.x * b_i_inv;
+	real_t r2ysq = r2.y * r2.y * b_i_inv;
+	real_t r2nxy = -r2.x * r2.y * b_i_inv;
+	k11 += r2ysq;
+	k12 += r2nxy;
+	k21 += r2nxy;
+	k22 += r2xsq;
+
+	// invert
+	real_t determinant = k11 * k22 - k12 * k21;
+	ERR_FAIL_COND(determinant == 0.0);
+
+	real_t det_inv = 1.0f / determinant;
+	*k1 = Vector2(k22 * det_inv, -k12 * det_inv);
+	*k2 = Vector2(-k21 * det_inv, k11 * det_inv);
+}
+
+static _FORCE_INLINE_ Vector2
+mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
+	return Vector2(vr.dot(k1), vr.dot(k2));
+}
+
+bool GodotGrooveJoint2D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	GodotSpace2D *space = A->get_space();
+	ERR_FAIL_NULL_V(space, false);
+
+	// calculate endpoints in worldspace
+	Vector2 ta = A->get_transform().xform(A_groove_1);
+	Vector2 tb = A->get_transform().xform(A_groove_2);
+
+	// calculate axis
+	Vector2 n = -(tb - ta).orthogonal().normalized();
+	real_t d = ta.dot(n);
+
+	xf_normal = n;
+	rB = B->get_transform().basis_xform(B_anchor);
+
+	// calculate tangential distance along the axis of rB
+	real_t td = (B->get_transform().get_origin() + rB).cross(n);
+	// calculate clamping factor and rB
+	if (td <= ta.cross(n)) {
+		clamp = 1.0f;
+		rA = ta - A->get_transform().get_origin();
+	} else if (td >= tb.cross(n)) {
+		clamp = -1.0f;
+		rA = tb - A->get_transform().get_origin();
+	} else {
+		clamp = 0.0f;
+		//joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
+		rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin();
+	}
+
+	// Calculate mass tensor
+	k_tensor(A, B, rA, rB, &k1, &k2);
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// calculate bias velocity
+	//cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+	//joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
+
+	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
+
+	real_t _b = get_bias();
+	gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias());
+
+	correct = true;
+	return true;
+}
+
+bool GodotGrooveJoint2D::pre_solve(real_t p_step) {
+	// Apply accumulated impulse.
+	if (dynamic_A) {
+		A->apply_impulse(-jn_acc, rA);
+	}
+	if (dynamic_B) {
+		B->apply_impulse(jn_acc, rB);
+	}
+
+	return true;
+}
+
+void GodotGrooveJoint2D::solve(real_t p_step) {
+	// compute impulse
+	Vector2 vr = relative_velocity(A, B, rA, rB);
+
+	Vector2 j = mult_k(gbias - vr, k1, k2);
+	Vector2 jOld = jn_acc;
+	j += jOld;
+
+	jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max);
+
+	j = jn_acc - jOld;
+
+	if (dynamic_A) {
+		A->apply_impulse(-j, rA);
+	}
+	if (dynamic_B) {
+		B->apply_impulse(j, rB);
+	}
+}
+
+GodotGrooveJoint2D::GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
+		GodotJoint2D(_arr, 2) {
+	A = p_body_a;
+	B = p_body_b;
+
+	A_groove_1 = A->get_inv_transform().xform(p_a_groove1);
+	A_groove_2 = A->get_inv_transform().xform(p_a_groove2);
+	B_anchor = B->get_inv_transform().xform(p_b_anchor);
+	A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal();
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+bool GodotDampedSpringJoint2D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B->get_transform().basis_xform(anchor_B);
+
+	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
+	real_t dist = delta.length();
+
+	if (dist) {
+		n = delta / dist;
+	} else {
+		n = Vector2();
+	}
+
+	real_t k = k_scalar(A, B, rA, rB, n);
+	n_mass = 1.0f / k;
+
+	target_vrn = 0.0f;
+	v_coef = 1.0f - Math::exp(-damping * (p_step)*k);
+
+	// Calculate spring force.
+	real_t f_spring = (rest_length - dist) * stiffness;
+	j = n * f_spring * (p_step);
+
+	return true;
+}
+
+bool GodotDampedSpringJoint2D::pre_solve(real_t p_step) {
+	// Apply spring force.
+	if (dynamic_A) {
+		A->apply_impulse(-j, rA);
+	}
+	if (dynamic_B) {
+		B->apply_impulse(j, rB);
+	}
+
+	return true;
+}
+
+void GodotDampedSpringJoint2D::solve(real_t p_step) {
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn;
+
+	// compute velocity loss from drag
+	// not 100% certain this is derived correctly, though it makes sense
+	real_t v_damp = -vrn * v_coef;
+	target_vrn = vrn + v_damp;
+	Vector2 j_new = n * v_damp * n_mass;
+
+	if (dynamic_A) {
+		A->apply_impulse(-j_new, rA);
+	}
+	if (dynamic_B) {
+		B->apply_impulse(j_new, rB);
+	}
+}
+
+void GodotDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: {
+			rest_length = p_value;
+		} break;
+		case PhysicsServer2D::DAMPED_SPRING_DAMPING: {
+			damping = p_value;
+		} break;
+		case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: {
+			stiffness = p_value;
+		} break;
+	}
+}
+
+real_t GodotDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: {
+			return rest_length;
+		} break;
+		case PhysicsServer2D::DAMPED_SPRING_DAMPING: {
+			return damping;
+		} break;
+		case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: {
+			return stiffness;
+		} break;
+	}
+
+	ERR_FAIL_V(0);
+}
+
+GodotDampedSpringJoint2D::GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
+		GodotJoint2D(_arr, 2) {
+	A = p_body_a;
+	B = p_body_b;
+	anchor_A = A->get_inv_transform().xform(p_anchor_a);
+	anchor_B = B->get_inv_transform().xform(p_anchor_b);
+
+	rest_length = p_anchor_a.distance_to(p_anchor_b);
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
diff --git a/modules/godot_physics_2d/godot_joints_2d.h b/modules/godot_physics_2d/godot_joints_2d.h
new file mode 100644
index 0000000000..c6a1fdb692
--- /dev/null
+++ b/modules/godot_physics_2d/godot_joints_2d.h
@@ -0,0 +1,192 @@
+/**************************************************************************/
+/*  godot_joints_2d.h                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_JOINTS_2D_H
+#define GODOT_JOINTS_2D_H
+
+#include "godot_body_2d.h"
+#include "godot_constraint_2d.h"
+
+class GodotJoint2D : public GodotConstraint2D {
+	real_t bias = 0;
+	real_t max_bias = 3.40282e+38;
+	real_t max_force = 3.40282e+38;
+
+protected:
+	bool dynamic_A = false;
+	bool dynamic_B = false;
+
+public:
+	_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; }
+	_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
+
+	_FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; }
+	_FORCE_INLINE_ real_t get_bias() const { return bias; }
+
+	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; }
+	_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
+
+	virtual bool setup(real_t p_step) override { return false; }
+	virtual bool pre_solve(real_t p_step) override { return false; }
+	virtual void solve(real_t p_step) override {}
+
+	void copy_settings_from(GodotJoint2D *p_joint);
+
+	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; }
+	GodotJoint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) :
+			GodotConstraint2D(p_body_ptr, p_body_count) {}
+
+	virtual ~GodotJoint2D() {
+		for (int i = 0; i < get_body_count(); i++) {
+			GodotBody2D *body = get_body_ptr()[i];
+			if (body) {
+				body->remove_constraint(this, i);
+			}
+		}
+	};
+};
+
+class GodotPinJoint2D : public GodotJoint2D {
+	union {
+		struct {
+			GodotBody2D *A;
+			GodotBody2D *B;
+		};
+
+		GodotBody2D *_arr[2] = { nullptr, nullptr };
+	};
+
+	Transform2D M;
+	Vector2 rA, rB;
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+	Vector2 bias;
+	real_t initial_angle = 0.0;
+	real_t bias_velocity = 0.0;
+	real_t jn_max = 0.0;
+	real_t j_acc = 0.0;
+	real_t i_sum = 0.0;
+	Vector2 P;
+	real_t softness = 0.0;
+	real_t angular_limit_lower = 0.0;
+	real_t angular_limit_upper = 0.0;
+	real_t motor_target_velocity = 0.0;
+	bool is_joint_at_limit = false;
+	bool motor_enabled = false;
+	bool angular_limit_enabled = false;
+
+public:
+	virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
+
+	void set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled);
+	bool get_flag(PhysicsServer2D::PinJointFlag p_flag) const;
+
+	GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b = nullptr);
+};
+
+class GodotGrooveJoint2D : public GodotJoint2D {
+	union {
+		struct {
+			GodotBody2D *A;
+			GodotBody2D *B;
+		};
+
+		GodotBody2D *_arr[2] = { nullptr, nullptr };
+	};
+
+	Vector2 A_groove_1;
+	Vector2 A_groove_2;
+	Vector2 A_groove_normal;
+	Vector2 B_anchor;
+	Vector2 jn_acc;
+	Vector2 gbias;
+	real_t jn_max = 0.0;
+	real_t clamp = 0.0;
+	Vector2 xf_normal;
+	Vector2 rA, rB;
+	Vector2 k1, k2;
+
+	bool correct = false;
+
+public:
+	virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b);
+};
+
+class GodotDampedSpringJoint2D : public GodotJoint2D {
+	union {
+		struct {
+			GodotBody2D *A;
+			GodotBody2D *B;
+		};
+
+		GodotBody2D *_arr[2] = { nullptr, nullptr };
+	};
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+
+	real_t rest_length = 0.0;
+	real_t damping = 1.5;
+	real_t stiffness = 20.0;
+
+	Vector2 rA, rB;
+	Vector2 n;
+	Vector2 j;
+	real_t n_mass = 0.0;
+	real_t target_vrn = 0.0;
+	real_t v_coef = 0.0;
+
+public:
+	virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual bool pre_solve(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const;
+
+	GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b);
+};
+
+#endif // GODOT_JOINTS_2D_H
diff --git a/modules/godot_physics_2d/godot_physics_server_2d.cpp b/modules/godot_physics_2d/godot_physics_server_2d.cpp
new file mode 100644
index 0000000000..8df17992ea
--- /dev/null
+++ b/modules/godot_physics_2d/godot_physics_server_2d.cpp
@@ -0,0 +1,1400 @@
+/**************************************************************************/
+/*  godot_physics_server_2d.cpp                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_physics_server_2d.h"
+
+#include "godot_body_direct_state_2d.h"
+#include "godot_broad_phase_2d_bvh.h"
+#include "godot_collision_solver_2d.h"
+
+#include "core/config/project_settings.h"
+#include "core/debugger/engine_debugger.h"
+#include "core/os/os.h"
+
+#define FLUSH_QUERY_CHECK(m_object) \
+	ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead.");
+
+RID GodotPhysicsServer2D::_shape_create(ShapeType p_shape) {
+	GodotShape2D *shape = nullptr;
+	switch (p_shape) {
+		case SHAPE_WORLD_BOUNDARY: {
+			shape = memnew(GodotWorldBoundaryShape2D);
+		} break;
+		case SHAPE_SEPARATION_RAY: {
+			shape = memnew(GodotSeparationRayShape2D);
+		} break;
+		case SHAPE_SEGMENT: {
+			shape = memnew(GodotSegmentShape2D);
+		} break;
+		case SHAPE_CIRCLE: {
+			shape = memnew(GodotCircleShape2D);
+		} break;
+		case SHAPE_RECTANGLE: {
+			shape = memnew(GodotRectangleShape2D);
+		} break;
+		case SHAPE_CAPSULE: {
+			shape = memnew(GodotCapsuleShape2D);
+		} break;
+		case SHAPE_CONVEX_POLYGON: {
+			shape = memnew(GodotConvexPolygonShape2D);
+		} break;
+		case SHAPE_CONCAVE_POLYGON: {
+			shape = memnew(GodotConcavePolygonShape2D);
+		} break;
+		case SHAPE_CUSTOM: {
+			ERR_FAIL_V(RID());
+
+		} break;
+	}
+
+	RID id = shape_owner.make_rid(shape);
+	shape->set_self(id);
+
+	return id;
+}
+
+RID GodotPhysicsServer2D::world_boundary_shape_create() {
+	return _shape_create(SHAPE_WORLD_BOUNDARY);
+}
+
+RID GodotPhysicsServer2D::separation_ray_shape_create() {
+	return _shape_create(SHAPE_SEPARATION_RAY);
+}
+
+RID GodotPhysicsServer2D::segment_shape_create() {
+	return _shape_create(SHAPE_SEGMENT);
+}
+
+RID GodotPhysicsServer2D::circle_shape_create() {
+	return _shape_create(SHAPE_CIRCLE);
+}
+
+RID GodotPhysicsServer2D::rectangle_shape_create() {
+	return _shape_create(SHAPE_RECTANGLE);
+}
+
+RID GodotPhysicsServer2D::capsule_shape_create() {
+	return _shape_create(SHAPE_CAPSULE);
+}
+
+RID GodotPhysicsServer2D::convex_polygon_shape_create() {
+	return _shape_create(SHAPE_CONVEX_POLYGON);
+}
+
+RID GodotPhysicsServer2D::concave_polygon_shape_create() {
+	return _shape_create(SHAPE_CONCAVE_POLYGON);
+}
+
+void GodotPhysicsServer2D::shape_set_data(RID p_shape, const Variant &p_data) {
+	GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+	shape->set_data(p_data);
+};
+
+void GodotPhysicsServer2D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
+	GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+	shape->set_custom_bias(p_bias);
+}
+
+PhysicsServer2D::ShapeType GodotPhysicsServer2D::shape_get_type(RID p_shape) const {
+	const GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL_V(shape, SHAPE_CUSTOM);
+	return shape->get_type();
+};
+
+Variant GodotPhysicsServer2D::shape_get_data(RID p_shape) const {
+	const GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL_V(shape, Variant());
+	ERR_FAIL_COND_V(!shape->is_configured(), Variant());
+	return shape->get_data();
+};
+
+real_t GodotPhysicsServer2D::shape_get_custom_solver_bias(RID p_shape) const {
+	const GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL_V(shape, 0);
+	return shape->get_custom_bias();
+}
+
+void GodotPhysicsServer2D::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
+	CollCbkData *cbk = static_cast<CollCbkData *>(p_userdata);
+
+	if (cbk->max == 0) {
+		return;
+	}
+
+	Vector2 rel_dir = (p_point_A - p_point_B);
+	real_t rel_length2 = rel_dir.length_squared();
+	if (cbk->valid_dir != Vector2()) {
+		if (cbk->valid_depth < 10e20) {
+			if (rel_length2 > cbk->valid_depth * cbk->valid_depth ||
+					(rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) {
+				cbk->invalid_by_dir++;
+				return;
+			}
+		} else {
+			if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) {
+				return;
+			}
+		}
+	}
+
+	if (cbk->amount == cbk->max) {
+		//find least deep
+		real_t min_depth = 1e20;
+		int min_depth_idx = 0;
+		for (int i = 0; i < cbk->amount; i++) {
+			real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]);
+			if (d < min_depth) {
+				min_depth = d;
+				min_depth_idx = i;
+			}
+		}
+
+		if (rel_length2 < min_depth) {
+			return;
+		}
+		cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
+		cbk->ptr[min_depth_idx * 2 + 1] = p_point_B;
+		cbk->passed++;
+
+	} else {
+		cbk->ptr[cbk->amount * 2 + 0] = p_point_A;
+		cbk->ptr[cbk->amount * 2 + 1] = p_point_B;
+		cbk->amount++;
+		cbk->passed++;
+	}
+}
+
+bool GodotPhysicsServer2D::shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) {
+	GodotShape2D *shape_A = shape_owner.get_or_null(p_shape_A);
+	ERR_FAIL_NULL_V(shape_A, false);
+	GodotShape2D *shape_B = shape_owner.get_or_null(p_shape_B);
+	ERR_FAIL_NULL_V(shape_B, false);
+
+	if (p_result_max == 0) {
+		return GodotCollisionSolver2D::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, nullptr, nullptr);
+	}
+
+	CollCbkData cbk;
+	cbk.max = p_result_max;
+	cbk.amount = 0;
+	cbk.passed = 0;
+	cbk.ptr = r_results;
+
+	bool res = GodotCollisionSolver2D::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk);
+	r_result_count = cbk.amount;
+	return res;
+}
+
+RID GodotPhysicsServer2D::space_create() {
+	GodotSpace2D *space = memnew(GodotSpace2D);
+	RID id = space_owner.make_rid(space);
+	space->set_self(id);
+	RID area_id = area_create();
+	GodotArea2D *area = area_owner.get_or_null(area_id);
+	ERR_FAIL_NULL_V(area, RID());
+	space->set_default_area(area);
+	area->set_space(space);
+	area->set_priority(-1);
+
+	return id;
+};
+
+void GodotPhysicsServer2D::space_set_active(RID p_space, bool p_active) {
+	GodotSpace2D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL(space);
+	if (p_active) {
+		active_spaces.insert(space);
+	} else {
+		active_spaces.erase(space);
+	}
+}
+
+bool GodotPhysicsServer2D::space_is_active(RID p_space) const {
+	const GodotSpace2D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, false);
+
+	return active_spaces.has(space);
+}
+
+void GodotPhysicsServer2D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) {
+	GodotSpace2D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL(space);
+
+	space->set_param(p_param, p_value);
+}
+
+real_t GodotPhysicsServer2D::space_get_param(RID p_space, SpaceParameter p_param) const {
+	const GodotSpace2D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, 0);
+	return space->get_param(p_param);
+}
+
+void GodotPhysicsServer2D::space_set_debug_contacts(RID p_space, int p_max_contacts) {
+	GodotSpace2D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL(space);
+	space->set_debug_contacts(p_max_contacts);
+}
+
+Vector<Vector2> GodotPhysicsServer2D::space_get_contacts(RID p_space) const {
+	GodotSpace2D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, Vector<Vector2>());
+	return space->get_debug_contacts();
+}
+
+int GodotPhysicsServer2D::space_get_contact_count(RID p_space) const {
+	GodotSpace2D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, 0);
+	return space->get_debug_contact_count();
+}
+
+PhysicsDirectSpaceState2D *GodotPhysicsServer2D::space_get_direct_state(RID p_space) {
+	GodotSpace2D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, nullptr);
+	ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification.");
+
+	return space->get_direct_state();
+}
+
+RID GodotPhysicsServer2D::area_create() {
+	GodotArea2D *area = memnew(GodotArea2D);
+	RID rid = area_owner.make_rid(area);
+	area->set_self(rid);
+	return rid;
+}
+
+void GodotPhysicsServer2D::area_set_space(RID p_area, RID p_space) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	GodotSpace2D *space = nullptr;
+	if (p_space.is_valid()) {
+		space = space_owner.get_or_null(p_space);
+		ERR_FAIL_NULL(space);
+	}
+
+	if (area->get_space() == space) {
+		return; //pointless
+	}
+
+	area->clear_constraints();
+	area->set_space(space);
+}
+
+RID GodotPhysicsServer2D::area_get_space(RID p_area) const {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, RID());
+
+	GodotSpace2D *space = area->get_space();
+	if (!space) {
+		return RID();
+	}
+	return space->get_self();
+}
+
+void GodotPhysicsServer2D::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	area->add_shape(shape, p_transform, p_disabled);
+}
+
+void GodotPhysicsServer2D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+	ERR_FAIL_COND(!shape->is_configured());
+
+	area->set_shape(p_shape_idx, shape);
+}
+
+void GodotPhysicsServer2D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_shape_transform(p_shape_idx, p_transform);
+}
+
+void GodotPhysicsServer2D::area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+	ERR_FAIL_INDEX(p_shape, area->get_shape_count());
+	FLUSH_QUERY_CHECK(area);
+
+	area->set_shape_disabled(p_shape, p_disabled);
+}
+
+int GodotPhysicsServer2D::area_get_shape_count(RID p_area) const {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, -1);
+
+	return area->get_shape_count();
+}
+
+RID GodotPhysicsServer2D::area_get_shape(RID p_area, int p_shape_idx) const {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, RID());
+
+	GodotShape2D *shape = area->get_shape(p_shape_idx);
+	ERR_FAIL_NULL_V(shape, RID());
+
+	return shape->get_self();
+}
+
+Transform2D GodotPhysicsServer2D::area_get_shape_transform(RID p_area, int p_shape_idx) const {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, Transform2D());
+
+	return area->get_shape_transform(p_shape_idx);
+}
+
+void GodotPhysicsServer2D::area_remove_shape(RID p_area, int p_shape_idx) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->remove_shape(p_shape_idx);
+}
+
+void GodotPhysicsServer2D::area_clear_shapes(RID p_area) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	while (area->get_shape_count()) {
+		area->remove_shape(0);
+	}
+}
+
+void GodotPhysicsServer2D::area_attach_object_instance_id(RID p_area, ObjectID p_id) {
+	if (space_owner.owns(p_area)) {
+		GodotSpace2D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+	area->set_instance_id(p_id);
+}
+
+ObjectID GodotPhysicsServer2D::area_get_object_instance_id(RID p_area) const {
+	if (space_owner.owns(p_area)) {
+		GodotSpace2D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, ObjectID());
+	return area->get_instance_id();
+}
+
+void GodotPhysicsServer2D::area_attach_canvas_instance_id(RID p_area, ObjectID p_id) {
+	if (space_owner.owns(p_area)) {
+		GodotSpace2D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+	area->set_canvas_instance_id(p_id);
+}
+
+ObjectID GodotPhysicsServer2D::area_get_canvas_instance_id(RID p_area) const {
+	if (space_owner.owns(p_area)) {
+		GodotSpace2D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, ObjectID());
+	return area->get_canvas_instance_id();
+}
+
+void GodotPhysicsServer2D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) {
+	if (space_owner.owns(p_area)) {
+		GodotSpace2D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+	area->set_param(p_param, p_value);
+};
+
+void GodotPhysicsServer2D::area_set_transform(RID p_area, const Transform2D &p_transform) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+	area->set_transform(p_transform);
+};
+
+Variant GodotPhysicsServer2D::area_get_param(RID p_area, AreaParameter p_param) const {
+	if (space_owner.owns(p_area)) {
+		GodotSpace2D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, Variant());
+
+	return area->get_param(p_param);
+};
+
+Transform2D GodotPhysicsServer2D::area_get_transform(RID p_area) const {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, Transform2D());
+
+	return area->get_transform();
+};
+
+void GodotPhysicsServer2D::area_set_pickable(RID p_area, bool p_pickable) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+	area->set_pickable(p_pickable);
+}
+
+void GodotPhysicsServer2D::area_set_monitorable(RID p_area, bool p_monitorable) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+	FLUSH_QUERY_CHECK(area);
+
+	area->set_monitorable(p_monitorable);
+}
+
+void GodotPhysicsServer2D::area_set_collision_layer(RID p_area, uint32_t p_layer) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_collision_layer(p_layer);
+}
+
+uint32_t GodotPhysicsServer2D::area_get_collision_layer(RID p_area) const {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, 0);
+
+	return area->get_collision_layer();
+}
+
+void GodotPhysicsServer2D::area_set_collision_mask(RID p_area, uint32_t p_mask) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_collision_mask(p_mask);
+}
+
+uint32_t GodotPhysicsServer2D::area_get_collision_mask(RID p_area) const {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, 0);
+
+	return area->get_collision_mask();
+}
+
+void GodotPhysicsServer2D::area_set_monitor_callback(RID p_area, const Callable &p_callback) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_monitor_callback(p_callback.is_valid() ? p_callback : Callable());
+}
+
+void GodotPhysicsServer2D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) {
+	GodotArea2D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_area_monitor_callback(p_callback.is_valid() ? p_callback : Callable());
+}
+
+/* BODY API */
+
+RID GodotPhysicsServer2D::body_create() {
+	GodotBody2D *body = memnew(GodotBody2D);
+	RID rid = body_owner.make_rid(body);
+	body->set_self(rid);
+	return rid;
+}
+
+void GodotPhysicsServer2D::body_set_space(RID p_body, RID p_space) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	GodotSpace2D *space = nullptr;
+	if (p_space.is_valid()) {
+		space = space_owner.get_or_null(p_space);
+		ERR_FAIL_NULL(space);
+	}
+
+	if (body->get_space() == space) {
+		return; //pointless
+	}
+
+	body->clear_constraint_list();
+	body->set_space(space);
+};
+
+RID GodotPhysicsServer2D::body_get_space(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, RID());
+
+	GodotSpace2D *space = body->get_space();
+	if (!space) {
+		return RID();
+	}
+	return space->get_self();
+};
+
+void GodotPhysicsServer2D::body_set_mode(RID p_body, BodyMode p_mode) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	FLUSH_QUERY_CHECK(body);
+
+	body->set_mode(p_mode);
+};
+
+PhysicsServer2D::BodyMode GodotPhysicsServer2D::body_get_mode(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, BODY_MODE_STATIC);
+
+	return body->get_mode();
+};
+
+void GodotPhysicsServer2D::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform, bool p_disabled) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	body->add_shape(shape, p_transform, p_disabled);
+}
+
+void GodotPhysicsServer2D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	GodotShape2D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+	ERR_FAIL_COND(!shape->is_configured());
+
+	body->set_shape(p_shape_idx, shape);
+}
+
+void GodotPhysicsServer2D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_shape_transform(p_shape_idx, p_transform);
+}
+
+int GodotPhysicsServer2D::body_get_shape_count(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, -1);
+
+	return body->get_shape_count();
+}
+
+RID GodotPhysicsServer2D::body_get_shape(RID p_body, int p_shape_idx) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, RID());
+
+	GodotShape2D *shape = body->get_shape(p_shape_idx);
+	ERR_FAIL_NULL_V(shape, RID());
+
+	return shape->get_self();
+}
+
+Transform2D GodotPhysicsServer2D::body_get_shape_transform(RID p_body, int p_shape_idx) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Transform2D());
+
+	return body->get_shape_transform(p_shape_idx);
+}
+
+void GodotPhysicsServer2D::body_remove_shape(RID p_body, int p_shape_idx) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->remove_shape(p_shape_idx);
+}
+
+void GodotPhysicsServer2D::body_clear_shapes(RID p_body) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	while (body->get_shape_count()) {
+		body->remove_shape(0);
+	}
+}
+
+void GodotPhysicsServer2D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
+	FLUSH_QUERY_CHECK(body);
+
+	body->set_shape_disabled(p_shape_idx, p_disabled);
+}
+
+void GodotPhysicsServer2D::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
+	FLUSH_QUERY_CHECK(body);
+
+	body->set_shape_as_one_way_collision(p_shape_idx, p_enable, p_margin);
+}
+
+void GodotPhysicsServer2D::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	body->set_continuous_collision_detection_mode(p_mode);
+}
+
+GodotPhysicsServer2D::CCDMode GodotPhysicsServer2D::body_get_continuous_collision_detection_mode(RID p_body) const {
+	const GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, CCD_MODE_DISABLED);
+
+	return body->get_continuous_collision_detection_mode();
+}
+
+void GodotPhysicsServer2D::body_attach_object_instance_id(RID p_body, ObjectID p_id) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_instance_id(p_id);
+}
+
+ObjectID GodotPhysicsServer2D::body_get_object_instance_id(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, ObjectID());
+
+	return body->get_instance_id();
+}
+
+void GodotPhysicsServer2D::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_canvas_instance_id(p_id);
+}
+
+ObjectID GodotPhysicsServer2D::body_get_canvas_instance_id(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, ObjectID());
+
+	return body->get_canvas_instance_id();
+}
+
+void GodotPhysicsServer2D::body_set_collision_layer(RID p_body, uint32_t p_layer) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t GodotPhysicsServer2D::body_get_collision_layer(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_collision_layer();
+}
+
+void GodotPhysicsServer2D::body_set_collision_mask(RID p_body, uint32_t p_mask) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_collision_mask();
+}
+
+void GodotPhysicsServer2D::body_set_collision_priority(RID p_body, real_t p_priority) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_collision_priority(p_priority);
+}
+
+real_t GodotPhysicsServer2D::body_get_collision_priority(RID p_body) const {
+	const GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_collision_priority();
+}
+
+void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_param(p_param, p_value);
+}
+
+Variant GodotPhysicsServer2D::body_get_param(RID p_body, BodyParameter p_param) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_param(p_param);
+}
+
+void GodotPhysicsServer2D::body_reset_mass_properties(RID p_body) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->reset_mass_properties();
+}
+
+void GodotPhysicsServer2D::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_state(p_state, p_variant);
+}
+
+Variant GodotPhysicsServer2D::body_get_state(RID p_body, BodyState p_state) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Variant());
+
+	return body->get_state(p_state);
+}
+
+void GodotPhysicsServer2D::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->apply_central_impulse(p_impulse);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_apply_torque_impulse(RID p_body, real_t p_torque) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	_update_shapes();
+
+	body->apply_torque_impulse(p_torque);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	_update_shapes();
+
+	body->apply_impulse(p_impulse, p_position);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_apply_central_force(RID p_body, const Vector2 &p_force) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->apply_central_force(p_force);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->apply_force(p_force, p_position);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_apply_torque(RID p_body, real_t p_torque) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->apply_torque(p_torque);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_add_constant_central_force(RID p_body, const Vector2 &p_force) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_constant_central_force(p_force);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_constant_force(p_force, p_position);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_add_constant_torque(RID p_body, real_t p_torque) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_constant_torque(p_torque);
+	body->wakeup();
+}
+
+void GodotPhysicsServer2D::body_set_constant_force(RID p_body, const Vector2 &p_force) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_constant_force(p_force);
+	if (!p_force.is_zero_approx()) {
+		body->wakeup();
+	}
+}
+
+Vector2 GodotPhysicsServer2D::body_get_constant_force(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Vector2());
+	return body->get_constant_force();
+}
+
+void GodotPhysicsServer2D::body_set_constant_torque(RID p_body, real_t p_torque) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_constant_torque(p_torque);
+	if (!Math::is_zero_approx(p_torque)) {
+		body->wakeup();
+	}
+}
+
+real_t GodotPhysicsServer2D::body_get_constant_torque(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_constant_torque();
+}
+
+void GodotPhysicsServer2D::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	_update_shapes();
+
+	Vector2 v = body->get_linear_velocity();
+	Vector2 axis = p_axis_velocity.normalized();
+	v -= axis * axis.dot(v);
+	v += p_axis_velocity;
+	body->set_linear_velocity(v);
+	body->wakeup();
+};
+
+void GodotPhysicsServer2D::body_add_collision_exception(RID p_body, RID p_body_b) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_exception(p_body_b);
+	body->wakeup();
+};
+
+void GodotPhysicsServer2D::body_remove_collision_exception(RID p_body, RID p_body_b) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->remove_exception(p_body_b);
+	body->wakeup();
+};
+
+void GodotPhysicsServer2D::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	for (int i = 0; i < body->get_exceptions().size(); i++) {
+		p_exceptions->push_back(body->get_exceptions()[i]);
+	}
+};
+
+void GodotPhysicsServer2D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+};
+
+real_t GodotPhysicsServer2D::body_get_contacts_reported_depth_threshold(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+	return 0;
+};
+
+void GodotPhysicsServer2D::body_set_omit_force_integration(RID p_body, bool p_omit) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_omit_force_integration(p_omit);
+};
+
+bool GodotPhysicsServer2D::body_is_omitting_force_integration(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, false);
+	return body->get_omit_force_integration();
+};
+
+void GodotPhysicsServer2D::body_set_max_contacts_reported(RID p_body, int p_contacts) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	body->set_max_contacts_reported(p_contacts);
+}
+
+int GodotPhysicsServer2D::body_get_max_contacts_reported(RID p_body) const {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, -1);
+	return body->get_max_contacts_reported();
+}
+
+void GodotPhysicsServer2D::body_set_state_sync_callback(RID p_body, const Callable &p_callable) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	body->set_state_sync_callback(p_callable);
+}
+
+void GodotPhysicsServer2D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	body->set_force_integration_callback(p_callable, p_udata);
+}
+
+bool GodotPhysicsServer2D::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, false);
+	ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false);
+
+	return shape_collide(body->get_shape(p_body_shape)->get_self(), body->get_transform() * body->get_shape_transform(p_body_shape), Vector2(), p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count);
+}
+
+void GodotPhysicsServer2D::body_set_pickable(RID p_body, bool p_pickable) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+	body->set_pickable(p_pickable);
+}
+
+bool GodotPhysicsServer2D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) {
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, false);
+	ERR_FAIL_NULL_V(body->get_space(), false);
+	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
+
+	_update_shapes();
+
+	return body->get_space()->test_body_motion(body, p_parameters, r_result);
+}
+
+PhysicsDirectBodyState2D *GodotPhysicsServer2D::body_get_direct_state(RID p_body) {
+	ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
+
+	if (!body_owner.owns(p_body)) {
+		return nullptr;
+	}
+
+	GodotBody2D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, nullptr);
+
+	if (!body->get_space()) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
+
+	return body->get_direct_state();
+}
+
+/* JOINT API */
+
+RID GodotPhysicsServer2D::joint_create() {
+	GodotJoint2D *joint = memnew(GodotJoint2D);
+	RID joint_rid = joint_owner.make_rid(joint);
+	joint->set_self(joint_rid);
+	return joint_rid;
+}
+
+void GodotPhysicsServer2D::joint_clear(RID p_joint) {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+	if (joint->get_type() != JOINT_TYPE_MAX) {
+		GodotJoint2D *empty_joint = memnew(GodotJoint2D);
+		empty_joint->copy_settings_from(joint);
+
+		joint_owner.replace(p_joint, empty_joint);
+		memdelete(joint);
+	}
+}
+
+void GodotPhysicsServer2D::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	switch (p_param) {
+		case JOINT_PARAM_BIAS:
+			joint->set_bias(p_value);
+			break;
+		case JOINT_PARAM_MAX_BIAS:
+			joint->set_max_bias(p_value);
+			break;
+		case JOINT_PARAM_MAX_FORCE:
+			joint->set_max_force(p_value);
+			break;
+	}
+}
+
+real_t GodotPhysicsServer2D::joint_get_param(RID p_joint, JointParam p_param) const {
+	const GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, -1);
+
+	switch (p_param) {
+		case JOINT_PARAM_BIAS:
+			return joint->get_bias();
+			break;
+		case JOINT_PARAM_MAX_BIAS:
+			return joint->get_max_bias();
+			break;
+		case JOINT_PARAM_MAX_FORCE:
+			return joint->get_max_force();
+			break;
+	}
+
+	return 0;
+}
+
+void GodotPhysicsServer2D::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	joint->disable_collisions_between_bodies(p_disable);
+
+	if (2 == joint->get_body_count()) {
+		GodotBody2D *body_a = *joint->get_body_ptr();
+		GodotBody2D *body_b = *(joint->get_body_ptr() + 1);
+
+		if (p_disable) {
+			body_add_collision_exception(body_a->get_self(), body_b->get_self());
+			body_add_collision_exception(body_b->get_self(), body_a->get_self());
+		} else {
+			body_remove_collision_exception(body_a->get_self(), body_b->get_self());
+			body_remove_collision_exception(body_b->get_self(), body_a->get_self());
+		}
+	}
+}
+
+bool GodotPhysicsServer2D::joint_is_disabled_collisions_between_bodies(RID p_joint) const {
+	const GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, true);
+
+	return joint->is_disabled_collisions_between_bodies();
+}
+
+void GodotPhysicsServer2D::joint_make_pin(RID p_joint, const Vector2 &p_pos, RID p_body_a, RID p_body_b) {
+	GodotBody2D *A = body_owner.get_or_null(p_body_a);
+	ERR_FAIL_NULL(A);
+	GodotBody2D *B = nullptr;
+	if (body_owner.owns(p_body_b)) {
+		B = body_owner.get_or_null(p_body_b);
+		ERR_FAIL_NULL(B);
+	}
+
+	GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(prev_joint);
+
+	GodotJoint2D *joint = memnew(GodotPinJoint2D(p_pos, A, B));
+
+	joint_owner.replace(p_joint, joint);
+	joint->copy_settings_from(prev_joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer2D::joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) {
+	GodotBody2D *A = body_owner.get_or_null(p_body_a);
+	ERR_FAIL_NULL(A);
+
+	GodotBody2D *B = body_owner.get_or_null(p_body_b);
+	ERR_FAIL_NULL(B);
+
+	GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(prev_joint);
+
+	GodotJoint2D *joint = memnew(GodotGrooveJoint2D(p_a_groove1, p_a_groove2, p_b_anchor, A, B));
+
+	joint_owner.replace(p_joint, joint);
+	joint->copy_settings_from(prev_joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer2D::joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) {
+	GodotBody2D *A = body_owner.get_or_null(p_body_a);
+	ERR_FAIL_NULL(A);
+
+	GodotBody2D *B = body_owner.get_or_null(p_body_b);
+	ERR_FAIL_NULL(B);
+
+	GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(prev_joint);
+
+	GodotJoint2D *joint = memnew(GodotDampedSpringJoint2D(p_anchor_a, p_anchor_b, A, B));
+
+	joint_owner.replace(p_joint, joint);
+	joint->copy_settings_from(prev_joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer2D::pin_joint_set_flag(RID p_joint, PinJointFlag p_flag, bool p_enabled) {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
+
+	GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint);
+	pin_joint->set_flag(p_flag, p_enabled);
+}
+
+bool GodotPhysicsServer2D::pin_joint_get_flag(RID p_joint, PinJointFlag p_flag) const {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0);
+
+	GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint);
+	return pin_joint->get_flag(p_flag);
+}
+
+void GodotPhysicsServer2D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
+
+	GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint);
+	pin_joint->set_param(p_param, p_value);
+}
+
+real_t GodotPhysicsServer2D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0);
+
+	GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint);
+	return pin_joint->get_param(p_param);
+}
+
+void GodotPhysicsServer2D::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_DAMPED_SPRING);
+
+	GodotDampedSpringJoint2D *dsj = static_cast<GodotDampedSpringJoint2D *>(joint);
+	dsj->set_param(p_param, p_value);
+}
+
+real_t GodotPhysicsServer2D::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_DAMPED_SPRING, 0);
+
+	GodotDampedSpringJoint2D *dsj = static_cast<GodotDampedSpringJoint2D *>(joint);
+	return dsj->get_param(p_param);
+}
+
+PhysicsServer2D::JointType GodotPhysicsServer2D::joint_get_type(RID p_joint) const {
+	GodotJoint2D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, JOINT_TYPE_PIN);
+
+	return joint->get_type();
+}
+
+void GodotPhysicsServer2D::free(RID p_rid) {
+	_update_shapes(); // just in case
+
+	if (shape_owner.owns(p_rid)) {
+		GodotShape2D *shape = shape_owner.get_or_null(p_rid);
+
+		while (shape->get_owners().size()) {
+			GodotShapeOwner2D *so = shape->get_owners().begin()->key;
+			so->remove_shape(shape);
+		}
+
+		shape_owner.free(p_rid);
+		memdelete(shape);
+	} else if (body_owner.owns(p_rid)) {
+		GodotBody2D *body = body_owner.get_or_null(p_rid);
+
+		body_set_space(p_rid, RID());
+
+		while (body->get_shape_count()) {
+			body->remove_shape(0);
+		}
+
+		body_owner.free(p_rid);
+		memdelete(body);
+
+	} else if (area_owner.owns(p_rid)) {
+		GodotArea2D *area = area_owner.get_or_null(p_rid);
+
+		area->set_space(nullptr);
+
+		while (area->get_shape_count()) {
+			area->remove_shape(0);
+		}
+
+		area_owner.free(p_rid);
+		memdelete(area);
+	} else if (space_owner.owns(p_rid)) {
+		GodotSpace2D *space = space_owner.get_or_null(p_rid);
+
+		while (space->get_objects().size()) {
+			GodotCollisionObject2D *co = static_cast<GodotCollisionObject2D *>(*space->get_objects().begin());
+			co->set_space(nullptr);
+		}
+
+		active_spaces.erase(space);
+		free(space->get_default_area()->get_self());
+		space_owner.free(p_rid);
+		memdelete(space);
+	} else if (joint_owner.owns(p_rid)) {
+		GodotJoint2D *joint = joint_owner.get_or_null(p_rid);
+
+		joint_owner.free(p_rid);
+		memdelete(joint);
+
+	} else {
+		ERR_FAIL_MSG("Invalid ID.");
+	}
+}
+
+void GodotPhysicsServer2D::set_active(bool p_active) {
+	active = p_active;
+}
+
+void GodotPhysicsServer2D::init() {
+	doing_sync = false;
+	stepper = memnew(GodotStep2D);
+}
+
+void GodotPhysicsServer2D::step(real_t p_step) {
+	if (!active) {
+		return;
+	}
+
+	_update_shapes();
+
+	island_count = 0;
+	active_objects = 0;
+	collision_pairs = 0;
+	for (const GodotSpace2D *E : active_spaces) {
+		stepper->step(const_cast<GodotSpace2D *>(E), p_step);
+		island_count += E->get_island_count();
+		active_objects += E->get_active_objects();
+		collision_pairs += E->get_collision_pairs();
+	}
+}
+
+void GodotPhysicsServer2D::sync() {
+	doing_sync = true;
+}
+
+void GodotPhysicsServer2D::flush_queries() {
+	if (!active) {
+		return;
+	}
+
+	flushing_queries = true;
+
+	uint64_t time_beg = OS::get_singleton()->get_ticks_usec();
+
+	for (const GodotSpace2D *E : active_spaces) {
+		GodotSpace2D *space = const_cast<GodotSpace2D *>(E);
+		space->call_queries();
+	}
+
+	flushing_queries = false;
+
+	if (EngineDebugger::is_profiling("servers")) {
+		uint64_t total_time[GodotSpace2D::ELAPSED_TIME_MAX];
+		static const char *time_name[GodotSpace2D::ELAPSED_TIME_MAX] = {
+			"integrate_forces",
+			"generate_islands",
+			"setup_constraints",
+			"solve_constraints",
+			"integrate_velocities"
+		};
+
+		for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) {
+			total_time[i] = 0;
+		}
+
+		for (const GodotSpace2D *E : active_spaces) {
+			for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) {
+				total_time[i] += E->get_elapsed_time(GodotSpace2D::ElapsedTime(i));
+			}
+		}
+
+		Array values;
+		values.resize(GodotSpace2D::ELAPSED_TIME_MAX * 2);
+		for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) {
+			values[i * 2 + 0] = time_name[i];
+			values[i * 2 + 1] = USEC_TO_SEC(total_time[i]);
+		}
+		values.push_back("flush_queries");
+		values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg));
+
+		values.push_front("physics_2d");
+		EngineDebugger::profiler_add_frame_data("servers", values);
+	}
+}
+
+void GodotPhysicsServer2D::end_sync() {
+	doing_sync = false;
+}
+
+void GodotPhysicsServer2D::finish() {
+	memdelete(stepper);
+}
+
+void GodotPhysicsServer2D::_update_shapes() {
+	while (pending_shape_update_list.first()) {
+		pending_shape_update_list.first()->self()->_shape_changed();
+		pending_shape_update_list.remove(pending_shape_update_list.first());
+	}
+}
+
+int GodotPhysicsServer2D::get_process_info(ProcessInfo p_info) {
+	switch (p_info) {
+		case INFO_ACTIVE_OBJECTS: {
+			return active_objects;
+		} break;
+		case INFO_COLLISION_PAIRS: {
+			return collision_pairs;
+		} break;
+		case INFO_ISLAND_COUNT: {
+			return island_count;
+		} break;
+	}
+
+	return 0;
+}
+
+GodotPhysicsServer2D *GodotPhysicsServer2D::godot_singleton = nullptr;
+
+GodotPhysicsServer2D::GodotPhysicsServer2D(bool p_using_threads) {
+	godot_singleton = this;
+	GodotBroadPhase2D::create_func = GodotBroadPhase2DBVH::_create;
+
+	using_threads = p_using_threads;
+}
diff --git a/modules/godot_physics_2d/godot_physics_server_2d.h b/modules/godot_physics_2d/godot_physics_server_2d.h
new file mode 100644
index 0000000000..991cf67c95
--- /dev/null
+++ b/modules/godot_physics_2d/godot_physics_server_2d.h
@@ -0,0 +1,307 @@
+/**************************************************************************/
+/*  godot_physics_server_2d.h                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_PHYSICS_SERVER_2D_H
+#define GODOT_PHYSICS_SERVER_2D_H
+
+#include "godot_joints_2d.h"
+#include "godot_shape_2d.h"
+#include "godot_space_2d.h"
+#include "godot_step_2d.h"
+
+#include "core/templates/rid_owner.h"
+#include "servers/physics_server_2d.h"
+
+class GodotPhysicsServer2D : public PhysicsServer2D {
+	GDCLASS(GodotPhysicsServer2D, PhysicsServer2D);
+
+	friend class GodotPhysicsDirectSpaceState2D;
+	friend class GodotPhysicsDirectBodyState2D;
+	bool active = true;
+	bool doing_sync = false;
+
+	int island_count = 0;
+	int active_objects = 0;
+	int collision_pairs = 0;
+
+	bool using_threads = false;
+
+	bool flushing_queries = false;
+
+	GodotStep2D *stepper = nullptr;
+	HashSet<const GodotSpace2D *> active_spaces;
+
+	mutable RID_PtrOwner<GodotShape2D, true> shape_owner;
+	mutable RID_PtrOwner<GodotSpace2D, true> space_owner;
+	mutable RID_PtrOwner<GodotArea2D, true> area_owner;
+	mutable RID_PtrOwner<GodotBody2D, true> body_owner;
+	mutable RID_PtrOwner<GodotJoint2D, true> joint_owner;
+
+	static GodotPhysicsServer2D *godot_singleton;
+
+	friend class GodotCollisionObject2D;
+	SelfList<GodotCollisionObject2D>::List pending_shape_update_list;
+	void _update_shapes();
+
+	RID _shape_create(ShapeType p_shape);
+
+public:
+	struct CollCbkData {
+		Vector2 valid_dir;
+		real_t valid_depth = 0.0;
+		int max = 0;
+		int amount = 0;
+		int passed = 0;
+		int invalid_by_dir = 0;
+		Vector2 *ptr = nullptr;
+	};
+
+	virtual RID world_boundary_shape_create() override;
+	virtual RID separation_ray_shape_create() override;
+	virtual RID segment_shape_create() override;
+	virtual RID circle_shape_create() override;
+	virtual RID rectangle_shape_create() override;
+	virtual RID capsule_shape_create() override;
+	virtual RID convex_polygon_shape_create() override;
+	virtual RID concave_polygon_shape_create() override;
+
+	static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata);
+
+	virtual void shape_set_data(RID p_shape, const Variant &p_data) override;
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override;
+
+	virtual ShapeType shape_get_type(RID p_shape) const override;
+	virtual Variant shape_get_data(RID p_shape) const override;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const override;
+
+	virtual bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override;
+
+	/* SPACE API */
+
+	virtual RID space_create() override;
+	virtual void space_set_active(RID p_space, bool p_active) override;
+	virtual bool space_is_active(RID p_space) const override;
+
+	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override;
+	virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override;
+
+	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override;
+	virtual Vector<Vector2> space_get_contacts(RID p_space) const override;
+	virtual int space_get_contact_count(RID p_space) const override;
+
+	// this function only works on physics process, errors and returns null otherwise
+	virtual PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override;
+
+	/* AREA API */
+
+	virtual RID area_create() override;
+
+	virtual void area_set_space(RID p_area, RID p_space) override;
+	virtual RID area_get_space(RID p_area) const override;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false) override;
+	virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override;
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) override;
+
+	virtual int area_get_shape_count(RID p_area) const override;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const override;
+	virtual Transform2D area_get_shape_transform(RID p_area, int p_shape_idx) const override;
+
+	virtual void area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) override;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx) override;
+	virtual void area_clear_shapes(RID p_area) override;
+
+	virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override;
+	virtual ObjectID area_get_object_instance_id(RID p_area) const override;
+
+	virtual void area_attach_canvas_instance_id(RID p_area, ObjectID p_id) override;
+	virtual ObjectID area_get_canvas_instance_id(RID p_area) const override;
+
+	virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override;
+	virtual void area_set_transform(RID p_area, const Transform2D &p_transform) override;
+
+	virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override;
+	virtual Transform2D area_get_transform(RID p_area) const override;
+	virtual void area_set_monitorable(RID p_area, bool p_monitorable) override;
+
+	virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override;
+	virtual uint32_t area_get_collision_layer(RID p_area) const override;
+
+	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override;
+	virtual uint32_t area_get_collision_mask(RID p_area) const override;
+
+	virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override;
+	virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override;
+
+	virtual void area_set_pickable(RID p_area, bool p_pickable) override;
+
+	/* BODY API */
+
+	// create a body of a given type
+	virtual RID body_create() override;
+
+	virtual void body_set_space(RID p_body, RID p_space) override;
+	virtual RID body_get_space(RID p_body) const override;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode) override;
+	virtual BodyMode body_get_mode(RID p_body) const override;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false) override;
+	virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override;
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) override;
+
+	virtual int body_get_shape_count(RID p_body) const override;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const override;
+	virtual Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) const override;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx) override;
+	virtual void body_clear_shapes(RID p_body) override;
+
+	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override;
+	virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) override;
+
+	virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override;
+	virtual ObjectID body_get_object_instance_id(RID p_body) const override;
+
+	virtual void body_attach_canvas_instance_id(RID p_body, ObjectID p_id) override;
+	virtual ObjectID body_get_canvas_instance_id(RID p_body) const override;
+
+	virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) override;
+	virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const override;
+
+	virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override;
+	virtual uint32_t body_get_collision_layer(RID p_body) const override;
+
+	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
+	virtual uint32_t body_get_collision_mask(RID p_body) const override;
+
+	virtual void body_set_collision_priority(RID p_body, real_t p_priority) override;
+	virtual real_t body_get_collision_priority(RID p_body) const override;
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
+	virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
+
+	virtual void body_reset_mass_properties(RID p_body) override;
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const override;
+
+	virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override;
+	virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override;
+	virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override;
+
+	virtual void body_apply_central_force(RID p_body, const Vector2 &p_force) override;
+	virtual void body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void body_apply_torque(RID p_body, real_t p_torque) override;
+
+	virtual void body_add_constant_central_force(RID p_body, const Vector2 &p_force) override;
+	virtual void body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void body_add_constant_torque(RID p_body, real_t p_torque) override;
+
+	virtual void body_set_constant_force(RID p_body, const Vector2 &p_force) override;
+	virtual Vector2 body_get_constant_force(RID p_body) const override;
+
+	virtual void body_set_constant_torque(RID p_body, real_t p_torque) override;
+	virtual real_t body_get_constant_torque(RID p_body) const override;
+
+	virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override;
+
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b) override;
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override;
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
+
+	virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override;
+	virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override;
+
+	virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override;
+	virtual bool body_is_omitting_force_integration(RID p_body) const override;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override;
+	virtual int body_get_max_contacts_reported(RID p_body) const override;
+
+	virtual void body_set_state_sync_callback(RID p_body, const Callable &p_callable) override;
+	virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override;
+
+	virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override;
+
+	virtual void body_set_pickable(RID p_body, bool p_pickable) override;
+
+	virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override;
+
+	// this function only works on physics process, errors and returns null otherwise
+	virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override;
+
+	/* JOINT API */
+
+	virtual RID joint_create() override;
+
+	virtual void joint_clear(RID p_joint) override;
+
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override;
+	virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override;
+
+	virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override;
+	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override;
+
+	virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override;
+	virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override;
+	virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override;
+
+	virtual void pin_joint_set_flag(RID p_joint, PinJointFlag p_flag, bool p_enabled) override;
+	virtual bool pin_joint_get_flag(RID p_joint, PinJointFlag p_flag) const override;
+	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override;
+	virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override;
+	virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override;
+	virtual real_t damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const override;
+
+	virtual JointType joint_get_type(RID p_joint) const override;
+
+	/* MISC */
+
+	virtual void free(RID p_rid) override;
+
+	virtual void set_active(bool p_active) override;
+	virtual void init() override;
+	virtual void step(real_t p_step) override;
+	virtual void sync() override;
+	virtual void flush_queries() override;
+	virtual void end_sync() override;
+	virtual void finish() override;
+
+	virtual bool is_flushing_queries() const override { return flushing_queries; }
+
+	int get_process_info(ProcessInfo p_info) override;
+
+	GodotPhysicsServer2D(bool p_using_threads = false);
+	~GodotPhysicsServer2D() {}
+};
+
+#endif // GODOT_PHYSICS_SERVER_2D_H
diff --git a/modules/godot_physics_2d/godot_shape_2d.cpp b/modules/godot_physics_2d/godot_shape_2d.cpp
new file mode 100644
index 0000000000..d77b1a77e3
--- /dev/null
+++ b/modules/godot_physics_2d/godot_shape_2d.cpp
@@ -0,0 +1,985 @@
+/**************************************************************************/
+/*  godot_shape_2d.cpp                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_shape_2d.h"
+
+#include "core/math/geometry_2d.h"
+#include "core/templates/sort_array.h"
+
+void GodotShape2D::configure(const Rect2 &p_aabb) {
+	aabb = p_aabb;
+	configured = true;
+	for (const KeyValue<GodotShapeOwner2D *, int> &E : owners) {
+		GodotShapeOwner2D *co = const_cast<GodotShapeOwner2D *>(E.key);
+		co->_shape_changed();
+	}
+}
+
+Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const {
+	Vector2 res[2];
+	int amnt;
+	get_supports(p_normal, res, amnt);
+	return res[0];
+}
+
+void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) {
+	HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner);
+	if (E) {
+		E->value++;
+	} else {
+		owners[p_owner] = 1;
+	}
+}
+
+void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) {
+	HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner);
+	ERR_FAIL_COND(!E);
+	E->value--;
+	if (E->value == 0) {
+		owners.remove(E);
+	}
+}
+
+bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const {
+	return owners.has(p_owner);
+}
+
+const HashMap<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const {
+	return owners;
+}
+
+GodotShape2D::~GodotShape2D() {
+	ERR_FAIL_COND(owners.size());
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+void GodotWorldBoundaryShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
+	r_amount = 0;
+}
+
+bool GodotWorldBoundaryShape2D::contains_point(const Vector2 &p_point) const {
+	return normal.dot(p_point) < d;
+}
+
+bool GodotWorldBoundaryShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	Vector2 segment = p_begin - p_end;
+	real_t den = normal.dot(segment);
+
+	//printf("den is %i\n",den);
+	if (Math::abs(den) <= CMP_EPSILON) {
+		return false;
+	}
+
+	real_t dist = (normal.dot(p_begin) - d) / den;
+	//printf("dist is %i\n",dist);
+
+	if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) {
+		return false;
+	}
+
+	r_point = p_begin + segment * -dist;
+	r_normal = normal;
+
+	return true;
+}
+
+real_t GodotWorldBoundaryShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
+	return 0;
+}
+
+void GodotWorldBoundaryShape2D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY);
+	Array arr = p_data;
+	ERR_FAIL_COND(arr.size() != 2);
+	normal = arr[0];
+	d = arr[1];
+	configure(Rect2(Vector2(-1e15, -1e15), Vector2(1e15 * 2, 1e15 * 2)));
+}
+
+Variant GodotWorldBoundaryShape2D::get_data() const {
+	Array arr;
+	arr.resize(2);
+	arr[0] = normal;
+	arr[1] = d;
+	return arr;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+void GodotSeparationRayShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
+	r_amount = 1;
+
+	if (p_normal.y > 0) {
+		*r_supports = Vector2(0, length);
+	} else {
+		*r_supports = Vector2();
+	}
+}
+
+bool GodotSeparationRayShape2D::contains_point(const Vector2 &p_point) const {
+	return false;
+}
+
+bool GodotSeparationRayShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	return false; //rays can't be intersected
+}
+
+real_t GodotSeparationRayShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
+	return 0; //rays are mass-less
+}
+
+void GodotSeparationRayShape2D::set_data(const Variant &p_data) {
+	Dictionary d = p_data;
+	length = d["length"];
+	slide_on_slope = d["slide_on_slope"];
+	configure(Rect2(0, 0, 0.001, length));
+}
+
+Variant GodotSeparationRayShape2D::get_data() const {
+	Dictionary d;
+	d["length"] = length;
+	d["slide_on_slope"] = slide_on_slope;
+	return d;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+void GodotSegmentShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
+	if (Math::abs(p_normal.dot(n)) > segment_is_valid_support_threshold) {
+		r_supports[0] = a;
+		r_supports[1] = b;
+		r_amount = 2;
+		return;
+	}
+
+	real_t dp = p_normal.dot(b - a);
+	if (dp > 0) {
+		*r_supports = b;
+	} else {
+		*r_supports = a;
+	}
+	r_amount = 1;
+}
+
+bool GodotSegmentShape2D::contains_point(const Vector2 &p_point) const {
+	return false;
+}
+
+bool GodotSegmentShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) {
+		return false;
+	}
+
+	if (n.dot(p_begin) > n.dot(a)) {
+		r_normal = n;
+	} else {
+		r_normal = -n;
+	}
+
+	return true;
+}
+
+real_t GodotSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
+	return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12;
+}
+
+void GodotSegmentShape2D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::RECT2);
+
+	Rect2 r = p_data;
+	a = r.position;
+	b = r.size;
+	n = (b - a).orthogonal();
+
+	Rect2 aabb_new;
+	aabb_new.position = a;
+	aabb_new.expand_to(b);
+	if (aabb_new.size.x == 0) {
+		aabb_new.size.x = 0.001;
+	}
+	if (aabb_new.size.y == 0) {
+		aabb_new.size.y = 0.001;
+	}
+	configure(aabb_new);
+}
+
+Variant GodotSegmentShape2D::get_data() const {
+	Rect2 r;
+	r.position = a;
+	r.size = b;
+	return r;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+void GodotCircleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
+	r_amount = 1;
+	*r_supports = p_normal * radius;
+}
+
+bool GodotCircleShape2D::contains_point(const Vector2 &p_point) const {
+	return p_point.length_squared() < radius * radius;
+}
+
+bool GodotCircleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	Vector2 line_vec = p_end - p_begin;
+
+	real_t a, b, c;
+
+	a = line_vec.dot(line_vec);
+	b = 2 * p_begin.dot(line_vec);
+	c = p_begin.dot(p_begin) - radius * radius;
+
+	real_t sqrtterm = b * b - 4 * a * c;
+
+	if (sqrtterm < 0) {
+		return false;
+	}
+	sqrtterm = Math::sqrt(sqrtterm);
+	real_t res = (-b - sqrtterm) / (2 * a);
+
+	if (res < 0 || res > 1 + CMP_EPSILON) {
+		return false;
+	}
+
+	r_point = p_begin + line_vec * res;
+	r_normal = r_point.normalized();
+	return true;
+}
+
+real_t GodotCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
+	real_t a = radius * p_scale.x;
+	real_t b = radius * p_scale.y;
+	return p_mass * (a * a + b * b) / 4;
+}
+
+void GodotCircleShape2D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(!p_data.is_num());
+	radius = p_data;
+	configure(Rect2(-radius, -radius, radius * 2, radius * 2));
+}
+
+Variant GodotCircleShape2D::get_data() const {
+	return radius;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
+	for (int i = 0; i < 2; i++) {
+		Vector2 ag;
+		ag[i] = 1.0;
+		real_t dp = ag.dot(p_normal);
+		if (Math::abs(dp) <= segment_is_valid_support_threshold) {
+			continue;
+		}
+
+		real_t sgn = dp > 0 ? 1.0 : -1.0;
+
+		r_amount = 2;
+
+		r_supports[0][i] = half_extents[i] * sgn;
+		r_supports[0][i ^ 1] = half_extents[i ^ 1];
+
+		r_supports[1][i] = half_extents[i] * sgn;
+		r_supports[1][i ^ 1] = -half_extents[i ^ 1];
+
+		return;
+	}
+
+	/* USE POINT */
+
+	r_amount = 1;
+	r_supports[0] = Vector2(
+			(p_normal.x < 0) ? -half_extents.x : half_extents.x,
+			(p_normal.y < 0) ? -half_extents.y : half_extents.y);
+}
+
+bool GodotRectangleShape2D::contains_point(const Vector2 &p_point) const {
+	real_t x = p_point.x;
+	real_t y = p_point.y;
+	real_t edge_x = half_extents.x;
+	real_t edge_y = half_extents.y;
+	return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y);
+}
+
+bool GodotRectangleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal);
+}
+
+real_t GodotRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
+	Vector2 he2 = half_extents * 2 * p_scale;
+	return p_mass * he2.dot(he2) / 12.0;
+}
+
+void GodotRectangleShape2D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2);
+
+	half_extents = p_data;
+	configure(Rect2(-half_extents, half_extents * 2.0));
+}
+
+Variant GodotRectangleShape2D::get_data() const {
+	return half_extents;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
+	Vector2 n = p_normal;
+
+	real_t h = height * 0.5 - radius; // half-height of the rectangle part
+
+	if (h > 0 && Math::abs(n.x) > segment_is_valid_support_threshold) {
+		// make it flat
+		n.y = 0.0;
+		n.x = SIGN(n.x) * radius;
+
+		r_amount = 2;
+		r_supports[0] = n;
+		r_supports[0].y += h;
+		r_supports[1] = n;
+		r_supports[1].y -= h;
+	} else {
+		n *= radius;
+		n.y += (n.y > 0) ? h : -h;
+		r_amount = 1;
+		*r_supports = n;
+	}
+}
+
+bool GodotCapsuleShape2D::contains_point(const Vector2 &p_point) const {
+	Vector2 p = p_point;
+	p.y = Math::abs(p.y);
+	p.y -= height * 0.5 - radius;
+	if (p.y < 0) {
+		p.y = 0;
+	}
+
+	return p.length_squared() < radius * radius;
+}
+
+bool GodotCapsuleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	real_t d = 1e10;
+	Vector2 n = (p_end - p_begin).normalized();
+	bool collided = false;
+
+	//try spheres
+	for (int i = 0; i < 2; i++) {
+		Vector2 begin = p_begin;
+		Vector2 end = p_end;
+		real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius;
+		begin.y += ofs;
+		end.y += ofs;
+
+		Vector2 line_vec = end - begin;
+
+		real_t a, b, c;
+
+		a = line_vec.dot(line_vec);
+		b = 2 * begin.dot(line_vec);
+		c = begin.dot(begin) - radius * radius;
+
+		real_t sqrtterm = b * b - 4 * a * c;
+
+		if (sqrtterm < 0) {
+			continue;
+		}
+
+		sqrtterm = Math::sqrt(sqrtterm);
+		real_t res = (-b - sqrtterm) / (2 * a);
+
+		if (res < 0 || res > 1 + CMP_EPSILON) {
+			continue;
+		}
+
+		Vector2 point = begin + line_vec * res;
+		Vector2 pointf(point.x, point.y - ofs);
+		real_t pd = n.dot(pointf);
+		if (pd < d) {
+			r_point = pointf;
+			r_normal = point.normalized();
+			d = pd;
+			collided = true;
+		}
+	}
+
+	Vector2 rpos, rnorm;
+	if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) {
+		real_t pd = n.dot(rpos);
+		if (pd < d) {
+			r_point = rpos;
+			r_normal = rnorm;
+			d = pd;
+			collided = true;
+		}
+	}
+
+	//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
+	return collided; //todo
+}
+
+real_t GodotCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
+	Vector2 he2 = Vector2(radius * 2, height) * p_scale;
+	return p_mass * he2.dot(he2) / 12.0;
+}
+
+void GodotCapsuleShape2D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2);
+
+	if (p_data.get_type() == Variant::ARRAY) {
+		Array arr = p_data;
+		ERR_FAIL_COND(arr.size() != 2);
+		height = arr[0];
+		radius = arr[1];
+	} else {
+		Point2 p = p_data;
+		radius = p.x;
+		height = p.y;
+	}
+
+	Point2 he(radius, height * 0.5);
+	configure(Rect2(-he, he * 2));
+}
+
+Variant GodotCapsuleShape2D::get_data() const {
+	return Point2(height, radius);
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
+	int support_idx = -1;
+	real_t d = -1e10;
+	r_amount = 0;
+
+	for (int i = 0; i < point_count; i++) {
+		//test point
+		real_t ld = p_normal.dot(points[i].pos);
+		if (ld > d) {
+			support_idx = i;
+			d = ld;
+		}
+
+		//test segment
+		if (points[i].normal.dot(p_normal) > segment_is_valid_support_threshold) {
+			r_amount = 2;
+			r_supports[0] = points[i].pos;
+			r_supports[1] = points[(i + 1) % point_count].pos;
+			return;
+		}
+	}
+
+	ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found.");
+
+	r_amount = 1;
+	r_supports[0] = points[support_idx].pos;
+}
+
+bool GodotConvexPolygonShape2D::contains_point(const Vector2 &p_point) const {
+	bool out = false;
+	bool in = false;
+
+	for (int i = 0; i < point_count; i++) {
+		real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos);
+		if (d > 0) {
+			out = true;
+		} else {
+			in = true;
+		}
+	}
+
+	return in != out;
+}
+
+bool GodotConvexPolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	Vector2 n = (p_end - p_begin).normalized();
+	real_t d = 1e10;
+	bool inters = false;
+
+	for (int i = 0; i < point_count; i++) {
+		Vector2 res;
+
+		if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) {
+			continue;
+		}
+
+		real_t nd = n.dot(res);
+		if (nd < d) {
+			d = nd;
+			r_point = res;
+			r_normal = points[i].normal;
+			inters = true;
+		}
+	}
+
+	return inters;
+}
+
+real_t GodotConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
+	ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points.");
+	Rect2 aabb_new;
+	aabb_new.position = points[0].pos * p_scale;
+	for (int i = 0; i < point_count; i++) {
+		aabb_new.expand_to(points[i].pos * p_scale);
+	}
+
+	return p_mass * aabb_new.size.dot(aabb_new.size) / 12.0;
+}
+
+void GodotConvexPolygonShape2D::set_data(const Variant &p_data) {
+#ifdef REAL_T_IS_DOUBLE
+	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
+#else
+	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
+#endif
+
+	if (points) {
+		memdelete_arr(points);
+	}
+	points = nullptr;
+	point_count = 0;
+
+	if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) {
+		Vector<Vector2> arr = p_data;
+		ERR_FAIL_COND(arr.is_empty());
+		point_count = arr.size();
+		points = memnew_arr(Point, point_count);
+		const Vector2 *r = arr.ptr();
+
+		for (int i = 0; i < point_count; i++) {
+			points[i].pos = r[i];
+		}
+
+		for (int i = 0; i < point_count; i++) {
+			Vector2 p = points[i].pos;
+			Vector2 pn = points[(i + 1) % point_count].pos;
+			points[i].normal = (pn - p).orthogonal().normalized();
+		}
+	} else {
+		Vector<real_t> dvr = p_data;
+		point_count = dvr.size() / 4;
+		ERR_FAIL_COND(point_count == 0);
+
+		points = memnew_arr(Point, point_count);
+		const real_t *r = dvr.ptr();
+
+		for (int i = 0; i < point_count; i++) {
+			int idx = i << 2;
+			points[i].pos.x = r[idx + 0];
+			points[i].pos.y = r[idx + 1];
+			points[i].normal.x = r[idx + 2];
+			points[i].normal.y = r[idx + 3];
+		}
+	}
+
+	ERR_FAIL_COND(point_count == 0);
+	Rect2 aabb_new;
+	aabb_new.position = points[0].pos;
+	for (int i = 1; i < point_count; i++) {
+		aabb_new.expand_to(points[i].pos);
+	}
+
+	configure(aabb_new);
+}
+
+Variant GodotConvexPolygonShape2D::get_data() const {
+	Vector<Vector2> dvr;
+
+	dvr.resize(point_count);
+
+	for (int i = 0; i < point_count; i++) {
+		dvr.set(i, points[i].pos);
+	}
+
+	return dvr;
+}
+
+GodotConvexPolygonShape2D::~GodotConvexPolygonShape2D() {
+	if (points) {
+		memdelete_arr(points);
+	}
+}
+
+//////////////////////////////////////////////////
+
+void GodotConcavePolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
+	real_t d = -1e10;
+	int idx = -1;
+	for (int i = 0; i < points.size(); i++) {
+		real_t ld = p_normal.dot(points[i]);
+		if (ld > d) {
+			d = ld;
+			idx = i;
+		}
+	}
+
+	r_amount = 1;
+	ERR_FAIL_COND(idx == -1);
+	*r_supports = points[idx];
+}
+
+bool GodotConcavePolygonShape2D::contains_point(const Vector2 &p_point) const {
+	return false; //sorry
+}
+
+bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
+	if (segments.size() == 0 || points.size() == 0) {
+		return false;
+	}
+
+	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
+
+	enum {
+		TEST_AABB_BIT = 0,
+		VISIT_LEFT_BIT = 1,
+		VISIT_RIGHT_BIT = 2,
+		VISIT_DONE_BIT = 3,
+		VISITED_BIT_SHIFT = 29,
+		NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+		VISITED_BIT_MASK = ~NODE_IDX_MASK,
+
+	};
+
+	Vector2 n = (p_end - p_begin).normalized();
+	real_t d = 1e10;
+	bool inters = false;
+
+	/*
+	for(int i=0;i<bvh_depth;i++)
+		stack[i]=0;
+	*/
+
+	int level = 0;
+
+	const Segment *segmentptr = &segments[0];
+	const Vector2 *pointptr = &points[0];
+	const BVH *bvhptr = &bvh[0];
+
+	stack[0] = 0;
+	while (true) {
+		uint32_t node = stack[level] & NODE_IDX_MASK;
+		const BVH &bvh2 = bvhptr[node];
+		bool done = false;
+
+		switch (stack[level] >> VISITED_BIT_SHIFT) {
+			case TEST_AABB_BIT: {
+				bool valid = bvh2.aabb.intersects_segment(p_begin, p_end);
+				if (!valid) {
+					stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+				} else {
+					if (bvh2.left < 0) {
+						const Segment &s = segmentptr[bvh2.right];
+						Vector2 a = pointptr[s.points[0]];
+						Vector2 b = pointptr[s.points[1]];
+
+						Vector2 res;
+
+						if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) {
+							real_t nd = n.dot(res);
+							if (nd < d) {
+								d = nd;
+								r_point = res;
+								r_normal = (b - a).orthogonal().normalized();
+								inters = true;
+							}
+						}
+
+						stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+					} else {
+						stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
+					}
+				}
+			}
+				continue;
+			case VISIT_LEFT_BIT: {
+				stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+				stack[level + 1] = bvh2.left | TEST_AABB_BIT;
+				level++;
+			}
+				continue;
+			case VISIT_RIGHT_BIT: {
+				stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				stack[level + 1] = bvh2.right | TEST_AABB_BIT;
+				level++;
+			}
+				continue;
+			case VISIT_DONE_BIT: {
+				if (level == 0) {
+					done = true;
+					break;
+				} else {
+					level--;
+				}
+			}
+				continue;
+		}
+
+		if (done) {
+			break;
+		}
+	}
+
+	if (inters) {
+		if (n.dot(r_normal) > 0) {
+			r_normal = -r_normal;
+		}
+	}
+
+	return inters;
+}
+
+int GodotConcavePolygonShape2D::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
+	if (p_len == 1) {
+		bvh_depth = MAX(p_depth, bvh_depth);
+		bvh.push_back(*p_bvh);
+		return bvh.size() - 1;
+	}
+
+	//else sort best
+
+	Rect2 global_aabb = p_bvh[0].aabb;
+	for (int i = 1; i < p_len; i++) {
+		global_aabb = global_aabb.merge(p_bvh[i].aabb);
+	}
+
+	if (global_aabb.size.x > global_aabb.size.y) {
+		SortArray<BVH, BVH_CompareX> sort;
+		sort.sort(p_bvh, p_len);
+
+	} else {
+		SortArray<BVH, BVH_CompareY> sort;
+		sort.sort(p_bvh, p_len);
+	}
+
+	int median = p_len / 2;
+
+	BVH node;
+	node.aabb = global_aabb;
+	int node_idx = bvh.size();
+	bvh.push_back(node);
+
+	int l = _generate_bvh(p_bvh, median, p_depth + 1);
+	int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1);
+	bvh.write[node_idx].left = l;
+	bvh.write[node_idx].right = r;
+
+	return node_idx;
+}
+
+void GodotConcavePolygonShape2D::set_data(const Variant &p_data) {
+#ifdef REAL_T_IS_DOUBLE
+	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
+#else
+	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
+#endif
+
+	Rect2 aabb_new;
+
+	if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) {
+		Vector<Vector2> p2arr = p_data;
+		int len = p2arr.size();
+		ERR_FAIL_COND(len % 2);
+
+		segments.clear();
+		points.clear();
+		bvh.clear();
+		bvh_depth = 1;
+
+		if (len == 0) {
+			configure(aabb_new);
+			return;
+		}
+
+		const Vector2 *arr = p2arr.ptr();
+
+		HashMap<Point2, int> pointmap;
+		for (int i = 0; i < len; i += 2) {
+			Point2 p1 = arr[i];
+			Point2 p2 = arr[i + 1];
+			int idx_p1, idx_p2;
+
+			if (pointmap.has(p1)) {
+				idx_p1 = pointmap[p1];
+			} else {
+				idx_p1 = pointmap.size();
+				pointmap[p1] = idx_p1;
+			}
+
+			if (pointmap.has(p2)) {
+				idx_p2 = pointmap[p2];
+			} else {
+				idx_p2 = pointmap.size();
+				pointmap[p2] = idx_p2;
+			}
+
+			Segment s;
+			s.points[0] = idx_p1;
+			s.points[1] = idx_p2;
+			segments.push_back(s);
+		}
+
+		points.resize(pointmap.size());
+		aabb_new.position = pointmap.begin()->key;
+		for (const KeyValue<Point2, int> &E : pointmap) {
+			aabb_new.expand_to(E.key);
+			points.write[E.value] = E.key;
+		}
+
+		Vector<BVH> main_vbh;
+		main_vbh.resize(segments.size());
+		for (int i = 0; i < main_vbh.size(); i++) {
+			main_vbh.write[i].aabb.position = points[segments[i].points[0]];
+			main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]);
+			main_vbh.write[i].left = -1;
+			main_vbh.write[i].right = i;
+		}
+
+		_generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1);
+
+	} else {
+		//dictionary with arrays
+	}
+
+	configure(aabb_new);
+}
+
+Variant GodotConcavePolygonShape2D::get_data() const {
+	Vector<Vector2> rsegments;
+	int len = segments.size();
+	rsegments.resize(len * 2);
+	Vector2 *w = rsegments.ptrw();
+	for (int i = 0; i < len; i++) {
+		w[(i << 1) + 0] = points[segments[i].points[0]];
+		w[(i << 1) + 1] = points[segments[i].points[1]];
+	}
+
+	return rsegments;
+}
+
+void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
+	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
+
+	enum {
+		TEST_AABB_BIT = 0,
+		VISIT_LEFT_BIT = 1,
+		VISIT_RIGHT_BIT = 2,
+		VISIT_DONE_BIT = 3,
+		VISITED_BIT_SHIFT = 29,
+		NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+		VISITED_BIT_MASK = ~NODE_IDX_MASK,
+
+	};
+
+	/*
+	for(int i=0;i<bvh_depth;i++)
+		stack[i]=0;
+	*/
+
+	if (segments.size() == 0 || points.size() == 0 || bvh.size() == 0) {
+		return;
+	}
+
+	int level = 0;
+
+	const Segment *segmentptr = &segments[0];
+	const Vector2 *pointptr = &points[0];
+	const BVH *bvhptr = &bvh[0];
+
+	stack[0] = 0;
+	while (true) {
+		uint32_t node = stack[level] & NODE_IDX_MASK;
+		const BVH &bvh2 = bvhptr[node];
+
+		switch (stack[level] >> VISITED_BIT_SHIFT) {
+			case TEST_AABB_BIT: {
+				bool valid = p_local_aabb.intersects(bvh2.aabb);
+				if (!valid) {
+					stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+				} else {
+					if (bvh2.left < 0) {
+						const Segment &s = segmentptr[bvh2.right];
+						Vector2 a = pointptr[s.points[0]];
+						Vector2 b = pointptr[s.points[1]];
+
+						GodotSegmentShape2D ss(a, b, (b - a).orthogonal().normalized());
+
+						if (p_callback(p_userdata, &ss)) {
+							return;
+						}
+						stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+					} else {
+						stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
+					}
+				}
+			}
+				continue;
+			case VISIT_LEFT_BIT: {
+				stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+				stack[level + 1] = bvh2.left | TEST_AABB_BIT;
+				level++;
+			}
+				continue;
+			case VISIT_RIGHT_BIT: {
+				stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				stack[level + 1] = bvh2.right | TEST_AABB_BIT;
+				level++;
+			}
+				continue;
+			case VISIT_DONE_BIT: {
+				if (level == 0) {
+					return;
+				} else {
+					level--;
+				}
+			}
+				continue;
+		}
+	}
+}
diff --git a/modules/godot_physics_2d/godot_shape_2d.h b/modules/godot_physics_2d/godot_shape_2d.h
new file mode 100644
index 0000000000..28c69574a0
--- /dev/null
+++ b/modules/godot_physics_2d/godot_shape_2d.h
@@ -0,0 +1,539 @@
+/**************************************************************************/
+/*  godot_shape_2d.h                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_SHAPE_2D_H
+#define GODOT_SHAPE_2D_H
+
+#include "servers/physics_server_2d.h"
+
+class GodotShape2D;
+
+class GodotShapeOwner2D {
+public:
+	virtual void _shape_changed() = 0;
+	virtual void remove_shape(GodotShape2D *p_shape) = 0;
+
+	virtual ~GodotShapeOwner2D() {}
+};
+
+class GodotShape2D {
+	RID self;
+	Rect2 aabb;
+	bool configured = false;
+	real_t custom_bias = 0.0;
+
+	HashMap<GodotShapeOwner2D *, int> owners;
+
+protected:
+	const double segment_is_valid_support_threshold = 0.99998;
+	const double segment_is_valid_support_threshold_lower =
+			Math::sqrt(1.0 - segment_is_valid_support_threshold * segment_is_valid_support_threshold);
+
+	void configure(const Rect2 &p_aabb);
+
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	virtual PhysicsServer2D::ShapeType get_type() const = 0;
+
+	_FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
+	_FORCE_INLINE_ bool is_configured() const { return configured; }
+
+	virtual bool allows_one_way_collision() const { return true; }
+
+	virtual bool is_concave() const { return false; }
+
+	virtual bool contains_point(const Vector2 &p_point) const = 0;
+
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0;
+	virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0;
+	virtual Vector2 get_support(const Vector2 &p_normal) const;
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0;
+
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0;
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0;
+	virtual void set_data(const Variant &p_data) = 0;
+	virtual Variant get_data() const = 0;
+
+	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; }
+	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
+
+	void add_owner(GodotShapeOwner2D *p_owner);
+	void remove_owner(GodotShapeOwner2D *p_owner);
+	bool is_owner(GodotShapeOwner2D *p_owner) const;
+	const HashMap<GodotShapeOwner2D *, int> &get_owners() const;
+
+	_FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const {
+		get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount);
+		for (int i = 0; i < r_amount; i++) {
+			r_supports[i] = p_xform.xform(r_supports[i]);
+		}
+
+		if (r_amount == 1) {
+			if (Math::abs(p_normal.dot(p_cast.normalized())) < segment_is_valid_support_threshold_lower) {
+				//make line because they are parallel
+				r_amount = 2;
+				r_supports[1] = r_supports[0] + p_cast;
+			} else if (p_cast.dot(p_normal) > 0) {
+				//normal points towards cast, add cast
+				r_supports[0] += p_cast;
+			}
+
+		} else {
+			if (Math::abs(p_normal.dot(p_cast.normalized())) < segment_is_valid_support_threshold_lower) {
+				//optimize line and make it larger because they are parallel
+				if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) {
+					//larger towards 1
+					r_supports[1] += p_cast;
+				} else {
+					//larger towards 0
+					r_supports[0] += p_cast;
+				}
+			} else if (p_cast.dot(p_normal) > 0) {
+				//normal points towards cast, add cast
+				r_supports[0] += p_cast;
+				r_supports[1] += p_cast;
+			}
+		}
+	}
+	GodotShape2D() {}
+	virtual ~GodotShape2D();
+};
+
+//let the optimizer do the magic
+#define DEFAULT_PROJECT_RANGE_CAST                                                                                                                                  \
+	virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \
+		project_range_cast(p_cast, p_normal, p_transform, r_min, r_max);                                                                                            \
+	}                                                                                                                                                               \
+	_FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {    \
+		real_t mina, maxa;                                                                                                                                          \
+		real_t minb, maxb;                                                                                                                                          \
+		Transform2D ofsb = p_transform;                                                                                                                             \
+		ofsb.columns[2] += p_cast;                                                                                                                                  \
+		project_range(p_normal, p_transform, mina, maxa);                                                                                                           \
+		project_range(p_normal, ofsb, minb, maxb);                                                                                                                  \
+		r_min = MIN(mina, minb);                                                                                                                                    \
+		r_max = MAX(maxa, maxb);                                                                                                                                    \
+	}
+
+class GodotWorldBoundaryShape2D : public GodotShape2D {
+	Vector2 normal;
+	real_t d = 0.0;
+
+public:
+	_FORCE_INLINE_ Vector2 get_normal() const { return normal; }
+	_FORCE_INLINE_ real_t get_d() const { return d; }
+
+	virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; }
+
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
+
+	virtual bool contains_point(const Vector2 &p_point) const override;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		r_min = -1e10;
+		r_max = 1e10;
+	}
+
+	virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override {
+		project_range_cast(p_cast, p_normal, p_transform, r_min, r_max);
+	}
+
+	_FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		r_min = -1e10;
+		r_max = 1e10;
+	}
+};
+
+class GodotSeparationRayShape2D : public GodotShape2D {
+	real_t length = 0.0;
+	bool slide_on_slope = false;
+
+public:
+	_FORCE_INLINE_ real_t get_length() const { return length; }
+	_FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; }
+
+	virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; }
+
+	virtual bool allows_one_way_collision() const override { return false; }
+
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
+
+	virtual bool contains_point(const Vector2 &p_point) const override;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		r_max = p_normal.dot(p_transform.get_origin());
+		r_min = p_normal.dot(p_transform.xform(Vector2(0, length)));
+		if (r_max < r_min) {
+			SWAP(r_max, r_min);
+		}
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+	_FORCE_INLINE_ GodotSeparationRayShape2D() {}
+	_FORCE_INLINE_ GodotSeparationRayShape2D(real_t p_length) { length = p_length; }
+};
+
+class GodotSegmentShape2D : public GodotShape2D {
+	Vector2 a;
+	Vector2 b;
+	Vector2 n;
+
+public:
+	_FORCE_INLINE_ const Vector2 &get_a() const { return a; }
+	_FORCE_INLINE_ const Vector2 &get_b() const { return b; }
+	_FORCE_INLINE_ const Vector2 &get_normal() const { return n; }
+
+	virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; }
+
+	_FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const {
+		return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal();
+	}
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
+
+	virtual bool contains_point(const Vector2 &p_point) const override;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		r_max = p_normal.dot(p_transform.xform(a));
+		r_min = p_normal.dot(p_transform.xform(b));
+		if (r_max < r_min) {
+			SWAP(r_max, r_min);
+		}
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+	_FORCE_INLINE_ GodotSegmentShape2D() {}
+	_FORCE_INLINE_ GodotSegmentShape2D(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) {
+		a = p_a;
+		b = p_b;
+		n = p_n;
+	}
+};
+
+class GodotCircleShape2D : public GodotShape2D {
+	real_t radius;
+
+public:
+	_FORCE_INLINE_ const real_t &get_radius() const { return radius; }
+
+	virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; }
+
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
+
+	virtual bool contains_point(const Vector2 &p_point) const override;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		real_t d = p_normal.dot(p_transform.get_origin());
+
+		// figure out scale at point
+		Vector2 local_normal = p_transform.basis_xform_inv(p_normal);
+		real_t scale = local_normal.length();
+
+		r_min = d - (radius)*scale;
+		r_max = d + (radius)*scale;
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+};
+
+class GodotRectangleShape2D : public GodotShape2D {
+	Vector2 half_extents;
+
+public:
+	_FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; }
+
+	virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; }
+
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
+
+	virtual bool contains_point(const Vector2 &p_point) const override;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		// no matter the angle, the box is mirrored anyway
+		r_max = -1e20;
+		r_min = 1e20;
+		for (int i = 0; i < 4; i++) {
+			real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y)));
+
+			if (d > r_max) {
+				r_max = d;
+			}
+			if (d < r_min) {
+				r_min = d;
+			}
+		}
+	}
+
+	_FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const {
+		Vector2 local_v = p_xform_inv.xform(p_circle);
+
+		Vector2 he(
+				(local_v.x < 0) ? -half_extents.x : half_extents.x,
+				(local_v.y < 0) ? -half_extents.y : half_extents.y);
+
+		return (p_xform.xform(he) - p_circle).normalized();
+	}
+
+	_FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const GodotRectangleShape2D *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const {
+		Vector2 a, b;
+
+		{
+			Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin());
+
+			Vector2 he(
+					(local_v.x < 0) ? -half_extents.x : half_extents.x,
+					(local_v.y < 0) ? -half_extents.y : half_extents.y);
+
+			a = p_xform.xform(he);
+		}
+		{
+			Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin());
+
+			Vector2 he(
+					(local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x,
+					(local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y);
+
+			b = p_B_xform.xform(he);
+		}
+
+		return (a - b).normalized();
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+};
+
+class GodotCapsuleShape2D : public GodotShape2D {
+	real_t radius = 0.0;
+	real_t height = 0.0;
+
+public:
+	_FORCE_INLINE_ const real_t &get_radius() const { return radius; }
+	_FORCE_INLINE_ const real_t &get_height() const { return height; }
+
+	virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; }
+
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
+
+	virtual bool contains_point(const Vector2 &p_point) const override;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		// no matter the angle, the box is mirrored anyway
+		Vector2 n = p_transform.basis_xform_inv(p_normal).normalized();
+		real_t h = height * 0.5 - radius;
+
+		n *= radius;
+		n.y += (n.y > 0) ? h : -h;
+
+		r_max = p_normal.dot(p_transform.xform(n));
+		r_min = p_normal.dot(p_transform.xform(-n));
+
+		if (r_max < r_min) {
+			SWAP(r_max, r_min);
+		}
+
+		//ERR_FAIL_COND( r_max < r_min );
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+};
+
+class GodotConvexPolygonShape2D : public GodotShape2D {
+	struct Point {
+		Vector2 pos;
+		Vector2 normal; //normal to next segment
+	};
+
+	Point *points = nullptr;
+	int point_count = 0;
+
+public:
+	_FORCE_INLINE_ int get_point_count() const { return point_count; }
+	_FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; }
+	_FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; }
+	_FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const {
+		Vector2 a = points[p_idx].pos;
+		p_idx++;
+		Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos;
+		return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal();
+	}
+
+	virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; }
+
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
+
+	virtual bool contains_point(const Vector2 &p_point) const override;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		if (!points || point_count <= 0) {
+			r_min = r_max = 0;
+			return;
+		}
+
+		r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));
+		for (int i = 1; i < point_count; i++) {
+			real_t d = p_normal.dot(p_transform.xform(points[i].pos));
+			if (d > r_max) {
+				r_max = d;
+			}
+			if (d < r_min) {
+				r_min = d;
+			}
+		}
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+	GodotConvexPolygonShape2D() {}
+	~GodotConvexPolygonShape2D();
+};
+
+class GodotConcaveShape2D : public GodotShape2D {
+public:
+	virtual bool is_concave() const override { return true; }
+
+	// Returns true to stop the query.
+	typedef bool (*QueryCallback)(void *p_userdata, GodotShape2D *p_convex);
+
+	virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
+};
+
+class GodotConcavePolygonShape2D : public GodotConcaveShape2D {
+	struct Segment {
+		int points[2] = {};
+	};
+
+	Vector<Segment> segments;
+	Vector<Point2> points;
+
+	struct BVH {
+		Rect2 aabb;
+		int left = 0, right = 0;
+	};
+
+	Vector<BVH> bvh;
+	int bvh_depth = 0;
+
+	struct BVH_CompareX {
+		_FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const {
+			return (a.aabb.position.x + a.aabb.size.x * 0.5) < (b.aabb.position.x + b.aabb.size.x * 0.5);
+		}
+	};
+
+	struct BVH_CompareY {
+		_FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const {
+			return (a.aabb.position.y + a.aabb.size.y * 0.5) < (b.aabb.position.y + b.aabb.size.y * 0.5);
+		}
+	};
+
+	int _generate_bvh(BVH *p_bvh, int p_len, int p_depth);
+
+public:
+	virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; }
+
+	virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override {
+		r_min = 0;
+		r_max = 0;
+		ERR_FAIL_MSG("Unsupported call to project_rangev in GodotConcavePolygonShape2D");
+	}
+
+	void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
+		r_min = 0;
+		r_max = 0;
+		ERR_FAIL_MSG("Unsupported call to project_range in GodotConcavePolygonShape2D");
+	}
+
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
+
+	virtual bool contains_point(const Vector2 &p_point) const override;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override;
+
+	virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; }
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override;
+
+	DEFAULT_PROJECT_RANGE_CAST
+};
+
+#undef DEFAULT_PROJECT_RANGE_CAST
+
+#endif // GODOT_SHAPE_2D_H
diff --git a/modules/godot_physics_2d/godot_space_2d.cpp b/modules/godot_physics_2d/godot_space_2d.cpp
new file mode 100644
index 0000000000..2966818beb
--- /dev/null
+++ b/modules/godot_physics_2d/godot_space_2d.cpp
@@ -0,0 +1,1240 @@
+/**************************************************************************/
+/*  godot_space_2d.cpp                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_space_2d.h"
+
+#include "godot_collision_solver_2d.h"
+#include "godot_physics_server_2d.h"
+
+#include "core/os/os.h"
+#include "core/templates/pair.h"
+
+#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001
+#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
+
+_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+	if (!(p_object->get_collision_layer() & p_collision_mask)) {
+		return false;
+	}
+
+	if (p_object->get_type() == GodotCollisionObject2D::TYPE_AREA && !p_collide_with_areas) {
+		return false;
+	}
+
+	if (p_object->get_type() == GodotCollisionObject2D::TYPE_BODY && !p_collide_with_bodies) {
+		return false;
+	}
+
+	return true;
+}
+
+int GodotPhysicsDirectSpaceState2D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
+	if (p_result_max <= 0) {
+		return 0;
+	}
+
+	Rect2 aabb;
+	aabb.position = p_parameters.position - Vector2(0.00001, 0.00001);
+	aabb.size = Vector2(0.00002, 0.00002);
+
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+
+	int cc = 0;
+
+	for (int i = 0; i < amount; i++) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
+			continue;
+		}
+
+		if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
+			continue;
+		}
+
+		const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
+
+		if (p_parameters.pick_point && !col_obj->is_pickable()) {
+			continue;
+		}
+
+		if (col_obj->get_canvas_instance_id() != p_parameters.canvas_instance_id) {
+			continue;
+		}
+
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		GodotShape2D *shape = col_obj->get_shape(shape_idx);
+
+		Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_parameters.position);
+
+		if (!shape->contains_point(local_point)) {
+			continue;
+		}
+
+		if (cc >= p_result_max) {
+			continue;
+		}
+
+		r_results[cc].collider_id = col_obj->get_instance_id();
+		if (r_results[cc].collider_id.is_valid()) {
+			r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
+		}
+		r_results[cc].rid = col_obj->get_self();
+		r_results[cc].shape = shape_idx;
+
+		cc++;
+	}
+
+	return cc;
+}
+
+bool GodotPhysicsDirectSpaceState2D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) {
+	ERR_FAIL_COND_V(space->locked, false);
+
+	Vector2 begin, end;
+	Vector2 normal;
+	begin = p_parameters.from;
+	end = p_parameters.to;
+	normal = (end - begin).normalized();
+
+	int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+
+	//todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
+
+	bool collided = false;
+	Vector2 res_point, res_normal;
+	int res_shape = -1;
+	const GodotCollisionObject2D *res_obj = nullptr;
+	real_t min_d = 1e10;
+
+	for (int i = 0; i < amount; i++) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
+			continue;
+		}
+
+		if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
+			continue;
+		}
+
+		const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
+
+		int shape_idx = space->intersection_query_subindex_results[i];
+		Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
+
+		Vector2 local_from = inv_xform.xform(begin);
+		Vector2 local_to = inv_xform.xform(end);
+
+		const GodotShape2D *shape = col_obj->get_shape(shape_idx);
+
+		Vector2 shape_point, shape_normal;
+
+		if (shape->contains_point(local_from)) {
+			if (p_parameters.hit_from_inside) {
+				// Hit shape at starting point.
+				min_d = 0;
+				res_point = begin;
+				res_normal = Vector2();
+				res_shape = shape_idx;
+				res_obj = col_obj;
+				collided = true;
+				break;
+			} else {
+				// Ignore shape when starting inside.
+				continue;
+			}
+		}
+
+		if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
+			Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+			shape_point = xform.xform(shape_point);
+
+			real_t ld = normal.dot(shape_point);
+
+			if (ld < min_d) {
+				min_d = ld;
+				res_point = shape_point;
+				res_normal = inv_xform.basis_xform_inv(shape_normal).normalized();
+				res_shape = shape_idx;
+				res_obj = col_obj;
+				collided = true;
+			}
+		}
+	}
+
+	if (!collided) {
+		return false;
+	}
+	ERR_FAIL_NULL_V(res_obj, false); // Shouldn't happen but silences warning.
+
+	r_result.collider_id = res_obj->get_instance_id();
+	if (r_result.collider_id.is_valid()) {
+		r_result.collider = ObjectDB::get_instance(r_result.collider_id);
+	}
+	r_result.normal = res_normal;
+	r_result.position = res_point;
+	r_result.rid = res_obj->get_self();
+	r_result.shape = res_shape;
+
+	return true;
+}
+
+int GodotPhysicsDirectSpaceState2D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
+	if (p_result_max <= 0) {
+		return 0;
+	}
+
+	GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, 0);
+
+	Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
+	aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
+	aabb = aabb.grow(p_parameters.margin);
+
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+
+	int cc = 0;
+
+	for (int i = 0; i < amount; i++) {
+		if (cc >= p_result_max) {
+			break;
+		}
+
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
+			continue;
+		}
+
+		if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
+			continue;
+		}
+
+		const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
+			continue;
+		}
+
+		r_results[cc].collider_id = col_obj->get_instance_id();
+		if (r_results[cc].collider_id.is_valid()) {
+			r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
+		}
+		r_results[cc].rid = col_obj->get_self();
+		r_results[cc].shape = shape_idx;
+
+		cc++;
+	}
+
+	return cc;
+}
+
+bool GodotPhysicsDirectSpaceState2D::cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) {
+	GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, false);
+
+	Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
+	aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
+	aabb = aabb.grow(p_parameters.margin);
+
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+
+	real_t best_safe = 1;
+	real_t best_unsafe = 1;
+
+	for (int i = 0; i < amount; i++) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
+			continue;
+		}
+
+		if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
+			continue; //ignore excluded
+		}
+
+		const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+		//test initial overlap, does it collide if going all the way?
+		if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
+			continue;
+		}
+
+		//test initial overlap, ignore objects it's inside of.
+		if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) {
+			continue;
+		}
+
+		Vector2 mnormal = p_parameters.motion.normalized();
+
+		//just do kinematic solving
+		real_t low = 0.0;
+		real_t hi = 1.0;
+		real_t fraction_coeff = 0.5;
+		for (int j = 0; j < 8; j++) { //steps should be customizable..
+			real_t fraction = low + (hi - low) * fraction_coeff;
+
+			Vector2 sep = mnormal; //important optimization for this to work fast enough
+			bool collided = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_parameters.margin);
+
+			if (collided) {
+				hi = fraction;
+				if ((j == 0) || (low > 0.0)) { // Did it not collide before?
+					// When alternating or first iteration, use dichotomy.
+					fraction_coeff = 0.5;
+				} else {
+					// When colliding again, converge faster towards low fraction
+					// for more accurate results with long motions that collide near the start.
+					fraction_coeff = 0.25;
+				}
+			} else {
+				low = fraction;
+				if ((j == 0) || (hi < 1.0)) { // Did it collide before?
+					// When alternating or first iteration, use dichotomy.
+					fraction_coeff = 0.5;
+				} else {
+					// When not colliding again, converge faster towards high fraction
+					// for more accurate results with long motions that collide near the end.
+					fraction_coeff = 0.75;
+				}
+			}
+		}
+
+		if (low < best_safe) {
+			best_safe = low;
+			best_unsafe = hi;
+		}
+	}
+
+	p_closest_safe = best_safe;
+	p_closest_unsafe = best_unsafe;
+
+	return true;
+}
+
+bool GodotPhysicsDirectSpaceState2D::collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) {
+	if (p_result_max <= 0) {
+		return false;
+	}
+
+	GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, 0);
+
+	Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
+	aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
+	aabb = aabb.grow(p_parameters.margin);
+
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+
+	bool collided = false;
+	r_result_count = 0;
+
+	GodotPhysicsServer2D::CollCbkData cbk;
+	cbk.max = p_result_max;
+	cbk.amount = 0;
+	cbk.passed = 0;
+	cbk.ptr = r_results;
+	GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
+
+	GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
+
+	for (int i = 0; i < amount; i++) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
+			continue;
+		}
+
+		const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
+
+		if (p_parameters.exclude.has(col_obj->get_self())) {
+			continue;
+		}
+
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		cbk.valid_dir = Vector2();
+		cbk.valid_depth = 0;
+
+		if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) {
+			collided = cbk.amount > 0;
+		}
+	}
+
+	r_result_count = cbk.amount;
+
+	return collided;
+}
+
+struct _RestCallbackData2D {
+	const GodotCollisionObject2D *object = nullptr;
+	const GodotCollisionObject2D *best_object = nullptr;
+	int local_shape = 0;
+	int best_local_shape = 0;
+	int shape = 0;
+	int best_shape = 0;
+	Vector2 best_contact;
+	Vector2 best_normal;
+	real_t best_len = 0.0;
+	Vector2 valid_dir;
+	real_t valid_depth = 0.0;
+	real_t min_allowed_depth = 0.0;
+};
+
+static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
+	_RestCallbackData2D *rd = static_cast<_RestCallbackData2D *>(p_userdata);
+
+	Vector2 contact_rel = p_point_B - p_point_A;
+	real_t len = contact_rel.length();
+
+	if (len < rd->min_allowed_depth) {
+		return;
+	}
+
+	if (len <= rd->best_len) {
+		return;
+	}
+
+	Vector2 normal = contact_rel / len;
+
+	if (rd->valid_dir != Vector2()) {
+		if (len > rd->valid_depth) {
+			return;
+		}
+
+		if (rd->valid_dir.dot(normal) > -CMP_EPSILON) {
+			return;
+		}
+	}
+
+	rd->best_len = len;
+	rd->best_contact = p_point_B;
+	rd->best_normal = normal;
+	rd->best_object = rd->object;
+	rd->best_shape = rd->shape;
+	rd->best_local_shape = rd->local_shape;
+}
+
+bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) {
+	GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, 0);
+
+	real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);
+
+	Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
+	aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
+	aabb = aabb.grow(margin);
+
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+
+	_RestCallbackData2D rcd;
+
+	// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
+	real_t motion_length = p_parameters.motion.length();
+	real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+	rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
+
+	for (int i = 0; i < amount; i++) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
+			continue;
+		}
+
+		const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
+
+		if (p_parameters.exclude.has(col_obj->get_self())) {
+			continue;
+		}
+
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		rcd.valid_dir = Vector2();
+		rcd.object = col_obj;
+		rcd.shape = shape_idx;
+		rcd.local_shape = 0;
+		bool sc = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, margin);
+		if (!sc) {
+			continue;
+		}
+	}
+
+	if (rcd.best_len == 0 || !rcd.best_object) {
+		return false;
+	}
+
+	r_info->collider_id = rcd.best_object->get_instance_id();
+	r_info->shape = rcd.best_shape;
+	r_info->normal = rcd.best_normal;
+	r_info->point = rcd.best_contact;
+	r_info->rid = rcd.best_object->get_self();
+	if (rcd.best_object->get_type() == GodotCollisionObject2D::TYPE_BODY) {
+		const GodotBody2D *body = static_cast<const GodotBody2D *>(rcd.best_object);
+		Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass());
+		r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
+
+	} else {
+		r_info->linear_velocity = Vector2();
+	}
+
+	return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int GodotSpace2D::_cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb) {
+	int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
+
+	for (int i = 0; i < amount; i++) {
+		bool keep = true;
+
+		if (intersection_query_results[i] == p_body) {
+			keep = false;
+		} else if (intersection_query_results[i]->get_type() == GodotCollisionObject2D::TYPE_AREA) {
+			keep = false;
+		} else if (!p_body->collides_with(static_cast<GodotBody2D *>(intersection_query_results[i]))) {
+			keep = false;
+		} else if (static_cast<GodotBody2D *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
+			keep = false;
+		}
+
+		if (!keep) {
+			if (i < amount - 1) {
+				SWAP(intersection_query_results[i], intersection_query_results[amount - 1]);
+				SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]);
+			}
+
+			amount--;
+			i--;
+		}
+	}
+
+	return amount;
+}
+
+bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) {
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+	if (r_result) {
+		r_result->collider_id = ObjectID();
+		r_result->collider_shape = 0;
+	}
+
+	Rect2 body_aabb;
+
+	bool shapes_found = false;
+
+	for (int i = 0; i < p_body->get_shape_count(); i++) {
+		if (p_body->is_shape_disabled(i)) {
+			continue;
+		}
+
+		if (!shapes_found) {
+			body_aabb = p_body->get_shape_aabb(i);
+			shapes_found = true;
+		} else {
+			body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
+		}
+	}
+
+	if (!shapes_found) {
+		if (r_result) {
+			*r_result = PhysicsServer2D::MotionResult();
+			r_result->travel = p_parameters.motion;
+		}
+		return false;
+	}
+
+	real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);
+
+	// Undo the currently transform the physics server is aware of and apply the provided one
+	body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb));
+	body_aabb = body_aabb.grow(margin);
+
+	static const int max_excluded_shape_pairs = 32;
+	ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
+	int excluded_shape_pair_count = 0;
+
+	real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
+	real_t motion_length = p_parameters.motion.length();
+	Vector2 motion_normal = p_parameters.motion / motion_length;
+
+	Transform2D body_transform = p_parameters.from;
+
+	bool recovered = false;
+
+	{
+		//STEP 1, FREE BODY IF STUCK
+
+		const int max_results = 32;
+		int recover_attempts = 4;
+		Vector2 sr[max_results * 2];
+		real_t priorities[max_results];
+
+		do {
+			GodotPhysicsServer2D::CollCbkData cbk;
+			cbk.max = max_results;
+			cbk.amount = 0;
+			cbk.passed = 0;
+			cbk.ptr = sr;
+			cbk.invalid_by_dir = 0;
+			excluded_shape_pair_count = 0; //last step is the one valid
+
+			GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
+			GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
+			int priority_amount = 0;
+
+			bool collided = false;
+
+			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_disabled(j)) {
+					continue;
+				}
+
+				GodotShape2D *body_shape = p_body->get_shape(j);
+				Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
+
+				for (int i = 0; i < amount; i++) {
+					const GodotCollisionObject2D *col_obj = intersection_query_results[i];
+					if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
+						continue;
+					}
+					if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
+						continue;
+					}
+
+					int shape_idx = intersection_query_subindex_results[i];
+
+					Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+
+					if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
+						cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
+
+						real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
+						cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
+						cbk.invalid_by_dir = 0;
+
+						if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
+							const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
+							if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
+								//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
+								Vector2 lv = b->get_linear_velocity();
+								//compute displacement from linear velocity
+								Vector2 motion = lv * last_step;
+								real_t motion_len = motion.length();
+								motion.normalize();
+								cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
+							}
+						}
+					} else {
+						cbk.valid_dir = Vector2();
+						cbk.valid_depth = 0;
+						cbk.invalid_by_dir = 0;
+					}
+
+					int current_passed = cbk.passed; //save how many points passed collision
+					bool did_collide = false;
+
+					GodotShape2D *against_shape = col_obj->get_shape(shape_idx);
+					if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) {
+						did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
+					}
+					while (cbk.amount > priority_amount) {
+						priorities[priority_amount] = col_obj->get_collision_priority();
+						priority_amount++;
+					}
+
+					if (!did_collide && cbk.invalid_by_dir > 0) {
+						//this shape must be excluded
+						if (excluded_shape_pair_count < max_excluded_shape_pairs) {
+							ExcludedShapeSW esp;
+							esp.local_shape = body_shape;
+							esp.against_object = col_obj;
+							esp.against_shape_index = shape_idx;
+							excluded_shape_pairs[excluded_shape_pair_count++] = esp;
+						}
+					}
+
+					if (did_collide) {
+						collided = true;
+					}
+				}
+			}
+
+			if (!collided) {
+				break;
+			}
+
+			real_t inv_total_weight = 0.0;
+			for (int i = 0; i < cbk.amount; i++) {
+				inv_total_weight += priorities[i];
+			}
+			inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight;
+
+			recovered = true;
+
+			Vector2 recover_motion;
+			for (int i = 0; i < cbk.amount; i++) {
+				Vector2 a = sr[i * 2 + 0];
+				Vector2 b = sr[i * 2 + 1];
+
+				// Compute plane on b towards a.
+				Vector2 n = (a - b).normalized();
+				real_t d = n.dot(b);
+
+				// Compute depth on recovered motion.
+				real_t depth = n.dot(a + recover_motion) - d;
+				if (depth > min_contact_depth + CMP_EPSILON) {
+					// Only recover if there is penetration.
+					recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight;
+				}
+			}
+
+			if (recover_motion == Vector2()) {
+				collided = false;
+				break;
+			}
+
+			body_transform.columns[2] += recover_motion;
+			body_aabb.position += recover_motion;
+
+			recover_attempts--;
+
+		} while (recover_attempts);
+	}
+
+	real_t safe = 1.0;
+	real_t unsafe = 1.0;
+	int best_shape = -1;
+
+	{
+		// STEP 2 ATTEMPT MOTION
+
+		Rect2 motion_aabb = body_aabb;
+		motion_aabb.position += p_parameters.motion;
+		motion_aabb = motion_aabb.merge(body_aabb);
+
+		int amount = _cull_aabb_for_body(p_body, motion_aabb);
+
+		for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) {
+			if (p_body->is_shape_disabled(body_shape_idx)) {
+				continue;
+			}
+
+			GodotShape2D *body_shape = p_body->get_shape(body_shape_idx);
+
+			// Colliding separation rays allows to properly snap to the ground,
+			// otherwise it's not needed in regular motion.
+			if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) {
+				// When slide on slope is on, separation ray shape acts like a regular shape.
+				if (!static_cast<GodotSeparationRayShape2D *>(body_shape)->get_slide_on_slope()) {
+					continue;
+				}
+			}
+
+			Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx);
+
+			bool stuck = false;
+
+			real_t best_safe = 1;
+			real_t best_unsafe = 1;
+
+			for (int i = 0; i < amount; i++) {
+				const GodotCollisionObject2D *col_obj = intersection_query_results[i];
+				if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
+					continue;
+				}
+				if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
+					continue;
+				}
+
+				int col_shape_idx = intersection_query_subindex_results[i];
+				GodotShape2D *against_shape = col_obj->get_shape(col_shape_idx);
+
+				bool excluded = false;
+
+				for (int k = 0; k < excluded_shape_pair_count; k++) {
+					if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) {
+						excluded = true;
+						break;
+					}
+				}
+
+				if (excluded) {
+					continue;
+				}
+
+				Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx);
+				//test initial overlap, does it collide if going all the way?
+				if (!GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
+					continue;
+				}
+
+				//test initial overlap
+				if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
+					if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
+						Vector2 direction = col_obj_shape_xform.columns[1].normalized();
+						if (motion_normal.dot(direction) < 0) {
+							continue;
+						}
+					}
+
+					stuck = true;
+					break;
+				}
+
+				//just do kinematic solving
+				real_t low = 0.0;
+				real_t hi = 1.0;
+				real_t fraction_coeff = 0.5;
+				for (int k = 0; k < 8; k++) { //steps should be customizable..
+					real_t fraction = low + (hi - low) * fraction_coeff;
+
+					Vector2 sep = motion_normal; //important optimization for this to work fast enough
+					bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
+
+					if (collided) {
+						hi = fraction;
+						if ((k == 0) || (low > 0.0)) { // Did it not collide before?
+							// When alternating or first iteration, use dichotomy.
+							fraction_coeff = 0.5;
+						} else {
+							// When colliding again, converge faster towards low fraction
+							// for more accurate results with long motions that collide near the start.
+							fraction_coeff = 0.25;
+						}
+					} else {
+						low = fraction;
+						if ((k == 0) || (hi < 1.0)) { // Did it collide before?
+							// When alternating or first iteration, use dichotomy.
+							fraction_coeff = 0.5;
+						} else {
+							// When not colliding again, converge faster towards high fraction
+							// for more accurate results with long motions that collide near the end.
+							fraction_coeff = 0.75;
+						}
+					}
+				}
+
+				if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
+					Vector2 cd[2];
+					GodotPhysicsServer2D::CollCbkData cbk;
+					cbk.max = 1;
+					cbk.amount = 0;
+					cbk.passed = 0;
+					cbk.ptr = cd;
+					cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
+
+					cbk.valid_depth = 10e20;
+
+					Vector2 sep = motion_normal; //important optimization for this to work fast enough
+					bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), GodotPhysicsServer2D::_shape_col_cbk, &cbk, &sep, 0);
+					if (!collided || cbk.amount == 0) {
+						continue;
+					}
+				}
+
+				if (low < best_safe) {
+					best_safe = low;
+					best_unsafe = hi;
+				}
+			}
+
+			if (stuck) {
+				safe = 0;
+				unsafe = 0;
+				best_shape = body_shape_idx; //sadly it's the best
+				break;
+			}
+			if (best_safe == 1.0) {
+				continue;
+			}
+			if (best_safe < safe) {
+				safe = best_safe;
+				unsafe = best_unsafe;
+				best_shape = body_shape_idx;
+			}
+		}
+	}
+
+	bool collided = false;
+
+	if ((p_parameters.recovery_as_collision && recovered) || (safe < 1)) {
+		if (safe >= 1) {
+			best_shape = -1; //no best shape with cast, reset to -1
+		}
+
+		//it collided, let's get the rest info in unsafe advance
+		Transform2D ugt = body_transform;
+		ugt.columns[2] += p_parameters.motion * unsafe;
+
+		_RestCallbackData2D rcd;
+
+		// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
+		rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
+
+		body_aabb.position += p_parameters.motion * unsafe;
+		int amount = _cull_aabb_for_body(p_body, body_aabb);
+
+		int from_shape = best_shape != -1 ? best_shape : 0;
+		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_disabled(j)) {
+				continue;
+			}
+
+			Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j);
+			GodotShape2D *body_shape = p_body->get_shape(j);
+
+			for (int i = 0; i < amount; i++) {
+				const GodotCollisionObject2D *col_obj = intersection_query_results[i];
+				if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
+					continue;
+				}
+				if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
+					continue;
+				}
+
+				int shape_idx = intersection_query_subindex_results[i];
+
+				GodotShape2D *against_shape = col_obj->get_shape(shape_idx);
+
+				bool excluded = false;
+				for (int k = 0; k < excluded_shape_pair_count; k++) {
+					if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) {
+						excluded = true;
+						break;
+					}
+				}
+				if (excluded) {
+					continue;
+				}
+
+				Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+
+				if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
+					rcd.valid_dir = col_obj_shape_xform.columns[1].normalized();
+
+					real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
+					rcd.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
+
+					if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
+						const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
+						if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
+							//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
+							Vector2 lv = b->get_linear_velocity();
+							//compute displacement from linear velocity
+							Vector2 motion = lv * last_step;
+							real_t motion_len = motion.length();
+							motion.normalize();
+							rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0);
+						}
+					}
+				} else {
+					rcd.valid_dir = Vector2();
+					rcd.valid_depth = 0;
+				}
+
+				rcd.object = col_obj;
+				rcd.shape = shape_idx;
+				rcd.local_shape = j;
+				bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, margin);
+				if (!sc) {
+					continue;
+				}
+			}
+		}
+
+		if (rcd.best_len != 0) {
+			if (r_result) {
+				r_result->collider = rcd.best_object->get_self();
+				r_result->collider_id = rcd.best_object->get_instance_id();
+				r_result->collider_shape = rcd.best_shape;
+				r_result->collision_local_shape = rcd.best_local_shape;
+				r_result->collision_normal = rcd.best_normal;
+				r_result->collision_point = rcd.best_contact;
+				r_result->collision_depth = rcd.best_len;
+				r_result->collision_safe_fraction = safe;
+				r_result->collision_unsafe_fraction = unsafe;
+
+				const GodotBody2D *body = static_cast<const GodotBody2D *>(rcd.best_object);
+				Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass());
+				r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
+
+				r_result->travel = safe * p_parameters.motion;
+				r_result->remainder = p_parameters.motion - safe * p_parameters.motion;
+				r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin());
+			}
+
+			collided = true;
+		}
+	}
+
+	if (!collided && r_result) {
+		r_result->travel = p_parameters.motion;
+		r_result->remainder = Vector2();
+		r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin());
+	}
+
+	return collided;
+}
+
+// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree.
+void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self) {
+	GodotCollisionObject2D::Type type_A = A->get_type();
+	GodotCollisionObject2D::Type type_B = B->get_type();
+	if (type_A > type_B) {
+		SWAP(A, B);
+		SWAP(p_subindex_A, p_subindex_B);
+		SWAP(type_A, type_B);
+	}
+
+	GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self);
+	self->collision_pairs++;
+
+	if (type_A == GodotCollisionObject2D::TYPE_AREA) {
+		GodotArea2D *area = static_cast<GodotArea2D *>(A);
+		if (type_B == GodotCollisionObject2D::TYPE_AREA) {
+			GodotArea2D *area_b = static_cast<GodotArea2D *>(B);
+			GodotArea2Pair2D *area2_pair = memnew(GodotArea2Pair2D(area_b, p_subindex_B, area, p_subindex_A));
+			return area2_pair;
+		} else {
+			GodotBody2D *body = static_cast<GodotBody2D *>(B);
+			GodotAreaPair2D *area_pair = memnew(GodotAreaPair2D(body, p_subindex_B, area, p_subindex_A));
+			return area_pair;
+		}
+
+	} else {
+		GodotBodyPair2D *b = memnew(GodotBodyPair2D(static_cast<GodotBody2D *>(A), p_subindex_A, static_cast<GodotBody2D *>(B), p_subindex_B));
+		return b;
+	}
+}
+
+void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self) {
+	if (!p_data) {
+		return;
+	}
+
+	GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self);
+	self->collision_pairs--;
+	GodotConstraint2D *c = static_cast<GodotConstraint2D *>(p_data);
+	memdelete(c);
+}
+
+const SelfList<GodotBody2D>::List &GodotSpace2D::get_active_body_list() const {
+	return active_list;
+}
+
+void GodotSpace2D::body_add_to_active_list(SelfList<GodotBody2D> *p_body) {
+	active_list.add(p_body);
+}
+
+void GodotSpace2D::body_remove_from_active_list(SelfList<GodotBody2D> *p_body) {
+	active_list.remove(p_body);
+}
+
+void GodotSpace2D::body_add_to_mass_properties_update_list(SelfList<GodotBody2D> *p_body) {
+	mass_properties_update_list.add(p_body);
+}
+
+void GodotSpace2D::body_remove_from_mass_properties_update_list(SelfList<GodotBody2D> *p_body) {
+	mass_properties_update_list.remove(p_body);
+}
+
+GodotBroadPhase2D *GodotSpace2D::get_broadphase() {
+	return broadphase;
+}
+
+void GodotSpace2D::add_object(GodotCollisionObject2D *p_object) {
+	ERR_FAIL_COND(objects.has(p_object));
+	objects.insert(p_object);
+}
+
+void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) {
+	ERR_FAIL_COND(!objects.has(p_object));
+	objects.erase(p_object);
+}
+
+const HashSet<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const {
+	return objects;
+}
+
+void GodotSpace2D::body_add_to_state_query_list(SelfList<GodotBody2D> *p_body) {
+	state_query_list.add(p_body);
+}
+
+void GodotSpace2D::body_remove_from_state_query_list(SelfList<GodotBody2D> *p_body) {
+	state_query_list.remove(p_body);
+}
+
+void GodotSpace2D::area_add_to_monitor_query_list(SelfList<GodotArea2D> *p_area) {
+	monitor_query_list.add(p_area);
+}
+
+void GodotSpace2D::area_remove_from_monitor_query_list(SelfList<GodotArea2D> *p_area) {
+	monitor_query_list.remove(p_area);
+}
+
+void GodotSpace2D::area_add_to_moved_list(SelfList<GodotArea2D> *p_area) {
+	area_moved_list.add(p_area);
+}
+
+void GodotSpace2D::area_remove_from_moved_list(SelfList<GodotArea2D> *p_area) {
+	area_moved_list.remove(p_area);
+}
+
+const SelfList<GodotArea2D>::List &GodotSpace2D::get_moved_area_list() const {
+	return area_moved_list;
+}
+
+void GodotSpace2D::call_queries() {
+	while (state_query_list.first()) {
+		GodotBody2D *b = state_query_list.first()->self();
+		state_query_list.remove(state_query_list.first());
+		b->call_queries();
+	}
+
+	while (monitor_query_list.first()) {
+		GodotArea2D *a = monitor_query_list.first()->self();
+		monitor_query_list.remove(monitor_query_list.first());
+		a->call_queries();
+	}
+}
+
+void GodotSpace2D::setup() {
+	contact_debug_count = 0;
+
+	while (mass_properties_update_list.first()) {
+		mass_properties_update_list.first()->self()->update_mass_properties();
+		mass_properties_update_list.remove(mass_properties_update_list.first());
+	}
+}
+
+void GodotSpace2D::update() {
+	broadphase->update();
+}
+
+void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
+			contact_recycle_radius = p_value;
+			break;
+		case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
+			contact_max_separation = p_value;
+			break;
+		case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION:
+			contact_max_allowed_penetration = p_value;
+			break;
+		case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS:
+			contact_bias = p_value;
+			break;
+		case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
+			body_linear_velocity_sleep_threshold = p_value;
+			break;
+		case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
+			body_angular_velocity_sleep_threshold = p_value;
+			break;
+		case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP:
+			body_time_to_sleep = p_value;
+			break;
+		case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
+			constraint_bias = p_value;
+			break;
+		case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS:
+			solver_iterations = p_value;
+			break;
+	}
+}
+
+real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
+			return contact_recycle_radius;
+		case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
+			return contact_max_separation;
+		case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION:
+			return contact_max_allowed_penetration;
+		case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS:
+			return contact_bias;
+		case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
+			return body_linear_velocity_sleep_threshold;
+		case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
+			return body_angular_velocity_sleep_threshold;
+		case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP:
+			return body_time_to_sleep;
+		case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
+			return constraint_bias;
+		case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS:
+			return solver_iterations;
+	}
+	return 0;
+}
+
+void GodotSpace2D::lock() {
+	locked = true;
+}
+
+void GodotSpace2D::unlock() {
+	locked = false;
+}
+
+bool GodotSpace2D::is_locked() const {
+	return locked;
+}
+
+GodotPhysicsDirectSpaceState2D *GodotSpace2D::get_direct_state() {
+	return direct_access;
+}
+
+GodotSpace2D::GodotSpace2D() {
+	body_linear_velocity_sleep_threshold = GLOBAL_GET("physics/2d/sleep_threshold_linear");
+	body_angular_velocity_sleep_threshold = GLOBAL_GET("physics/2d/sleep_threshold_angular");
+	body_time_to_sleep = GLOBAL_GET("physics/2d/time_before_sleep");
+	solver_iterations = GLOBAL_GET("physics/2d/solver/solver_iterations");
+	contact_recycle_radius = GLOBAL_GET("physics/2d/solver/contact_recycle_radius");
+	contact_max_separation = GLOBAL_GET("physics/2d/solver/contact_max_separation");
+	contact_max_allowed_penetration = GLOBAL_GET("physics/2d/solver/contact_max_allowed_penetration");
+	contact_bias = GLOBAL_GET("physics/2d/solver/default_contact_bias");
+	constraint_bias = GLOBAL_GET("physics/2d/solver/default_constraint_bias");
+
+	broadphase = GodotBroadPhase2D::create_func();
+	broadphase->set_pair_callback(_broadphase_pair, this);
+	broadphase->set_unpair_callback(_broadphase_unpair, this);
+
+	direct_access = memnew(GodotPhysicsDirectSpaceState2D);
+	direct_access->space = this;
+}
+
+GodotSpace2D::~GodotSpace2D() {
+	memdelete(broadphase);
+	memdelete(direct_access);
+}
diff --git a/modules/godot_physics_2d/godot_space_2d.h b/modules/godot_physics_2d/godot_space_2d.h
new file mode 100644
index 0000000000..ded3b08d5b
--- /dev/null
+++ b/modules/godot_physics_2d/godot_space_2d.h
@@ -0,0 +1,214 @@
+/**************************************************************************/
+/*  godot_space_2d.h                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_SPACE_2D_H
+#define GODOT_SPACE_2D_H
+
+#include "godot_area_2d.h"
+#include "godot_area_pair_2d.h"
+#include "godot_body_2d.h"
+#include "godot_body_pair_2d.h"
+#include "godot_broad_phase_2d.h"
+#include "godot_collision_object_2d.h"
+
+#include "core/config/project_settings.h"
+#include "core/templates/hash_map.h"
+#include "core/typedefs.h"
+
+class GodotPhysicsDirectSpaceState2D : public PhysicsDirectSpaceState2D {
+	GDCLASS(GodotPhysicsDirectSpaceState2D, PhysicsDirectSpaceState2D);
+
+public:
+	GodotSpace2D *space = nullptr;
+
+	virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
+	virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override;
+	virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
+	virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) override;
+	virtual bool collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) override;
+	virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override;
+
+	GodotPhysicsDirectSpaceState2D() {}
+};
+
+class GodotSpace2D {
+public:
+	enum ElapsedTime {
+		ELAPSED_TIME_INTEGRATE_FORCES,
+		ELAPSED_TIME_GENERATE_ISLANDS,
+		ELAPSED_TIME_SETUP_CONSTRAINTS,
+		ELAPSED_TIME_SOLVE_CONSTRAINTS,
+		ELAPSED_TIME_INTEGRATE_VELOCITIES,
+		ELAPSED_TIME_MAX
+
+	};
+
+private:
+	struct ExcludedShapeSW {
+		GodotShape2D *local_shape = nullptr;
+		const GodotCollisionObject2D *against_object = nullptr;
+		int against_shape_index = 0;
+	};
+
+	uint64_t elapsed_time[ELAPSED_TIME_MAX] = {};
+
+	GodotPhysicsDirectSpaceState2D *direct_access = nullptr;
+	RID self;
+
+	GodotBroadPhase2D *broadphase = nullptr;
+	SelfList<GodotBody2D>::List active_list;
+	SelfList<GodotBody2D>::List mass_properties_update_list;
+	SelfList<GodotBody2D>::List state_query_list;
+	SelfList<GodotArea2D>::List monitor_query_list;
+	SelfList<GodotArea2D>::List area_moved_list;
+
+	static void *_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self);
+	static void _broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self);
+
+	HashSet<GodotCollisionObject2D *> objects;
+
+	GodotArea2D *area = nullptr;
+
+	int solver_iterations = 0;
+
+	real_t contact_recycle_radius = 0.0;
+	real_t contact_max_separation = 0.0;
+	real_t contact_max_allowed_penetration = 0.0;
+	real_t contact_bias = 0.0;
+	real_t constraint_bias = 0.0;
+
+	enum {
+		INTERSECTION_QUERY_MAX = 2048
+	};
+
+	GodotCollisionObject2D *intersection_query_results[INTERSECTION_QUERY_MAX];
+	int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
+
+	real_t body_linear_velocity_sleep_threshold = 0.0;
+	real_t body_angular_velocity_sleep_threshold = 0.0;
+	real_t body_time_to_sleep = 0.0;
+
+	bool locked = false;
+
+	real_t last_step = 0.001;
+
+	int island_count = 0;
+	int active_objects = 0;
+	int collision_pairs = 0;
+
+	int _cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb);
+
+	Vector<Vector2> contact_debug;
+	int contact_debug_count = 0;
+
+	friend class GodotPhysicsDirectSpaceState2D;
+
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	void set_default_area(GodotArea2D *p_area) { area = p_area; }
+	GodotArea2D *get_default_area() const { return area; }
+
+	const SelfList<GodotBody2D>::List &get_active_body_list() const;
+	void body_add_to_active_list(SelfList<GodotBody2D> *p_body);
+	void body_remove_from_active_list(SelfList<GodotBody2D> *p_body);
+	void body_add_to_mass_properties_update_list(SelfList<GodotBody2D> *p_body);
+	void body_remove_from_mass_properties_update_list(SelfList<GodotBody2D> *p_body);
+	void area_add_to_moved_list(SelfList<GodotArea2D> *p_area);
+	void area_remove_from_moved_list(SelfList<GodotArea2D> *p_area);
+	const SelfList<GodotArea2D>::List &get_moved_area_list() const;
+
+	void body_add_to_state_query_list(SelfList<GodotBody2D> *p_body);
+	void body_remove_from_state_query_list(SelfList<GodotBody2D> *p_body);
+
+	void area_add_to_monitor_query_list(SelfList<GodotArea2D> *p_area);
+	void area_remove_from_monitor_query_list(SelfList<GodotArea2D> *p_area);
+
+	GodotBroadPhase2D *get_broadphase();
+
+	void add_object(GodotCollisionObject2D *p_object);
+	void remove_object(GodotCollisionObject2D *p_object);
+	const HashSet<GodotCollisionObject2D *> &get_objects() const;
+
+	_FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; }
+	_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
+	_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
+	_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
+	_FORCE_INLINE_ real_t get_contact_bias() const { return contact_bias; }
+	_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
+	_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; }
+	_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; }
+	_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
+
+	void update();
+	void setup();
+	void call_queries();
+
+	bool is_locked() const;
+	void lock();
+	void unlock();
+
+	real_t get_last_step() const { return last_step; }
+	void set_last_step(real_t p_step) { last_step = p_step; }
+
+	void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value);
+	real_t get_param(PhysicsServer2D::SpaceParameter p_param) const;
+
+	void set_island_count(int p_island_count) { island_count = p_island_count; }
+	int get_island_count() const { return island_count; }
+
+	void set_active_objects(int p_active_objects) { active_objects = p_active_objects; }
+	int get_active_objects() const { return active_objects; }
+
+	int get_collision_pairs() const { return collision_pairs; }
+
+	bool test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result);
+
+	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }
+	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); }
+	_FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) {
+		if (contact_debug_count < contact_debug.size()) {
+			contact_debug.write[contact_debug_count++] = p_contact;
+		}
+	}
+	_FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; }
+	_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
+
+	GodotPhysicsDirectSpaceState2D *get_direct_state();
+
+	void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; }
+	uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
+
+	GodotSpace2D();
+	~GodotSpace2D();
+};
+
+#endif // GODOT_SPACE_2D_H
diff --git a/modules/godot_physics_2d/godot_step_2d.cpp b/modules/godot_physics_2d/godot_step_2d.cpp
new file mode 100644
index 0000000000..bbaec8be2b
--- /dev/null
+++ b/modules/godot_physics_2d/godot_step_2d.cpp
@@ -0,0 +1,306 @@
+/**************************************************************************/
+/*  godot_step_2d.cpp                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_step_2d.h"
+
+#include "core/object/worker_thread_pool.h"
+#include "core/os/os.h"
+
+#define BODY_ISLAND_COUNT_RESERVE 128
+#define BODY_ISLAND_SIZE_RESERVE 512
+#define ISLAND_COUNT_RESERVE 128
+#define ISLAND_SIZE_RESERVE 512
+#define CONSTRAINT_COUNT_RESERVE 1024
+
+void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island) {
+	p_body->set_island_step(_step);
+
+	if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
+		// Only rigid bodies are tested for activation.
+		p_body_island.push_back(p_body);
+	}
+
+	for (const Pair<GodotConstraint2D *, int> &E : p_body->get_constraint_list()) {
+		GodotConstraint2D *constraint = const_cast<GodotConstraint2D *>(E.first);
+		if (constraint->get_island_step() == _step) {
+			continue; // Already processed.
+		}
+		constraint->set_island_step(_step);
+		p_constraint_island.push_back(constraint);
+		all_constraints.push_back(constraint);
+
+		for (int i = 0; i < constraint->get_body_count(); i++) {
+			if (i == E.second) {
+				continue;
+			}
+			GodotBody2D *other_body = constraint->get_body_ptr()[i];
+			if (other_body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) {
+				continue; // Static bodies don't connect islands.
+			}
+			_populate_island(other_body, p_body_island, p_constraint_island);
+		}
+	}
+}
+
+void GodotStep2D::_setup_constraint(uint32_t p_constraint_index, void *p_userdata) {
+	GodotConstraint2D *constraint = all_constraints[p_constraint_index];
+	constraint->setup(delta);
+}
+
+void GodotStep2D::_pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const {
+	uint32_t constraint_count = p_constraint_island.size();
+	uint32_t valid_constraint_count = 0;
+	for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+		GodotConstraint2D *constraint = p_constraint_island[constraint_index];
+		if (p_constraint_island[constraint_index]->pre_solve(delta)) {
+			// Keep this constraint for solving.
+			p_constraint_island[valid_constraint_count++] = constraint;
+		}
+	}
+	p_constraint_island.resize(valid_constraint_count);
+}
+
+void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const {
+	const LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[p_island_index];
+
+	for (int i = 0; i < iterations; i++) {
+		uint32_t constraint_count = constraint_island.size();
+		for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+			constraint_island[constraint_index]->solve(delta);
+		}
+	}
+}
+
+void GodotStep2D::_check_suspend(LocalVector<GodotBody2D *> &p_body_island) const {
+	bool can_sleep = true;
+
+	uint32_t body_count = p_body_island.size();
+	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
+		GodotBody2D *body = p_body_island[body_index];
+
+		if (!body->sleep_test(delta)) {
+			can_sleep = false;
+		}
+	}
+
+	// Put all to sleep or wake up everyone.
+	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
+		GodotBody2D *body = p_body_island[body_index];
+
+		bool active = body->is_active();
+
+		if (active == can_sleep) {
+			body->set_active(!can_sleep);
+		}
+	}
+}
+
+void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
+	p_space->lock(); // can't access space during this
+
+	p_space->setup(); //update inertias, etc
+
+	p_space->set_last_step(p_delta);
+
+	iterations = p_space->get_solver_iterations();
+	delta = p_delta;
+
+	const SelfList<GodotBody2D>::List *body_list = &p_space->get_active_body_list();
+
+	/* INTEGRATE FORCES */
+
+	uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
+	uint64_t profile_endtime = 0;
+
+	int active_count = 0;
+
+	const SelfList<GodotBody2D> *b = body_list->first();
+	while (b) {
+		b->self()->integrate_forces(p_delta);
+		b = b->next();
+		active_count++;
+	}
+
+	p_space->set_active_objects(active_count);
+
+	// Update the broadphase to register collision pairs.
+	p_space->update();
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
+	}
+
+	/* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
+
+	uint32_t island_count = 0;
+
+	const SelfList<GodotArea2D>::List &aml = p_space->get_moved_area_list();
+
+	while (aml.first()) {
+		for (GodotConstraint2D *E : aml.first()->self()->get_constraints()) {
+			GodotConstraint2D *constraint = E;
+			if (constraint->get_island_step() == _step) {
+				continue;
+			}
+			constraint->set_island_step(_step);
+
+			// Each constraint can be on a separate island for areas as there's no solving phase.
+			++island_count;
+			if (constraint_islands.size() < island_count) {
+				constraint_islands.resize(island_count);
+			}
+			LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+
+			all_constraints.push_back(constraint);
+			constraint_island.push_back(constraint);
+		}
+		p_space->area_remove_from_moved_list((SelfList<GodotArea2D> *)aml.first()); //faster to remove here
+	}
+
+	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
+
+	b = body_list->first();
+
+	uint32_t body_island_count = 0;
+
+	while (b) {
+		GodotBody2D *body = b->self();
+
+		if (body->get_island_step() != _step) {
+			++body_island_count;
+			if (body_islands.size() < body_island_count) {
+				body_islands.resize(body_island_count);
+			}
+			LocalVector<GodotBody2D *> &body_island = body_islands[body_island_count - 1];
+			body_island.clear();
+			body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
+
+			++island_count;
+			if (constraint_islands.size() < island_count) {
+				constraint_islands.resize(island_count);
+			}
+			LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+			constraint_island.reserve(ISLAND_SIZE_RESERVE);
+
+			_populate_island(body, body_island, constraint_island);
+
+			if (body_island.is_empty()) {
+				--body_island_count;
+			}
+
+			if (constraint_island.is_empty()) {
+				--island_count;
+			}
+		}
+		b = b->next();
+	}
+
+	p_space->set_island_count((int)island_count);
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
+	}
+
+	/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
+
+	uint32_t total_constraint_count = all_constraints.size();
+	WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_constraint, nullptr, total_constraint_count, -1, true, SNAME("Physics2DConstraintSetup"));
+	WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
+	}
+
+	/* PRE-SOLVE CONSTRAINT ISLANDS */
+
+	// Warning: This doesn't run on threads, because it involves thread-unsafe processing.
+	for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
+		_pre_solve_island(constraint_islands[island_index]);
+	}
+
+	/* SOLVE CONSTRAINT ISLANDS */
+
+	// Warning: _solve_island modifies the constraint islands for optimization purpose,
+	// their content is not reliable after these calls and shouldn't be used anymore.
+	group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics2DConstraintSolveIslands"));
+	WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
+	}
+
+	/* INTEGRATE VELOCITIES */
+
+	b = body_list->first();
+	while (b) {
+		const SelfList<GodotBody2D> *n = b->next();
+		b->self()->integrate_velocities(p_delta);
+		b = n; // in case it shuts itself down
+	}
+
+	/* SLEEP / WAKE UP ISLANDS */
+
+	for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
+		_check_suspend(body_islands[island_index]);
+	}
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
+		//profile_begtime=profile_endtime;
+	}
+
+	all_constraints.clear();
+
+	p_space->unlock();
+	_step++;
+}
+
+GodotStep2D::GodotStep2D() {
+	body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
+	constraint_islands.reserve(ISLAND_COUNT_RESERVE);
+	all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
+}
+
+GodotStep2D::~GodotStep2D() {
+}
diff --git a/modules/godot_physics_2d/godot_step_2d.h b/modules/godot_physics_2d/godot_step_2d.h
new file mode 100644
index 0000000000..c08c6379de
--- /dev/null
+++ b/modules/godot_physics_2d/godot_step_2d.h
@@ -0,0 +1,60 @@
+/**************************************************************************/
+/*  godot_step_2d.h                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_STEP_2D_H
+#define GODOT_STEP_2D_H
+
+#include "godot_space_2d.h"
+
+#include "core/templates/local_vector.h"
+
+class GodotStep2D {
+	uint64_t _step = 1;
+
+	int iterations = 0;
+	real_t delta = 0.0;
+
+	LocalVector<LocalVector<GodotBody2D *>> body_islands;
+	LocalVector<LocalVector<GodotConstraint2D *>> constraint_islands;
+	LocalVector<GodotConstraint2D *> all_constraints;
+
+	void _populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island);
+	void _setup_constraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
+	void _pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const;
+	void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
+	void _check_suspend(LocalVector<GodotBody2D *> &p_body_island) const;
+
+public:
+	void step(GodotSpace2D *p_space, real_t p_delta);
+	GodotStep2D();
+	~GodotStep2D();
+};
+
+#endif // GODOT_STEP_2D_H
diff --git a/modules/godot_physics_2d/register_types.cpp b/modules/godot_physics_2d/register_types.cpp
new file mode 100644
index 0000000000..57422b1814
--- /dev/null
+++ b/modules/godot_physics_2d/register_types.cpp
@@ -0,0 +1,61 @@
+/**************************************************************************/
+/*  register_types.cpp                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "register_types.h"
+
+#include "godot_physics_server_2d.h"
+#include "servers/physics_server_2d.h"
+#include "servers/physics_server_2d_wrap_mt.h"
+
+static PhysicsServer2D *_createGodotPhysics2DCallback() {
+#ifdef THREADS_ENABLED
+	bool using_threads = GLOBAL_GET("physics/2d/run_on_separate_thread");
+#else
+	bool using_threads = false;
+#endif
+
+	PhysicsServer2D *physics_server_2d = memnew(GodotPhysicsServer2D(using_threads));
+
+	return memnew(PhysicsServer2DWrapMT(physics_server_2d, using_threads));
+}
+
+void initialize_godot_physics_2d_module(ModuleInitializationLevel p_level) {
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		return;
+	}
+	PhysicsServer2DManager::get_singleton()->register_server("GodotPhysics2D", callable_mp_static(_createGodotPhysics2DCallback));
+	PhysicsServer2DManager::get_singleton()->set_default_server("GodotPhysics2D");
+}
+
+void uninitialize_godot_physics_2d_module(ModuleInitializationLevel p_level) {
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		return;
+	}
+}
diff --git a/modules/godot_physics_2d/register_types.h b/modules/godot_physics_2d/register_types.h
new file mode 100644
index 0000000000..1d2d1301b9
--- /dev/null
+++ b/modules/godot_physics_2d/register_types.h
@@ -0,0 +1,39 @@
+/**************************************************************************/
+/*  register_types.h                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef GODOT_PHYSICS_2D_REGISTER_TYPES_H
+#define GODOT_PHYSICS_2D_REGISTER_TYPES_H
+
+#include "modules/register_module_types.h"
+
+void initialize_godot_physics_2d_module(ModuleInitializationLevel p_level);
+void uninitialize_godot_physics_2d_module(ModuleInitializationLevel p_level);
+
+#endif // GODOT_PHYSICS_2D_REGISTER_TYPES_H