Merge pull request #95261 from rburing/2d_physics_module

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

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