initial commit, 4.5 stable

modules/godot_physics_2d/SCsub

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+from misc.utility.scons_hints import *
+
+Import("env")
+
+env.add_source_files(env.modules_sources, "*.cpp")

modules/godot_physics_2d/config.py

@@ -0,0 +1,6 @@
+def can_build(env, platform):
+    return not env["disable_physics_2d"]
+
+
+def configure(env):
+    pass

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() {
+}

modules/godot_physics_2d/godot_area_2d.h

@@ -0,0 +1,188 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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();
+	}
+}

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

modules/godot_physics_2d/godot_area_pair_2d.h

@@ -0,0 +1,75 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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();
+};

modules/godot_physics_2d/godot_body_2d.cpp

@@ -0,0 +1,763 @@
+/**************************************************************************/
+/*  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_constraint_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 + std::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.is_empty() && 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);
+	}
+}

modules/godot_physics_2d/godot_body_2d.h

@@ -0,0 +1,387 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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) {
+		ERR_FAIL_INDEX(p_size, MAX_CONTACTS_REPORTED_2D_MAX);
+		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;
+}

modules/godot_physics_2d/godot_body_direct_state_2d.cpp

@@ -0,0 +1,244 @@
+/**************************************************************************/
+/*  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_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();
+}
+
+void GodotPhysicsDirectBodyState2D::set_collision_layer(uint32_t p_layer) {
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t GodotPhysicsDirectBodyState2D::get_collision_layer() const {
+	return body->get_collision_layer();
+}
+
+void GodotPhysicsDirectBodyState2D::set_collision_mask(uint32_t p_mask) {
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t GodotPhysicsDirectBodyState2D::get_collision_mask() const {
+	return body->get_collision_mask();
+}
+
+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();
+}

modules/godot_physics_2d/godot_body_direct_state_2d.h

@@ -0,0 +1,107 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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 void set_collision_layer(uint32_t p_layer) override;
+	virtual uint32_t get_collision_layer() const override;
+
+	virtual void set_collision_mask(uint32_t p_mask) override;
+	virtual uint32_t get_collision_mask() 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;
+};

modules/godot_physics_2d/godot_body_pair_2d.cpp

@@ -0,0 +1,610 @@
+/**************************************************************************/
+/*  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 Math::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);
+}

modules/godot_physics_2d/godot_body_pair_2d.h

@@ -0,0 +1,98 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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();
+};

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() {
+}

modules/godot_physics_2d/godot_broad_phase_2d.h

@@ -0,0 +1,68 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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();
+};

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

modules/godot_physics_2d/godot_broad_phase_2d_bvh.h

@@ -0,0 +1,98 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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();
+};

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;
+}

modules/godot_physics_2d/godot_collision_object_2d.h

@@ -0,0 +1,195 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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() {}
+};

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

modules/godot_physics_2d/godot_collision_solver_2d.h

@@ -0,0 +1,47 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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);
+};

modules/godot_physics_2d/godot_collision_solver_2d_sat.h

@@ -0,0 +1,35 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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);

modules/godot_physics_2d/godot_constraint_2d.h

@@ -0,0 +1,67 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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() {}
+};

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(false);
+}
+
+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 influence 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);
+}

modules/godot_physics_2d/godot_joints_2d.h

@@ -0,0 +1,189 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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);
+};

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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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<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{ 65536, 1048576 };
+	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_step(RID p_space, real_t p_delta) override;
+	virtual void space_flush_queries(RID p_space) 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;
+
+	virtual int space_get_last_process_info(RID p_space, ProcessInfo p_info) override;
+	GodotPhysicsServer2D(bool p_using_threads = false);
+	~GodotPhysicsServer2D() {}
+};

modules/godot_physics_2d/godot_shape_2d.cpp

@@ -0,0 +1,984 @@
+/**************************************************************************/
+/*  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) {
+		E.key->_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.is_empty() || points.is_empty()) {
+		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.is_empty() || points.is_empty() || bvh.is_empty()) {
+		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;
+		}
+	}
+}

modules/godot_physics_2d/godot_shape_2d.h

@@ -0,0 +1,536 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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

modules/godot_physics_2d/godot_space_2d.h

@@ -0,0 +1,207 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "godot_area_2d.h"
+#include "godot_body_2d.h"
+#include "godot_broad_phase_2d.h"
+#include "godot_collision_object_2d.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();
+};

modules/godot_physics_2d/godot_step_2d.cpp

@@ -0,0 +1,307 @@
+/**************************************************************************/
+/*  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"
+#include "godot_constraint_2d.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 = 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() {
+}

modules/godot_physics_2d/godot_step_2d.h

@@ -0,0 +1,57 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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();
+};

modules/godot_physics_2d/register_types.cpp

@@ -0,0 +1,62 @@
+/**************************************************************************/
+/*  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 "core/config/project_settings.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;
+	}
+}

modules/godot_physics_2d/register_types.h

@@ -0,0 +1,36 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "modules/register_module_types.h"
+
+void initialize_godot_physics_2d_module(ModuleInitializationLevel p_level);
+void uninitialize_godot_physics_2d_module(ModuleInitializationLevel p_level);