initial commit, 4.5 stable

modules/godot_physics_3d/SCsub

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

modules/godot_physics_3d/config.py

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

modules/godot_physics_3d/gjk_epa.h

@@ -0,0 +1,37 @@
+/**************************************************************************/
+/*  gjk_epa.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_3d.h"
+#include "godot_shape_3d.h"
+
+bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0);
+bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);

modules/godot_physics_3d/godot_area_3d.cpp

@@ -0,0 +1,346 @@
+/**************************************************************************/
+/*  godot_area_3d.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_3d.h"
+
+#include "godot_body_3d.h"
+#include "godot_soft_body_3d.h"
+#include "godot_space_3d.h"
+
+GodotArea3D::BodyKey::BodyKey(GodotSoftBody3D *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;
+}
+
+GodotArea3D::BodyKey::BodyKey(GodotBody3D *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;
+}
+
+GodotArea3D::BodyKey::BodyKey(GodotArea3D *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 GodotArea3D::_shapes_changed() {
+	if (!moved_list.in_list() && get_space()) {
+		get_space()->area_add_to_moved_list(&moved_list);
+	}
+}
+
+void GodotArea3D::set_transform(const Transform3D &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 GodotArea3D::set_space(GodotSpace3D *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 GodotArea3D::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 GodotArea3D::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 GodotArea3D::_set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode) {
+	bool do_override = p_new_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override == (r_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) {
+		return;
+	}
+	_unregister_shapes();
+	r_mode = p_new_mode;
+	_shape_changed();
+}
+
+void GodotArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE:
+			_set_space_override_mode(gravity_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value);
+			break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY:
+			gravity = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR:
+			gravity_vector = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT:
+			gravity_is_point = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE:
+			gravity_point_unit_distance = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE:
+			_set_space_override_mode(linear_damping_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value);
+			break;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
+			linear_damp = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE:
+			_set_space_override_mode(angular_damping_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value);
+			break;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
+			angular_damp = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_PRIORITY:
+			priority = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE:
+			ERR_FAIL_COND_MSG(wind_force_magnitude < 0, "Wind force magnitude must be a non-negative real number, but a negative number was specified.");
+			wind_force_magnitude = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_WIND_SOURCE:
+			wind_source = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION:
+			wind_direction = p_value;
+			break;
+		case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR:
+			ERR_FAIL_COND_MSG(wind_attenuation_factor < 0, "Wind attenuation factor must be a non-negative real number, but a negative number was specified.");
+			wind_attenuation_factor = p_value;
+			break;
+	}
+}
+
+Variant GodotArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE:
+			return gravity_override_mode;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY:
+			return gravity;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR:
+			return gravity_vector;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT:
+			return gravity_is_point;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE:
+			return gravity_point_unit_distance;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE:
+			return linear_damping_override_mode;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
+			return linear_damp;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE:
+			return angular_damping_override_mode;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
+			return angular_damp;
+		case PhysicsServer3D::AREA_PARAM_PRIORITY:
+			return priority;
+		case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE:
+			return wind_force_magnitude;
+		case PhysicsServer3D::AREA_PARAM_WIND_SOURCE:
+			return wind_source;
+		case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION:
+			return wind_direction;
+		case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR:
+			return wind_attenuation_factor;
+	}
+
+	return Variant();
+}
+
+void GodotArea3D::_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 GodotArea3D::set_monitorable(bool p_monitorable) {
+	if (monitorable == p_monitorable) {
+		return;
+	}
+
+	monitorable = p_monitorable;
+	_set_static(!monitorable);
+	_shapes_changed();
+}
+
+void GodotArea3D::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 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::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 monitor 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 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::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 area monitor callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce));
+				}
+			}
+		} else {
+			monitored_areas.clear();
+			area_monitor_callback = Callable();
+		}
+	}
+}
+
+void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const {
+	if (is_gravity_point()) {
+		const real_t gr_unit_dist = get_gravity_point_unit_distance();
+		Vector3 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 = Vector3();
+			}
+		} else {
+			r_gravity = v.normalized() * get_gravity();
+		}
+	} else {
+		r_gravity = get_gravity_vector() * get_gravity();
+	}
+}
+
+GodotArea3D::GodotArea3D() :
+		GodotCollisionObject3D(TYPE_AREA),
+		monitor_query_list(this),
+		moved_list(this) {
+	_set_static(true); //areas are never active
+	set_ray_pickable(false);
+}
+
+GodotArea3D::~GodotArea3D() {
+}

modules/godot_physics_3d/godot_area_3d.h

@@ -0,0 +1,237 @@
+/**************************************************************************/
+/*  godot_area_3d.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_3d.h"
+
+#include "core/templates/self_list.h"
+#include "servers/physics_server_3d.h"
+
+class GodotSpace3D;
+class GodotBody3D;
+class GodotSoftBody3D;
+class GodotConstraint3D;
+
+class GodotArea3D : public GodotCollisionObject3D {
+	PhysicsServer3D::AreaSpaceOverrideMode gravity_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	PhysicsServer3D::AreaSpaceOverrideMode linear_damping_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	PhysicsServer3D::AreaSpaceOverrideMode angular_damping_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+
+	real_t gravity = 9.80665;
+	Vector3 gravity_vector = Vector3(0, -1, 0);
+	bool gravity_is_point = false;
+	real_t gravity_point_unit_distance = 0.0;
+	real_t linear_damp = 0.1;
+	real_t angular_damp = 0.1;
+	real_t wind_force_magnitude = 0.0;
+	real_t wind_attenuation_factor = 0.0;
+	Vector3 wind_source;
+	Vector3 wind_direction;
+	int priority = 0;
+	bool monitorable = false;
+
+	Callable monitor_callback;
+	Callable area_monitor_callback;
+
+	SelfList<GodotArea3D> monitor_query_list;
+	SelfList<GodotArea3D> 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(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+		BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+		BodyKey(GodotArea3D *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_soft_bodies;
+	HashMap<BodyKey, BodyState, BodyKey> monitored_bodies;
+	HashMap<BodyKey, BodyState, BodyKey> monitored_areas;
+
+	HashSet<GodotConstraint3D *> constraints;
+
+	virtual void _shapes_changed() override;
+	void _queue_monitor_update();
+
+	void _set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::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(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+
+	_FORCE_INLINE_ void add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
+
+	_FORCE_INLINE_ void add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
+	_FORCE_INLINE_ void remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
+
+	void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value);
+	Variant get_param(PhysicsServer3D::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 Vector3 &p_gravity) { gravity_vector = p_gravity; }
+	_FORCE_INLINE_ Vector3 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 set_wind_force_magnitude(real_t p_wind_force_magnitude) { wind_force_magnitude = p_wind_force_magnitude; }
+	_FORCE_INLINE_ real_t get_wind_force_magnitude() const { return wind_force_magnitude; }
+
+	_FORCE_INLINE_ void set_wind_attenuation_factor(real_t p_wind_attenuation_factor) { wind_attenuation_factor = p_wind_attenuation_factor; }
+	_FORCE_INLINE_ real_t get_wind_attenuation_factor() const { return wind_attenuation_factor; }
+
+	_FORCE_INLINE_ void set_wind_source(const Vector3 &p_wind_source) { wind_source = p_wind_source; }
+	_FORCE_INLINE_ const Vector3 &get_wind_source() const { return wind_source; }
+
+	_FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; }
+	_FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; }
+
+	_FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const HashSet<GodotConstraint3D *> &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 Transform3D &p_transform);
+
+	void set_space(GodotSpace3D *p_space) override;
+
+	void call_queries();
+
+	void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const;
+
+	GodotArea3D();
+	~GodotArea3D();
+};
+
+void GodotArea3D::add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
+	BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
+	monitored_soft_bodies[bk].inc();
+	if (!monitor_query_list.in_list()) {
+		_queue_monitor_update();
+	}
+}
+
+void GodotArea3D::remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
+	BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
+	monitored_soft_bodies[bk].dec();
+	if (get_space() && !monitor_query_list.in_list()) {
+		_queue_monitor_update();
+	}
+}
+
+void GodotArea3D::add_body_to_query(GodotBody3D *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 GodotArea3D::remove_body_from_query(GodotBody3D *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 GodotArea3D::add_area_to_query(GodotArea3D *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 GodotArea3D::remove_area_from_query(GodotArea3D *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();
+	}
+}
+
+struct AreaCMP {
+	GodotArea3D *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(GodotArea3D *p_area) {
+		area = p_area;
+		refCount = 1;
+	}
+};

modules/godot_physics_3d/godot_area_pair_3d.cpp

@@ -0,0 +1,294 @@
+/**************************************************************************/
+/*  godot_area_pair_3d.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_3d.h"
+
+#include "godot_collision_solver_3d.h"
+
+bool GodotAreaPair3D::setup(real_t p_step) {
+	bool result = false;
+	if (area->collides_with(body) && GodotCollisionSolver3D::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) {
+		result = true;
+	}
+
+	process_collision = false;
+	has_space_override = false;
+	if (result != colliding) {
+		if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		} else if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		} else if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE) != PhysicsServer3D::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 GodotAreaPair3D::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 GodotAreaPair3D::solve(real_t p_step) {
+	// Nothing to do.
+}
+
+GodotAreaPair3D::GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *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() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		p_body->set_active(true);
+	}
+}
+
+GodotAreaPair3D::~GodotAreaPair3D() {
+	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);
+	area->remove_constraint(this);
+}
+
+////////////////////////////////////////////////////
+
+bool GodotArea2Pair3D::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) && !GodotCollisionSolver3D::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), 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 GodotArea2Pair3D::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 GodotArea2Pair3D::solve(real_t p_step) {
+	// Nothing to do.
+}
+
+GodotArea2Pair3D::GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *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);
+}
+
+GodotArea2Pair3D::~GodotArea2Pair3D() {
+	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);
+}
+
+////////////////////////////////////////////////////
+
+bool GodotAreaSoftBodyPair3D::setup(real_t p_step) {
+	bool result = false;
+	if (
+			area->collides_with(soft_body) &&
+			GodotCollisionSolver3D::solve_static(
+					soft_body->get_shape(soft_body_shape),
+					soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape),
+					area->get_shape(area_shape),
+					area->get_transform() * area->get_shape_transform(area_shape),
+					nullptr,
+					this)) {
+		result = true;
+	}
+
+	process_collision = false;
+	has_space_override = false;
+	if (result != colliding) {
+		if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		} else if (area->get_wind_force_magnitude() > CMP_EPSILON) {
+			has_space_override = true;
+		}
+
+		if (area->has_monitor_callback()) {
+			process_collision = true;
+		}
+
+		colliding = result;
+	}
+
+	return process_collision;
+}
+
+bool GodotAreaSoftBodyPair3D::pre_solve(real_t p_step) {
+	if (!process_collision) {
+		return false;
+	}
+
+	if (colliding) {
+		if (has_space_override) {
+			body_has_attached_area = true;
+			soft_body->add_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape);
+		}
+	} else {
+		if (has_space_override) {
+			body_has_attached_area = false;
+			soft_body->remove_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape);
+		}
+	}
+
+	return false; // Never do any post solving.
+}
+
+void GodotAreaSoftBodyPair3D::solve(real_t p_step) {
+	// Nothing to do.
+}
+
+GodotAreaSoftBodyPair3D::GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_soft_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape) {
+	soft_body = p_soft_body;
+	area = p_area;
+	soft_body_shape = p_soft_body_shape;
+	area_shape = p_area_shape;
+	soft_body->add_constraint(this);
+	area->add_constraint(this);
+}
+
+GodotAreaSoftBodyPair3D::~GodotAreaSoftBodyPair3D() {
+	if (colliding) {
+		if (body_has_attached_area) {
+			body_has_attached_area = false;
+			soft_body->remove_area(area);
+		}
+		if (area->has_monitor_callback()) {
+			area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape);
+		}
+	}
+	soft_body->remove_constraint(this);
+	area->remove_constraint(this);
+}

modules/godot_physics_3d/godot_area_pair_3d.h

@@ -0,0 +1,95 @@
+/**************************************************************************/
+/*  godot_area_pair_3d.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_3d.h"
+#include "godot_body_3d.h"
+#include "godot_constraint_3d.h"
+#include "godot_soft_body_3d.h"
+
+class GodotAreaPair3D : public GodotConstraint3D {
+	GodotBody3D *body = nullptr;
+	GodotArea3D *area = nullptr;
+	int body_shape;
+	int area_shape;
+	bool colliding = false;
+	bool process_collision = false;
+	bool has_space_override = 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;
+
+	GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape);
+	~GodotAreaPair3D();
+};
+
+class GodotArea2Pair3D : public GodotConstraint3D {
+	GodotArea3D *area_a = nullptr;
+	GodotArea3D *area_b = nullptr;
+	int shape_a;
+	int shape_b;
+	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;
+
+	GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b);
+	~GodotArea2Pair3D();
+};
+
+class GodotAreaSoftBodyPair3D : public GodotConstraint3D {
+	GodotSoftBody3D *soft_body = nullptr;
+	GodotArea3D *area = nullptr;
+	int soft_body_shape;
+	int area_shape;
+	bool colliding = false;
+	bool process_collision = false;
+	bool has_space_override = 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;
+
+	GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_sof_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape);
+	~GodotAreaSoftBodyPair3D();
+};

modules/godot_physics_3d/godot_body_3d.cpp

@@ -0,0 +1,842 @@
+/**************************************************************************/
+/*  godot_body_3d.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_3d.h"
+
+#include "godot_area_3d.h"
+#include "godot_body_direct_state_3d.h"
+#include "godot_constraint_3d.h"
+#include "godot_space_3d.h"
+
+void GodotBody3D::_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 GodotBody3D::_update_transform_dependent() {
+	center_of_mass = get_transform().basis.xform(center_of_mass_local);
+	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
+
+	// Update inertia tensor.
+	Basis tb = principal_inertia_axes;
+	Basis tbt = tb.transposed();
+	Basis diag;
+	diag.scale(_inv_inertia);
+	_inv_inertia_tensor = tb * diag * tbt;
+}
+
+void GodotBody3D::update_mass_properties() {
+	// Update shapes and motions.
+
+	switch (mode) {
+		case PhysicsServer3D::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_area(i);
+			}
+
+			if (calculate_center_of_mass) {
+				// We have to recompute the center of mass.
+				center_of_mass_local.zero();
+
+				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_area(i);
+
+						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).origin;
+					}
+
+					center_of_mass_local /= mass;
+				}
+			}
+
+			if (calculate_inertia) {
+				// Recompute the inertia tensor.
+				Basis inertia_tensor;
+				inertia_tensor.set_zero();
+				bool inertia_set = false;
+
+				for (int i = 0; i < get_shape_count(); i++) {
+					if (is_shape_disabled(i)) {
+						continue;
+					}
+
+					real_t area = get_shape_area(i);
+					if (area == 0.0) {
+						continue;
+					}
+
+					inertia_set = true;
+
+					const GodotShape3D *shape = get_shape(i);
+
+					real_t mass_new = area * mass / total_area;
+
+					Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass_new));
+					Transform3D shape_transform = get_shape_transform(i);
+					Basis shape_basis = shape_transform.basis.orthonormalized();
+
+					// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
+					shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
+
+					Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
+					inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass_new;
+				}
+
+				// Set the inertia to a valid value when there are no valid shapes.
+				if (!inertia_set) {
+					inertia_tensor = Basis();
+				}
+
+				// Handle partial custom inertia.
+				if (inertia.x > 0.0) {
+					inertia_tensor[0][0] = inertia.x;
+				}
+				if (inertia.y > 0.0) {
+					inertia_tensor[1][1] = inertia.y;
+				}
+				if (inertia.z > 0.0) {
+					inertia_tensor[2][2] = inertia.z;
+				}
+
+				// Compute the principal axes of inertia.
+				principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
+				_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
+			}
+
+			if (mass) {
+				_inv_mass = 1.0 / mass;
+			} else {
+				_inv_mass = 0;
+			}
+
+		} break;
+		case PhysicsServer3D::BODY_MODE_KINEMATIC:
+		case PhysicsServer3D::BODY_MODE_STATIC: {
+			_inv_inertia = Vector3();
+			_inv_mass = 0;
+		} break;
+		case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: {
+			_inv_inertia_tensor.set_zero();
+			_inv_mass = 1.0 / mass;
+
+		} break;
+	}
+
+	_update_transform_dependent();
+}
+
+void GodotBody3D::reset_mass_properties() {
+	calculate_inertia = true;
+	calculate_center_of_mass = true;
+	_mass_properties_changed();
+}
+
+void GodotBody3D::set_active(bool p_active) {
+	if (active == p_active) {
+		return;
+	}
+
+	active = p_active;
+
+	if (active) {
+		if (mode == PhysicsServer3D::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 GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
+			bounce = p_value;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_FRICTION: {
+			friction = p_value;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_MASS: {
+			real_t mass_value = p_value;
+			ERR_FAIL_COND(mass_value <= 0);
+			mass = mass_value;
+			if (mode >= PhysicsServer3D::BODY_MODE_RIGID) {
+				_mass_properties_changed();
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_INERTIA: {
+			inertia = p_value;
+			if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) {
+				calculate_inertia = true;
+				if (mode == PhysicsServer3D::BODY_MODE_RIGID) {
+					_mass_properties_changed();
+				}
+			} else {
+				calculate_inertia = false;
+				if (mode == PhysicsServer3D::BODY_MODE_RIGID) {
+					principal_inertia_axes_local = Basis();
+					_inv_inertia = inertia.inverse();
+					_update_transform_dependent();
+				}
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+			calculate_center_of_mass = false;
+			center_of_mass_local = p_value;
+			_update_transform_dependent();
+		} break;
+		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
+			if (Math::is_zero_approx(gravity_scale)) {
+				wakeup();
+			}
+			gravity_scale = p_value;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: {
+			int mode_value = p_value;
+			linear_damp_mode = (PhysicsServer3D::BodyDampMode)mode_value;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: {
+			int mode_value = p_value;
+			angular_damp_mode = (PhysicsServer3D::BodyDampMode)mode_value;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: {
+			linear_damp = p_value;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: {
+			angular_damp = p_value;
+		} break;
+		default: {
+		}
+	}
+}
+
+Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
+			return bounce;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_FRICTION: {
+			return friction;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_MASS: {
+			return mass;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_INERTIA: {
+			if (mode == PhysicsServer3D::BODY_MODE_RIGID) {
+				return _inv_inertia.inverse();
+			} else {
+				return Vector3();
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+			return center_of_mass_local;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
+			return gravity_scale;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: {
+			return linear_damp_mode;
+		}
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: {
+			return angular_damp_mode;
+		}
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: {
+			return linear_damp;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: {
+			return angular_damp;
+		} break;
+
+		default: {
+		}
+	}
+
+	return 0;
+}
+
+void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
+	PhysicsServer3D::BodyMode prev = mode;
+	mode = p_mode;
+
+	switch (p_mode) {
+		case PhysicsServer3D::BODY_MODE_STATIC:
+		case PhysicsServer3D::BODY_MODE_KINEMATIC: {
+			_set_inv_transform(get_transform().affine_inverse());
+			_inv_mass = 0;
+			_inv_inertia = Vector3();
+			_set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC);
+			set_active(p_mode == PhysicsServer3D::BODY_MODE_KINEMATIC && contacts.size());
+			linear_velocity = Vector3();
+			angular_velocity = Vector3();
+			if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) {
+				first_time_kinematic = true;
+			}
+			_update_transform_dependent();
+
+		} break;
+		case PhysicsServer3D::BODY_MODE_RIGID: {
+			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			if (!calculate_inertia) {
+				principal_inertia_axes_local = Basis();
+				_inv_inertia = inertia.inverse();
+				_update_transform_dependent();
+			}
+			_mass_properties_changed();
+			_set_static(false);
+			set_active(true);
+
+		} break;
+		case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: {
+			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			_inv_inertia = Vector3();
+			angular_velocity = Vector3();
+			_update_transform_dependent();
+			_set_static(false);
+			set_active(true);
+		}
+	}
+}
+
+PhysicsServer3D::BodyMode GodotBody3D::get_mode() const {
+	return mode;
+}
+
+void GodotBody3D::_shapes_changed() {
+	_mass_properties_changed();
+	wakeup();
+	wakeup_neighbours();
+}
+
+void GodotBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			if (mode == PhysicsServer3D::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 == PhysicsServer3D::BODY_MODE_STATIC) {
+				_set_transform(p_variant);
+				_set_inv_transform(get_transform().affine_inverse());
+				wakeup_neighbours();
+			} else {
+				Transform3D t = p_variant;
+				t.orthonormalize();
+				new_transform = get_transform(); //used as old to compute motion
+				if (new_transform == t) {
+					break;
+				}
+				_set_transform(t);
+				_set_inv_transform(get_transform().inverse());
+				_update_transform_dependent();
+			}
+			wakeup();
+
+		} break;
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			linear_velocity = p_variant;
+			constant_linear_velocity = linear_velocity;
+			wakeup();
+		} break;
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			angular_velocity = p_variant;
+			constant_angular_velocity = angular_velocity;
+			wakeup();
+
+		} break;
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
+				break;
+			}
+			bool do_sleep = p_variant;
+			if (do_sleep) {
+				linear_velocity = Vector3();
+				//biased_linear_velocity=Vector3();
+				angular_velocity = Vector3();
+				//biased_angular_velocity=Vector3();
+				set_active(false);
+			} else {
+				set_active(true);
+			}
+		} break;
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			can_sleep = p_variant;
+			if (mode >= PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) {
+				set_active(true);
+			}
+
+		} break;
+	}
+}
+
+Variant GodotBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			return get_transform();
+		} break;
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			return linear_velocity;
+		} break;
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			return angular_velocity;
+		} break;
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			return !is_active();
+		} break;
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			return can_sleep;
+		} break;
+	}
+
+	return Variant();
+}
+
+void GodotBody3D::set_space(GodotSpace3D *p_space) {
+	if (get_space()) {
+		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 GodotBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) {
+	if (lock) {
+		locked_axis |= p_axis;
+	} else {
+		locked_axis &= ~p_axis;
+	}
+}
+
+bool GodotBody3D::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const {
+	return locked_axis & p_axis;
+}
+
+void GodotBody3D::integrate_forces(real_t p_step) {
+	if (mode == PhysicsServer3D::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 = Vector3(0, 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) {
+				PhysicsServer3D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE);
+				if (area_gravity_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+					Vector3 area_gravity;
+					aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity);
+					switch (area_gravity_mode) {
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							gravity += area_gravity;
+							gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							gravity = area_gravity;
+							gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
+				}
+			}
+			if (!linear_damp_done) {
+				PhysicsServer3D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE);
+				if (area_linear_damp_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+					real_t area_linear_damp = aa[i].area->get_linear_damp();
+					switch (area_linear_damp_mode) {
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							total_linear_damp += area_linear_damp;
+							linear_damp_done = area_linear_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							total_linear_damp = area_linear_damp;
+							linear_damp_done = area_linear_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
+				}
+			}
+			if (!angular_damp_done) {
+				PhysicsServer3D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE);
+				if (area_angular_damp_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+					real_t area_angular_damp = aa[i].area->get_angular_damp();
+					switch (area_angular_damp_mode) {
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							total_angular_damp += area_angular_damp;
+							angular_damp_done = area_angular_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							total_angular_damp = area_angular_damp;
+							angular_damp_done = area_angular_damp_mode == PhysicsServer3D::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) {
+		GodotArea3D *default_area = get_space()->get_default_area();
+		ERR_FAIL_NULL(default_area);
+
+		if (!gravity_done) {
+			Vector3 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 PhysicsServer3D::BODY_DAMP_MODE_COMBINE: {
+			total_linear_damp += linear_damp;
+		} break;
+		case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: {
+			total_linear_damp = linear_damp;
+		} break;
+	}
+
+	// Override angular damping with body's value.
+	switch (angular_damp_mode) {
+		case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: {
+			total_angular_damp += angular_damp;
+		} break;
+		case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: {
+			total_angular_damp = angular_damp;
+		} break;
+	}
+
+	gravity *= gravity_scale;
+
+	prev_linear_velocity = linear_velocity;
+	prev_angular_velocity = angular_velocity;
+
+	Vector3 motion;
+	bool do_motion = false;
+
+	if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		//compute motion, angular and etc. velocities from prev transform
+		motion = new_transform.origin - get_transform().origin;
+		do_motion = true;
+		linear_velocity = constant_linear_velocity + motion / p_step;
+
+		//compute a FAKE angular velocity, not so easy
+		Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed();
+		Vector3 axis;
+		real_t angle;
+
+		rot.get_axis_angle(axis, angle);
+		axis.normalize();
+		angular_velocity = constant_angular_velocity + axis * (angle / p_step);
+	} else {
+		if (!omit_force_integration) {
+			//overridden by direct state query
+
+			Vector3 force = gravity * mass + applied_force + constant_force;
+			Vector3 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_tensor.xform(torque) * p_step;
+		}
+
+		if (continuous_cd) {
+			motion = linear_velocity * p_step;
+			do_motion = true;
+		}
+	}
+
+	applied_force = Vector3();
+	applied_torque = Vector3();
+
+	biased_angular_velocity = Vector3();
+	biased_linear_velocity = Vector3();
+
+	if (do_motion) { //shapes temporarily extend for raycast
+		_update_shapes_with_motion(motion);
+	}
+
+	contact_count = 0;
+}
+
+void GodotBody3D::integrate_velocities(real_t p_step) {
+	if (mode == PhysicsServer3D::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);
+	}
+
+	//apply axis lock linear
+	for (int i = 0; i < 3; i++) {
+		if (is_axis_locked((PhysicsServer3D::BodyAxis)(1 << i))) {
+			linear_velocity[i] = 0;
+			biased_linear_velocity[i] = 0;
+			new_transform.origin[i] = get_transform().origin[i];
+		}
+	}
+	//apply axis lock angular
+	for (int i = 0; i < 3; i++) {
+		if (is_axis_locked((PhysicsServer3D::BodyAxis)(1 << (i + 3)))) {
+			angular_velocity[i] = 0;
+			biased_angular_velocity[i] = 0;
+		}
+	}
+
+	if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		_set_transform(new_transform, false);
+		_set_inv_transform(new_transform.affine_inverse());
+		if (contacts.is_empty() && linear_velocity == Vector3() && angular_velocity == Vector3()) {
+			set_active(false); //stopped moving, deactivate
+		}
+
+		return;
+	}
+
+	Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity;
+
+	real_t ang_vel = total_angular_velocity.length();
+	Transform3D transform_new = get_transform();
+
+	if (!Math::is_zero_approx(ang_vel)) {
+		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
+		Basis rot(ang_vel_axis, ang_vel * p_step);
+		Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
+		transform_new.origin += ((identity3 - rot) * transform_new.basis).xform(center_of_mass_local);
+		transform_new.basis = rot * transform_new.basis;
+		transform_new.orthonormalize();
+	}
+
+	Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity;
+	/*for(int i=0;i<3;i++) {
+		if (axis_lock&(1<<i)) {
+			transform_new.origin[i]=0.0;
+		}
+	}*/
+
+	transform_new.origin += total_linear_velocity * p_step;
+
+	_set_transform(transform_new);
+	_set_inv_transform(get_transform().inverse());
+
+	_update_transform_dependent();
+}
+
+void GodotBody3D::wakeup_neighbours() {
+	for (const KeyValue<GodotConstraint3D *, int> &E : constraint_map) {
+		const GodotConstraint3D *c = E.key;
+		GodotBody3D **n = c->get_body_ptr();
+		int bc = c->get_body_count();
+
+		for (int i = 0; i < bc; i++) {
+			if (i == E.value) {
+				continue;
+			}
+			GodotBody3D *b = n[i];
+			if (b->mode < PhysicsServer3D::BODY_MODE_RIGID) {
+				continue;
+			}
+
+			if (!b->is_active()) {
+				b->set_active(true);
+			}
+		}
+	}
+}
+
+void GodotBody3D::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;
+			int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2;
+			Variant rv;
+			fi_callback_data->callable.callp(vp, argc, rv, ce);
+		}
+	}
+
+	if (body_state_callback.is_valid()) {
+		body_state_callback.call(direct_state_variant);
+	}
+}
+
+bool GodotBody3D::sleep_test(real_t p_step) {
+	if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		return true;
+	} else if (!can_sleep) {
+		return false;
+	}
+
+	ERR_FAIL_NULL_V(get_space(), true);
+
+	if (Math::abs(angular_velocity.length()) < 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 GodotBody3D::set_state_sync_callback(const Callable &p_callable) {
+	body_state_callback = p_callable;
+}
+
+void GodotBody3D::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;
+	}
+}
+
+GodotPhysicsDirectBodyState3D *GodotBody3D::get_direct_state() {
+	if (!direct_state) {
+		direct_state = memnew(GodotPhysicsDirectBodyState3D);
+		direct_state->body = this;
+	}
+	return direct_state;
+}
+
+GodotBody3D::GodotBody3D() :
+		GodotCollisionObject3D(TYPE_BODY),
+		active_list(this),
+		mass_properties_update_list(this),
+		direct_state_query_list(this) {
+	_set_static(false);
+}
+
+GodotBody3D::~GodotBody3D() {
+	if (fi_callback_data) {
+		memdelete(fi_callback_data);
+	}
+	if (direct_state) {
+		memdelete(direct_state);
+	}
+}

modules/godot_physics_3d/godot_body_3d.h

@@ -0,0 +1,394 @@
+/**************************************************************************/
+/*  godot_body_3d.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_3d.h"
+#include "godot_collision_object_3d.h"
+
+#include "core/templates/vset.h"
+
+class GodotConstraint3D;
+class GodotPhysicsDirectBodyState3D;
+
+class GodotBody3D : public GodotCollisionObject3D {
+	PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_RIGID;
+
+	Vector3 linear_velocity;
+	Vector3 angular_velocity;
+
+	Vector3 prev_linear_velocity;
+	Vector3 prev_angular_velocity;
+
+	Vector3 constant_linear_velocity;
+	Vector3 constant_angular_velocity;
+
+	Vector3 biased_linear_velocity;
+	Vector3 biased_angular_velocity;
+	real_t mass = 1.0;
+	real_t bounce = 0.0;
+	real_t friction = 1.0;
+	Vector3 inertia;
+
+	PhysicsServer3D::BodyDampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
+	PhysicsServer3D::BodyDampMode angular_damp_mode = PhysicsServer3D::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;
+
+	uint16_t locked_axis = 0;
+
+	real_t _inv_mass = 1.0;
+	Vector3 _inv_inertia; // Relative to the principal axes of inertia
+
+	// Relative to the local frame of reference
+	Basis principal_inertia_axes_local;
+	Vector3 center_of_mass_local;
+
+	// In world orientation with local origin
+	Basis _inv_inertia_tensor;
+	Basis principal_inertia_axes;
+	Vector3 center_of_mass;
+
+	bool calculate_inertia = true;
+	bool calculate_center_of_mass = true;
+
+	Vector3 gravity;
+
+	real_t still_time = 0.0;
+
+	Vector3 applied_force;
+	Vector3 applied_torque;
+
+	Vector3 constant_force;
+	Vector3 constant_torque;
+
+	SelfList<GodotBody3D> active_list;
+	SelfList<GodotBody3D> mass_properties_update_list;
+	SelfList<GodotBody3D> direct_state_query_list;
+
+	VSet<RID> exceptions;
+	bool omit_force_integration = false;
+	bool active = true;
+
+	bool continuous_cd = false;
+	bool can_sleep = true;
+	bool first_time_kinematic = false;
+
+	void _mass_properties_changed();
+	virtual void _shapes_changed() override;
+	Transform3D new_transform;
+
+	HashMap<GodotConstraint3D *, int> constraint_map;
+
+	Vector<AreaCMP> areas;
+
+	struct Contact {
+		Vector3 local_pos;
+		Vector3 local_normal;
+		Vector3 local_velocity_at_pos;
+		real_t depth = 0.0;
+		int local_shape = 0;
+		Vector3 collider_pos;
+		int collider_shape = 0;
+		ObjectID collider_instance_id;
+		RID collider;
+		Vector3 collider_velocity_at_pos;
+		Vector3 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;
+
+	GodotPhysicsDirectBodyState3D *direct_state = nullptr;
+
+	uint64_t island_step = 0;
+
+	void _update_transform_dependent();
+
+	friend class GodotPhysicsDirectBodyState3D; // 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());
+
+	GodotPhysicsDirectBodyState3D *get_direct_state();
+
+	_FORCE_INLINE_ void add_area(GodotArea3D *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(GodotArea3D *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_3D_MAX);
+		contacts.resize(p_size);
+		contact_count = 0;
+		if (mode == PhysicsServer3D::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 Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_local_velocity_at_pos, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &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(GodotConstraint3D *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
+	_FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraint_map.erase(p_constraint); }
+	const HashMap<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; }
+	_FORCE_INLINE_ void clear_constraint_map() { constraint_map.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_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; }
+	_FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; }
+	_FORCE_INLINE_ Vector3 get_center_of_mass_local() const { return center_of_mass_local; }
+	_FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); }
+
+	_FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; }
+	_FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; }
+
+	_FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; }
+	_FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; }
+
+	_FORCE_INLINE_ Vector3 get_prev_linear_velocity() const { return prev_linear_velocity; }
+	_FORCE_INLINE_ Vector3 get_prev_angular_velocity() const { return prev_angular_velocity; }
+
+	_FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; }
+	_FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; }
+
+	_FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_impulse) {
+		linear_velocity += p_impulse * _inv_mass;
+	}
+
+	_FORCE_INLINE_ void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) {
+		linear_velocity += p_impulse * _inv_mass;
+		angular_velocity += _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse));
+	}
+
+	_FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_impulse) {
+		angular_velocity += _inv_inertia_tensor.xform(p_impulse);
+	}
+
+	_FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3(), real_t p_max_delta_av = -1.0) {
+		biased_linear_velocity += p_impulse * _inv_mass;
+		if (p_max_delta_av != 0.0) {
+			Vector3 delta_av = _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse));
+			if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) {
+				delta_av = delta_av.normalized() * p_max_delta_av;
+			}
+			biased_angular_velocity += delta_av;
+		}
+	}
+
+	_FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_impulse) {
+		biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse);
+	}
+
+	_FORCE_INLINE_ void apply_central_force(const Vector3 &p_force) {
+		applied_force += p_force;
+	}
+
+	_FORCE_INLINE_ void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
+		applied_force += p_force;
+		applied_torque += (p_position - center_of_mass).cross(p_force);
+	}
+
+	_FORCE_INLINE_ void apply_torque(const Vector3 &p_torque) {
+		applied_torque += p_torque;
+	}
+
+	_FORCE_INLINE_ void add_constant_central_force(const Vector3 &p_force) {
+		constant_force += p_force;
+	}
+
+	_FORCE_INLINE_ void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
+		constant_force += p_force;
+		constant_torque += (p_position - center_of_mass).cross(p_force);
+	}
+
+	_FORCE_INLINE_ void add_constant_torque(const Vector3 &p_torque) {
+		constant_torque += p_torque;
+	}
+
+	void set_constant_force(const Vector3 &p_force) { constant_force = p_force; }
+	Vector3 get_constant_force() const { return constant_force; }
+
+	void set_constant_torque(const Vector3 &p_torque) { constant_torque = p_torque; }
+	Vector3 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 == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
+			return;
+		}
+		set_active(true);
+	}
+
+	void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value);
+	Variant get_param(PhysicsServer3D::BodyParameter p_param) const;
+
+	void set_mode(PhysicsServer3D::BodyMode p_mode);
+	PhysicsServer3D::BodyMode get_mode() const;
+
+	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
+	Variant get_state(PhysicsServer3D::BodyState p_state) const;
+
+	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; }
+	_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
+
+	void set_space(GodotSpace3D *p_space) override;
+
+	void update_mass_properties();
+	void reset_mass_properties();
+
+	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
+	_FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; }
+	_FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; }
+	_FORCE_INLINE_ real_t get_friction() const { return friction; }
+	_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
+
+	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock);
+	bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const;
+
+	void integrate_forces(real_t p_step);
+	void integrate_velocities(real_t p_step);
+
+	_FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3 &rel_pos) const {
+		return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass);
+	}
+
+	_FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const {
+		Vector3 r0 = p_pos - get_transform().origin - center_of_mass;
+
+		Vector3 c0 = (r0).cross(p_normal);
+
+		Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0);
+
+		return _inv_mass + p_normal.dot(vec);
+	}
+
+	_FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const {
+		return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis));
+	}
+
+	//void simulate_motion(const Transform3D& p_xform,real_t p_step);
+	void call_queries();
+	void wakeup_neighbours();
+
+	bool sleep_test(real_t p_step);
+
+	GodotBody3D();
+	~GodotBody3D();
+};
+
+//add contact inline
+
+void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_local_velocity_at_pos, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &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_3d/godot_body_direct_state_3d.cpp

@@ -0,0 +1,253 @@
+/**************************************************************************/
+/*  godot_body_direct_state_3d.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_3d.h"
+
+#include "godot_body_3d.h"
+#include "godot_space_3d.h"
+
+Vector3 GodotPhysicsDirectBodyState3D::get_total_gravity() const {
+	return body->gravity;
+}
+
+real_t GodotPhysicsDirectBodyState3D::get_total_angular_damp() const {
+	return body->total_angular_damp;
+}
+
+real_t GodotPhysicsDirectBodyState3D::get_total_linear_damp() const {
+	return body->total_linear_damp;
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass() const {
+	return body->get_center_of_mass();
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass_local() const {
+	return body->get_center_of_mass_local();
+}
+
+Basis GodotPhysicsDirectBodyState3D::get_principal_inertia_axes() const {
+	return body->get_principal_inertia_axes();
+}
+
+real_t GodotPhysicsDirectBodyState3D::get_inverse_mass() const {
+	return body->get_inv_mass();
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_inverse_inertia() const {
+	return body->get_inv_inertia();
+}
+
+Basis GodotPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const {
+	return body->get_inv_inertia_tensor();
+}
+
+void GodotPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) {
+	body->wakeup();
+	body->set_linear_velocity(p_velocity);
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_linear_velocity() const {
+	return body->get_linear_velocity();
+}
+
+void GodotPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) {
+	body->wakeup();
+	body->set_angular_velocity(p_velocity);
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_angular_velocity() const {
+	return body->get_angular_velocity();
+}
+
+void GodotPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) {
+	body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform);
+}
+
+Transform3D GodotPhysicsDirectBodyState3D::get_transform() const {
+	return body->get_transform();
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_position) const {
+	return body->get_velocity_in_local_point(p_position);
+}
+
+void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) {
+	body->wakeup();
+	body->apply_central_impulse(p_impulse);
+}
+
+void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
+	body->wakeup();
+	body->apply_impulse(p_impulse, p_position);
+}
+
+void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) {
+	body->wakeup();
+	body->apply_torque_impulse(p_impulse);
+}
+
+void GodotPhysicsDirectBodyState3D::apply_central_force(const Vector3 &p_force) {
+	body->wakeup();
+	body->apply_central_force(p_force);
+}
+
+void GodotPhysicsDirectBodyState3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) {
+	body->wakeup();
+	body->apply_force(p_force, p_position);
+}
+
+void GodotPhysicsDirectBodyState3D::apply_torque(const Vector3 &p_torque) {
+	body->wakeup();
+	body->apply_torque(p_torque);
+}
+
+void GodotPhysicsDirectBodyState3D::add_constant_central_force(const Vector3 &p_force) {
+	body->wakeup();
+	body->add_constant_central_force(p_force);
+}
+
+void GodotPhysicsDirectBodyState3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) {
+	body->wakeup();
+	body->add_constant_force(p_force, p_position);
+}
+
+void GodotPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) {
+	body->wakeup();
+	body->add_constant_torque(p_torque);
+}
+
+void GodotPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) {
+	if (!p_force.is_zero_approx()) {
+		body->wakeup();
+	}
+	body->set_constant_force(p_force);
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_constant_force() const {
+	return body->get_constant_force();
+}
+
+void GodotPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) {
+	if (!p_torque.is_zero_approx()) {
+		body->wakeup();
+	}
+	body->set_constant_torque(p_torque);
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_constant_torque() const {
+	return body->get_constant_torque();
+}
+
+void GodotPhysicsDirectBodyState3D::set_sleep_state(bool p_sleep) {
+	body->set_active(!p_sleep);
+}
+
+bool GodotPhysicsDirectBodyState3D::is_sleeping() const {
+	return !body->is_active();
+}
+
+void GodotPhysicsDirectBodyState3D::set_collision_layer(uint32_t p_layer) {
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t GodotPhysicsDirectBodyState3D::get_collision_layer() const {
+	return body->get_collision_layer();
+}
+
+void GodotPhysicsDirectBodyState3D::set_collision_mask(uint32_t p_mask) {
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t GodotPhysicsDirectBodyState3D::get_collision_mask() const {
+	return body->get_collision_mask();
+}
+
+int GodotPhysicsDirectBodyState3D::get_contact_count() const {
+	return body->contact_count;
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].local_pos;
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].local_normal;
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].impulse;
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].local_velocity_at_pos;
+}
+
+int GodotPhysicsDirectBodyState3D::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;
+}
+
+RID GodotPhysicsDirectBodyState3D::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;
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_contact_collider_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].collider_pos;
+}
+
+ObjectID GodotPhysicsDirectBodyState3D::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 GodotPhysicsDirectBodyState3D::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;
+}
+
+Vector3 GodotPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].collider_velocity_at_pos;
+}
+
+PhysicsDirectSpaceState3D *GodotPhysicsDirectBodyState3D::get_space_state() {
+	return body->get_space()->get_direct_state();
+}
+
+real_t GodotPhysicsDirectBodyState3D::get_step() const {
+	return body->get_space()->get_last_step();
+}

modules/godot_physics_3d/godot_body_direct_state_3d.h

@@ -0,0 +1,110 @@
+/**************************************************************************/
+/*  godot_body_direct_state_3d.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_3d.h"
+
+class GodotBody3D;
+
+class GodotPhysicsDirectBodyState3D : public PhysicsDirectBodyState3D {
+	GDCLASS(GodotPhysicsDirectBodyState3D, PhysicsDirectBodyState3D);
+
+public:
+	GodotBody3D *body = nullptr;
+
+	virtual Vector3 get_total_gravity() const override;
+	virtual real_t get_total_angular_damp() const override;
+	virtual real_t get_total_linear_damp() const override;
+
+	virtual Vector3 get_center_of_mass() const override;
+	virtual Vector3 get_center_of_mass_local() const override;
+	virtual Basis get_principal_inertia_axes() const override;
+
+	virtual real_t get_inverse_mass() const override;
+	virtual Vector3 get_inverse_inertia() const override;
+	virtual Basis get_inverse_inertia_tensor() const override;
+
+	virtual void set_linear_velocity(const Vector3 &p_velocity) override;
+	virtual Vector3 get_linear_velocity() const override;
+
+	virtual void set_angular_velocity(const Vector3 &p_velocity) override;
+	virtual Vector3 get_angular_velocity() const override;
+
+	virtual void set_transform(const Transform3D &p_transform) override;
+	virtual Transform3D get_transform() const override;
+
+	virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override;
+
+	virtual void apply_central_impulse(const Vector3 &p_impulse) override;
+	virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override;
+	virtual void apply_torque_impulse(const Vector3 &p_impulse) override;
+
+	virtual void apply_central_force(const Vector3 &p_force) override;
+	virtual void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void apply_torque(const Vector3 &p_torque) override;
+
+	virtual void add_constant_central_force(const Vector3 &p_force) override;
+	virtual void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void add_constant_torque(const Vector3 &p_torque) override;
+
+	virtual void set_constant_force(const Vector3 &p_force) override;
+	virtual Vector3 get_constant_force() const override;
+
+	virtual void set_constant_torque(const Vector3 &p_torque) override;
+	virtual Vector3 get_constant_torque() const override;
+
+	virtual void set_sleep_state(bool p_sleep) 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 Vector3 get_contact_local_position(int p_contact_idx) const override;
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const override;
+	virtual Vector3 get_contact_impulse(int p_contact_idx) const override;
+	virtual int get_contact_local_shape(int p_contact_idx) const override;
+	virtual Vector3 get_contact_local_velocity_at_position(int p_contact_idx) const override;
+
+	virtual RID get_contact_collider(int p_contact_idx) const override;
+	virtual Vector3 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 Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
+
+	virtual PhysicsDirectSpaceState3D *get_space_state() override;
+
+	virtual real_t get_step() const override;
+};

modules/godot_physics_3d/godot_body_pair_3d.cpp

@@ -0,0 +1,988 @@
+/**************************************************************************/
+/*  godot_body_pair_3d.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_3d.h"
+
+#include "godot_collision_solver_3d.h"
+#include "godot_space_3d.h"
+
+#define MIN_VELOCITY 0.0001
+#define MAX_BIAS_ROTATION (Math::PI / 8)
+
+void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
+	GodotBodyPair3D *pair = static_cast<GodotBodyPair3D *>(p_userdata);
+	pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal);
+}
+
+void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) {
+	Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
+	Vector3 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.index_A = p_index_A;
+	contact.index_B = p_index_B;
+	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 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) < (contact_recycle_radius * contact_recycle_radius) &&
+				c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) {
+			contact.acc_normal_impulse = c.acc_normal_impulse;
+			contact.acc_bias_impulse = c.acc_bias_impulse;
+			contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
+			contact.acc_tangent_impulse = c.acc_tangent_impulse;
+			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 Basis &basis_A = A->get_transform().basis;
+		const Basis &basis_B = B->get_transform().basis;
+
+		int least_deep = -1;
+		real_t min_depth;
+
+		// Start with depth for new contact.
+		{
+			Vector3 global_A = basis_A.xform(contact.local_A);
+			Vector3 global_B = basis_B.xform(contact.local_B) + offset_B;
+
+			Vector3 axis = global_A - global_B;
+			min_depth = axis.dot(contact.normal);
+		}
+
+		for (int i = 0; i < contact_count; i++) {
+			const Contact &c = contacts[i];
+			Vector3 global_A = basis_A.xform(c.local_A);
+			Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
+
+			Vector3 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 GodotBodyPair3D::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 Basis &basis_A = A->get_transform().basis;
+	const Basis &basis_B = B->get_transform().basis;
+
+	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;
+
+			Vector3 global_A = basis_A.xform(c.local_A);
+			Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
+			Vector3 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 GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) {
+	GodotShape3D *shape_A_ptr = p_A->get_shape(p_shape_A);
+
+	Vector3 motion = p_A->get_linear_velocity() * p_step;
+	real_t mlen = motion.length();
+	if (mlen < CMP_EPSILON) {
+		return false;
+	}
+
+	Vector3 mnormal = motion / mlen;
+
+	real_t min = 0.0, max = 0.0;
+	shape_A_ptr->project_range(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; // moving slow enough that there's no chance of tunneling.
+	}
+
+	// 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).
+	Transform3D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step);
+
+	// Support points are the farthest forward points on A in the direction of the motion vector.
+	// i.e. the candidate points of which one should hit B first if any collision does occur.
+	static const int max_supports = 16;
+	Vector3 supports_A[max_supports];
+	int support_count_A;
+	GodotShape3D::FeatureType support_type_A;
+	// Convert mnormal into body A's local xform because get_supports requires (and returns) local coordinates.
+	shape_A_ptr->get_supports(p_xform_A.basis.xform_inv(mnormal).normalized(), max_supports, supports_A, support_count_A, support_type_A);
+
+	// Cast a segment from each support point of A in the motion direction.
+	int segment_support_idx = -1;
+	float segment_hit_length = FLT_MAX;
+	Vector3 segment_hit_local;
+	for (int i = 0; i < support_count_A; i++) {
+		supports_A[i] = p_xform_A.xform(supports_A[i]);
+
+		Vector3 from = supports_A[i];
+		Vector3 to = from + motion;
+
+		Transform3D 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.
+		Vector3 local_from = from_inv.xform(from - motion * 0.1);
+		Vector3 local_to = from_inv.xform(to);
+
+		Vector3 rpos, rnorm;
+		int fi = -1;
+		if (p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, fi, true)) {
+			float hit_length = local_from.distance_to(rpos);
+			if (hit_length < segment_hit_length) {
+				segment_support_idx = i;
+				segment_hit_length = hit_length;
+				segment_hit_local = rpos;
+			}
+		}
+	}
+
+	if (segment_support_idx == -1) {
+		// 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;
+	}
+
+	Vector3 hitpos = predicted_xform_B.xform(segment_hit_local);
+
+	real_t newlen = hitpos.distance_to(supports_A[segment_support_idx]);
+	// 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.
+	newlen += (max - min) * 0.01;
+	// FIXME: This doesn't always work well when colliding with a triangle face of a trimesh shape.
+
+	p_A->set_linear_velocity((mnormal * newlen) / p_step);
+
+	return true;
+}
+
+real_t combine_bounce(GodotBody3D *A, GodotBody3D *B) {
+	return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1);
+}
+
+real_t combine_friction(GodotBody3D *A, GodotBody3D *B) {
+	return Math::abs(MIN(A->get_friction(), B->get_friction()));
+}
+
+bool GodotBodyPair3D::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() > PhysicsServer3D::BODY_MODE_KINEMATIC) && A->collides_with(B);
+	collide_B = (B->get_mode() > PhysicsServer3D::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;
+		}
+	}
+
+	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
+
+	validate_contacts();
+
+	const Vector3 &offset_A = A->get_transform().get_origin();
+	Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3());
+	Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A);
+
+	Transform3D xform_Bu = B->get_transform();
+	xform_Bu.origin -= offset_A;
+	Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B);
+
+	GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
+	GodotShape3D *shape_B_ptr = B->get_shape(shape_B);
+
+	collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
+
+	if (!collided) {
+		if (A->is_continuous_collision_detection_enabled() && collide_A) {
+			check_ccd = true;
+			return true;
+		}
+
+		if (B->is_continuous_collision_detection_enabled() && collide_B) {
+			check_ccd = true;
+			return true;
+		}
+
+		return false;
+	}
+
+	return true;
+}
+
+bool GodotBodyPair3D::pre_solve(real_t p_step) {
+	if (!collided) {
+		if (check_ccd) {
+			const Vector3 &offset_A = A->get_transform().get_origin();
+			Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3());
+			Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A);
+
+			Transform3D xform_Bu = B->get_transform();
+			xform_Bu.origin -= offset_A;
+			Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B);
+
+			if (A->is_continuous_collision_detection_enabled() && collide_A) {
+				_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
+			}
+
+			if (B->is_continuous_collision_detection_enabled() && 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 = 0.8;
+
+	GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
+	GodotShape3D *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 Vector3 &offset_A = A->get_transform().get_origin();
+
+	const Basis &basis_A = A->get_transform().basis;
+	const Basis &basis_B = B->get_transform().basis;
+
+	Basis zero_basis;
+	zero_basis.set_zero();
+
+	const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis;
+	const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis;
+
+	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;
+
+		Vector3 global_A = basis_A.xform(c.local_A);
+		Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
+
+		Vector3 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.
+		Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal));
+		Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal));
+		real_t kNormal = inv_mass_A + inv_mass_B;
+		kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB));
+		c.mass_normal = 1.0f / kNormal;
+
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+		c.depth = depth;
+
+		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
+
+		c.acc_impulse -= j_vec;
+
+		// contact query reporting...
+
+		if (A->can_report_contacts() || B->can_report_contacts()) {
+			Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
+			Vector3 crA = A->get_angular_velocity().cross(c.rA) + 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;
+		}
+
+		c.active = true;
+		do_process = true;
+
+		if (collide_A) {
+			A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
+		}
+		if (collide_B) {
+			B->apply_impulse(j_vec, c.rB + B->get_center_of_mass());
+		}
+
+		c.bounce = combine_bounce(A, B);
+		if (c.bounce) {
+			Vector3 crA = A->get_prev_angular_velocity().cross(c.rA);
+			Vector3 crB = B->get_prev_angular_velocity().cross(c.rB);
+			Vector3 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA;
+			c.bounce = c.bounce * dv.dot(c.normal);
+		}
+	}
+
+	return do_process;
+}
+
+void GodotBodyPair3D::solve(real_t p_step) {
+	if (!collided) {
+		return;
+	}
+
+	const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
+
+	Basis zero_basis;
+	zero_basis.set_zero();
+
+	const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis;
+	const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis;
+
+	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;
+		}
+
+		c.active = false; //try to deactivate, will activate itself if still needed
+
+		//bias impulse
+
+		Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA);
+		Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB);
+		Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
+
+		real_t vbn = dbv.dot(c.normal);
+
+		if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
+			real_t jbn = (-vbn + c.bias) * c.mass_normal;
+			real_t jbnOld = c.acc_bias_impulse;
+			c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
+
+			Vector3 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 = A->get_biased_angular_velocity().cross(c.rA);
+			crbB = B->get_biased_angular_velocity().cross(c.rB);
+			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);
+
+				Vector3 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);
+				}
+			}
+
+			c.active = true;
+		}
+
+		Vector3 crA = A->get_angular_velocity().cross(c.rA);
+		Vector3 crB = B->get_angular_velocity().cross(c.rB);
+		Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
+
+		//normal impulse
+		real_t vn = dv.dot(c.normal);
+
+		if (Math::abs(vn) > MIN_VELOCITY) {
+			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);
+
+			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
+
+			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;
+
+			c.active = true;
+		}
+
+		//friction impulse
+
+		real_t friction = combine_friction(A, B);
+
+		Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA);
+		Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB);
+
+		Vector3 dtv = lvB - lvA;
+		real_t tn = c.normal.dot(dtv);
+
+		// tangential velocity
+		Vector3 tv = dtv - c.normal * tn;
+		real_t tvl = tv.length();
+
+		if (tvl > MIN_VELOCITY) {
+			tv /= tvl;
+
+			Vector3 temp1 = inv_inertia_tensor_A.xform(c.rA.cross(tv));
+			Vector3 temp2 = inv_inertia_tensor_B.xform(c.rB.cross(tv));
+
+			real_t t = -tvl / (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
+
+			Vector3 jt = t * tv;
+
+			Vector3 jtOld = c.acc_tangent_impulse;
+			c.acc_tangent_impulse += jt;
+
+			real_t fi_len = c.acc_tangent_impulse.length();
+			real_t jtMax = c.acc_normal_impulse * friction;
+
+			if (fi_len > CMP_EPSILON && fi_len > jtMax) {
+				c.acc_tangent_impulse *= jtMax / fi_len;
+			}
+
+			jt = c.acc_tangent_impulse - jtOld;
+
+			if (collide_A) {
+				A->apply_impulse(-jt, c.rA + A->get_center_of_mass());
+			}
+			if (collide_B) {
+				B->apply_impulse(jt, c.rB + B->get_center_of_mass());
+			}
+			c.acc_impulse -= jt;
+
+			c.active = true;
+		}
+	}
+}
+
+GodotBodyPair3D::GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B) :
+		GodotBodyContact3D(_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);
+}
+
+GodotBodyPair3D::~GodotBodyPair3D() {
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+}
+
+void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
+	GodotBodySoftBodyPair3D *pair = static_cast<GodotBodySoftBodyPair3D *>(p_userdata);
+	pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal);
+}
+
+void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) {
+	Vector3 local_A = body->get_inv_transform().xform(p_point_A);
+	Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B);
+
+	Contact contact;
+	contact.index_A = p_index_A;
+	contact.index_B = p_index_B;
+	contact.local_A = local_A;
+	contact.local_B = local_B;
+	contact.normal = (normal.dot((p_point_A - p_point_B)) < 0 ? -normal : normal);
+	contact.used = true;
+
+	// Attempt to determine if the contact will be reused.
+	real_t contact_recycle_radius = space->get_contact_recycle_radius();
+
+	uint32_t contact_count = contacts.size();
+	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
+		Contact &c = contacts[contact_index];
+		if (c.index_B == p_index_B) {
+			if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) &&
+					c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) {
+				contact.acc_normal_impulse = c.acc_normal_impulse;
+				contact.acc_bias_impulse = c.acc_bias_impulse;
+				contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
+				contact.acc_tangent_impulse = c.acc_tangent_impulse;
+			}
+			c = contact;
+			return;
+		}
+	}
+
+	contacts.push_back(contact);
+}
+
+void GodotBodySoftBodyPair3D::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 Transform3D &transform_A = body->get_transform();
+
+	uint32_t contact_count = contacts.size();
+	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
+		Contact &c = contacts[contact_index];
+
+		bool erase = false;
+		if (!c.used) {
+			// Was left behind in previous frame.
+			erase = true;
+		} else {
+			c.used = false;
+
+			Vector3 global_A = transform_A.xform(c.local_A);
+			Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
+			Vector3 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 ((contact_index + 1) < contact_count) {
+				// Swap with the last one.
+				SWAP(c, contacts[contact_count - 1]);
+			}
+
+			contact_index--;
+			contact_count--;
+		}
+	}
+
+	contacts.resize(contact_count);
+}
+
+bool GodotBodySoftBodyPair3D::setup(real_t p_step) {
+	if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
+		collided = false;
+		return false;
+	}
+
+	body_collides = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && body->collides_with(soft_body);
+	soft_body_collides = soft_body->collides_with(body);
+
+	if (!body_collides && !soft_body_collides) {
+		if (body->get_max_contacts_reported() > 0) {
+			report_contacts_only = true;
+		} else {
+			collided = false;
+			return false;
+		}
+	}
+
+	const Transform3D &xform_Au = body->get_transform();
+	Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape);
+
+	Transform3D xform_Bu = soft_body->get_transform();
+	Transform3D xform_B = xform_Bu * soft_body->get_shape_transform(0);
+
+	validate_contacts();
+
+	GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
+	GodotShape3D *shape_B_ptr = soft_body->get_shape(0);
+
+	collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
+
+	return collided;
+}
+
+bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
+	if (!collided) {
+		return false;
+	}
+
+	real_t max_penetration = space->get_contact_max_allowed_penetration();
+
+	real_t bias = space->get_contact_bias();
+
+	GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
+
+	if (shape_A_ptr->get_custom_bias()) {
+		bias = shape_A_ptr->get_custom_bias();
+	}
+
+	real_t inv_dt = 1.0 / p_step;
+
+	bool do_process = false;
+
+	const Transform3D &transform_A = body->get_transform();
+
+	Basis zero_basis;
+	zero_basis.set_zero();
+
+	const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis;
+
+	real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0;
+
+	uint32_t contact_count = contacts.size();
+	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
+		Contact &c = contacts[contact_index];
+		c.active = false;
+
+		real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0;
+		if ((node_inv_mass == 0.0) && (body_inv_mass == 0.0)) {
+			continue;
+		}
+
+		Vector3 global_A = transform_A.xform(c.local_A);
+		Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
+		Vector3 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);
+			space->add_debug_contact(global_B);
+		}
+#endif
+
+		c.rA = global_A - transform_A.origin - body->get_center_of_mass();
+		c.rB = global_B;
+
+		// Precompute normal mass, tangent mass, and bias.
+		Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal));
+		real_t kNormal = body_inv_mass + node_inv_mass;
+		kNormal += c.normal.dot(inertia_A.cross(c.rA));
+		c.mass_normal = 1.0f / kNormal;
+
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+		c.depth = depth;
+
+		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
+		if (body_collides) {
+			body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass());
+		}
+		if (soft_body_collides) {
+			soft_body->apply_node_impulse(c.index_B, j_vec);
+		}
+		c.acc_impulse -= j_vec;
+
+		if (body->can_report_contacts()) {
+			Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity();
+			Vector3 crB = soft_body->get_node_velocity(c.index_B);
+			body->add_contact(global_A, -c.normal, depth, body_shape, crA, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crB, c.acc_impulse);
+		}
+		if (report_contacts_only) {
+			collided = false;
+			continue;
+		}
+
+		c.active = true;
+		do_process = true;
+
+		if (body_collides) {
+			body->set_active(true);
+		}
+
+		c.bounce = body->get_bounce();
+
+		if (c.bounce) {
+			Vector3 crA = body->get_angular_velocity().cross(c.rA);
+			Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA;
+
+			// Normal impulse.
+			c.bounce = c.bounce * dv.dot(c.normal);
+		}
+	}
+
+	return do_process;
+}
+
+void GodotBodySoftBodyPair3D::solve(real_t p_step) {
+	if (!collided) {
+		return;
+	}
+
+	const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
+
+	Basis zero_basis;
+	zero_basis.set_zero();
+
+	const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis;
+
+	real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0;
+
+	uint32_t contact_count = contacts.size();
+	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
+		Contact &c = contacts[contact_index];
+		if (!c.active) {
+			continue;
+		}
+
+		c.active = false;
+
+		real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0;
+
+		// Bias impulse.
+		Vector3 crbA = body->get_biased_angular_velocity().cross(c.rA);
+		Vector3 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA;
+
+		real_t vbn = dbv.dot(c.normal);
+
+		if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
+			real_t jbn = (-vbn + c.bias) * c.mass_normal;
+			real_t jbnOld = c.acc_bias_impulse;
+			c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
+
+			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
+
+			if (body_collides) {
+				body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av);
+			}
+			if (soft_body_collides) {
+				soft_body->apply_node_bias_impulse(c.index_B, jb);
+			}
+
+			crbA = body->get_biased_angular_velocity().cross(c.rA);
+			dbv = soft_body->get_node_biased_velocity(c.index_B) - body->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) / (body_inv_mass + node_inv_mass);
+				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);
+
+				Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
+
+				if (body_collides) {
+					body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f);
+				}
+				if (soft_body_collides) {
+					soft_body->apply_node_bias_impulse(c.index_B, jb_com);
+				}
+			}
+
+			c.active = true;
+		}
+
+		Vector3 crA = body->get_angular_velocity().cross(c.rA);
+		Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA;
+
+		// Normal impulse.
+		real_t vn = dv.dot(c.normal);
+
+		if (Math::abs(vn) > MIN_VELOCITY) {
+			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);
+
+			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
+
+			if (body_collides) {
+				body->apply_impulse(-j, c.rA + body->get_center_of_mass());
+			}
+			if (soft_body_collides) {
+				soft_body->apply_node_impulse(c.index_B, j);
+			}
+			c.acc_impulse -= j;
+
+			c.active = true;
+		}
+
+		// Friction impulse.
+		real_t friction = body->get_friction();
+
+		Vector3 lvA = body->get_linear_velocity() + body->get_angular_velocity().cross(c.rA);
+		Vector3 lvB = soft_body->get_node_velocity(c.index_B);
+		Vector3 dtv = lvB - lvA;
+
+		real_t tn = c.normal.dot(dtv);
+
+		// Tangential velocity.
+		Vector3 tv = dtv - c.normal * tn;
+		real_t tvl = tv.length();
+
+		if (tvl > MIN_VELOCITY) {
+			tv /= tvl;
+
+			Vector3 temp1 = body_inv_inertia_tensor.xform(c.rA.cross(tv));
+
+			real_t t = -tvl / (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA)));
+
+			Vector3 jt = t * tv;
+
+			Vector3 jtOld = c.acc_tangent_impulse;
+			c.acc_tangent_impulse += jt;
+
+			real_t fi_len = c.acc_tangent_impulse.length();
+			real_t jtMax = c.acc_normal_impulse * friction;
+
+			if (fi_len > CMP_EPSILON && fi_len > jtMax) {
+				c.acc_tangent_impulse *= jtMax / fi_len;
+			}
+
+			jt = c.acc_tangent_impulse - jtOld;
+
+			if (body_collides) {
+				body->apply_impulse(-jt, c.rA + body->get_center_of_mass());
+			}
+			if (soft_body_collides) {
+				soft_body->apply_node_impulse(c.index_B, jt);
+			}
+			c.acc_impulse -= jt;
+
+			c.active = true;
+		}
+	}
+}
+
+GodotBodySoftBodyPair3D::GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B) :
+		GodotBodyContact3D(&body, 1) {
+	body = p_A;
+	soft_body = p_B;
+	body_shape = p_shape_A;
+	space = p_A->get_space();
+	body->add_constraint(this, 0);
+	soft_body->add_constraint(this);
+}
+
+GodotBodySoftBodyPair3D::~GodotBodySoftBodyPair3D() {
+	body->remove_constraint(this);
+	soft_body->remove_constraint(this);
+}

modules/godot_physics_3d/godot_body_pair_3d.h

@@ -0,0 +1,144 @@
+/**************************************************************************/
+/*  godot_body_pair_3d.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_3d.h"
+#include "godot_constraint_3d.h"
+#include "godot_soft_body_3d.h"
+
+#include "core/templates/local_vector.h"
+
+class GodotBodyContact3D : public GodotConstraint3D {
+protected:
+	struct Contact {
+		Vector3 position;
+		Vector3 normal;
+		int index_A = 0, index_B = 0;
+		Vector3 local_A, local_B;
+		Vector3 acc_impulse; // accumulated impulse - only one of the object's impulse is needed as impulse_a == -impulse_b
+		real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
+		Vector3 acc_tangent_impulse; // 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 = 0.0;
+		real_t bias = 0.0;
+		real_t bounce = 0.0;
+
+		real_t depth = 0.0;
+		bool active = false;
+		bool used = false;
+		Vector3 rA, rB; // Offset in world orientation with respect to center of mass
+	};
+
+	Vector3 sep_axis;
+	bool collided = false;
+	bool check_ccd = false;
+
+	GodotSpace3D *space = nullptr;
+
+	GodotBodyContact3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) :
+			GodotConstraint3D(p_body_ptr, p_body_count) {
+	}
+};
+
+class GodotBodyPair3D : public GodotBodyContact3D {
+	enum {
+		MAX_CONTACTS = 4
+	};
+
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
+	};
+
+	int shape_A = 0;
+	int shape_B = 0;
+
+	bool collide_A = false;
+	bool collide_B = false;
+
+	bool report_contacts_only = false;
+
+	Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
+
+	Contact contacts[MAX_CONTACTS];
+	int contact_count = 0;
+
+	static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
+
+	void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal);
+
+	void validate_contacts();
+	bool _test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_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;
+
+	GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B);
+	~GodotBodyPair3D();
+};
+
+class GodotBodySoftBodyPair3D : public GodotBodyContact3D {
+	GodotBody3D *body = nullptr;
+	GodotSoftBody3D *soft_body = nullptr;
+
+	int body_shape = 0;
+
+	bool body_collides = false;
+	bool soft_body_collides = false;
+
+	bool report_contacts_only = false;
+
+	LocalVector<Contact> contacts;
+
+	static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
+
+	void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal);
+
+	void validate_contacts();
+
+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;
+
+	virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const override { return soft_body; }
+	virtual int get_soft_body_count() const override { return 1; }
+
+	GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B);
+	~GodotBodySoftBodyPair3D();
+};

modules/godot_physics_3d/godot_broad_phase_3d.cpp

@@ -0,0 +1,36 @@
+/**************************************************************************/
+/*  godot_broad_phase_3d.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_3d.h"
+
+GodotBroadPhase3D::CreateFunction GodotBroadPhase3D::create_func = nullptr;
+
+GodotBroadPhase3D::~GodotBroadPhase3D() {
+}

modules/godot_physics_3d/godot_broad_phase_3d.h

@@ -0,0 +1,69 @@
+/**************************************************************************/
+/*  godot_broad_phase_3d.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/aabb.h"
+#include "core/math/math_funcs.h"
+
+class GodotCollisionObject3D;
+
+class GodotBroadPhase3D {
+public:
+	typedef GodotBroadPhase3D *(*CreateFunction)();
+
+	static CreateFunction create_func;
+
+	typedef uint32_t ID;
+
+	typedef void *(*PairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_userdata);
+	typedef void (*UnpairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_userdata);
+
+	// 0 is an invalid ID
+	virtual ID create(GodotCollisionObject3D *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
+	virtual void move(ID p_id, const AABB &p_aabb) = 0;
+	virtual void set_static(ID p_id, bool p_static) = 0;
+	virtual void remove(ID p_id) = 0;
+
+	virtual GodotCollisionObject3D *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_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
+	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
+	virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **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 ~GodotBroadPhase3D();
+};

modules/godot_physics_3d/godot_broad_phase_3d_bvh.cpp

@@ -0,0 +1,128 @@
+/**************************************************************************/
+/*  godot_broad_phase_3d_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_3d_bvh.h"
+
+#include "godot_collision_object_3d.h"
+
+GodotBroadPhase3DBVH::ID GodotBroadPhase3DBVH::create(GodotCollisionObject3D *p_object, int p_subindex, const AABB &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 GodotBroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
+	ERR_FAIL_COND(!p_id);
+	bvh.move(p_id - 1, p_aabb);
+}
+
+void GodotBroadPhase3DBVH::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 GodotBroadPhase3DBVH::remove(ID p_id) {
+	ERR_FAIL_COND(!p_id);
+	bvh.erase(p_id - 1);
+}
+
+GodotCollisionObject3D *GodotBroadPhase3DBVH::get_object(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, nullptr);
+	GodotCollisionObject3D *it = bvh.get(p_id - 1);
+	ERR_FAIL_NULL_V(it, nullptr);
+	return it;
+}
+
+bool GodotBroadPhase3DBVH::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 GodotBroadPhase3DBVH::get_subindex(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, 0);
+	return bvh.get_subindex(p_id - 1);
+}
+
+int GodotBroadPhase3DBVH::cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) {
+	return bvh.cull_point(p_point, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
+}
+
+int GodotBroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **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 GodotBroadPhase3DBVH::cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **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 *GodotBroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B) {
+	GodotBroadPhase3DBVH *bpo = static_cast<GodotBroadPhase3DBVH *>(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 GodotBroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B, void *pairdata) {
+	GodotBroadPhase3DBVH *bpo = static_cast<GodotBroadPhase3DBVH *>(self);
+	if (!bpo->unpair_callback) {
+		return;
+	}
+
+	bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
+}
+
+void GodotBroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
+	pair_callback = p_pair_callback;
+	pair_userdata = p_userdata;
+}
+
+void GodotBroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
+	unpair_callback = p_unpair_callback;
+	unpair_userdata = p_userdata;
+}
+
+void GodotBroadPhase3DBVH::update() {
+	bvh.update();
+}
+
+GodotBroadPhase3D *GodotBroadPhase3DBVH::_create() {
+	return memnew(GodotBroadPhase3DBVH);
+}
+
+GodotBroadPhase3DBVH::GodotBroadPhase3DBVH() {
+	bvh.set_pair_callback(_pair_callback, this);
+	bvh.set_unpair_callback(_unpair_callback, this);
+}

modules/godot_physics_3d/godot_broad_phase_3d_bvh.h

@@ -0,0 +1,97 @@
+/**************************************************************************/
+/*  godot_broad_phase_3d_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_3d.h"
+
+#include "core/math/bvh.h"
+
+class GodotBroadPhase3DBVH : public GodotBroadPhase3D {
+	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<GodotCollisionObject3D, 2, true, 128, UserPairTestFunction<GodotCollisionObject3D>, UserCullTestFunction<GodotCollisionObject3D>> bvh;
+
+	static void *_pair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int);
+	static void _unpair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, 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(GodotCollisionObject3D *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) override;
+	virtual void move(ID p_id, const AABB &p_aabb) override;
+	virtual void set_static(ID p_id, bool p_static) override;
+	virtual void remove(ID p_id) override;
+
+	virtual GodotCollisionObject3D *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_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) override;
+	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) override;
+	virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **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 GodotBroadPhase3D *_create();
+	GodotBroadPhase3DBVH();
+};

modules/godot_physics_3d/godot_collision_object_3d.cpp

@@ -0,0 +1,242 @@
+/**************************************************************************/
+/*  godot_collision_object_3d.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_3d.h"
+
+#include "godot_physics_server_3d.h"
+#include "godot_space_3d.h"
+
+void GodotCollisionObject3D::add_shape(GodotShape3D *p_shape, const Transform3D &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;
+	shapes.push_back(s);
+	p_shape->add_owner(this);
+
+	if (!pending_shape_update_list.in_list()) {
+		GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+}
+
+void GodotCollisionObject3D::set_shape(int p_index, GodotShape3D *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()) {
+		GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+}
+
+void GodotCollisionObject3D::set_shape_transform(int p_index, const Transform3D &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()) {
+		GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+}
+
+void GodotCollisionObject3D::set_shape_disabled(int p_idx, bool p_disabled) {
+	ERR_FAIL_INDEX(p_idx, shapes.size());
+
+	GodotCollisionObject3D::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()) {
+			GodotPhysicsServer3D::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()) {
+			GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+		}
+	}
+}
+
+void GodotCollisionObject3D::remove_shape(GodotShape3D *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 GodotCollisionObject3D::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()) {
+		GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+}
+
+void GodotCollisionObject3D::_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 GodotCollisionObject3D::_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 GodotCollisionObject3D::_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..
+		AABB shape_aabb = s.shape->get_aabb();
+		Transform3D 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;
+
+		Vector3 scale = xform.get_basis().get_scale();
+		s.area_cache = s.shape->get_volume() * scale.x * scale.y * scale.z;
+
+		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 GodotCollisionObject3D::_update_shapes_with_motion(const Vector3 &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..
+		AABB shape_aabb = s.shape->get_aabb();
+		Transform3D xform = transform * s.xform;
+		shape_aabb = xform.xform(shape_aabb);
+		shape_aabb.merge_with(AABB(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 GodotCollisionObject3D::_set_space(GodotSpace3D *p_space) {
+	GodotSpace3D *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 GodotCollisionObject3D::_shape_changed() {
+	_update_shapes();
+	_shapes_changed();
+}
+
+GodotCollisionObject3D::GodotCollisionObject3D(Type p_type) :
+		pending_shape_update_list(this) {
+	type = p_type;
+}

modules/godot_physics_3d/godot_collision_object_3d.h

@@ -0,0 +1,191 @@
+/**************************************************************************/
+/*  godot_collision_object_3d.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_3d.h"
+#include "godot_shape_3d.h"
+
+#include "core/templates/self_list.h"
+#include "servers/physics_server_3d.h"
+
+#ifdef DEBUG_ENABLED
+#define MAX_OBJECT_DISTANCE 3.1622776601683791e+18
+
+#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE)
+#endif
+
+class GodotSpace3D;
+
+class GodotCollisionObject3D : public GodotShapeOwner3D {
+public:
+	enum Type {
+		TYPE_AREA,
+		TYPE_BODY,
+		TYPE_SOFT_BODY,
+	};
+
+private:
+	Type type;
+	RID self;
+	ObjectID instance_id;
+	uint32_t collision_layer = 1;
+	uint32_t collision_mask = 1;
+	real_t collision_priority = 1.0;
+
+	struct Shape {
+		Transform3D xform;
+		Transform3D xform_inv;
+		GodotBroadPhase3D::ID bpid;
+		AABB aabb_cache; //for rayqueries
+		real_t area_cache = 0.0;
+		GodotShape3D *shape = nullptr;
+		bool disabled = false;
+	};
+
+	Vector<Shape> shapes;
+	GodotSpace3D *space = nullptr;
+	Transform3D transform;
+	Transform3D inv_transform;
+	bool _static = true;
+
+	SelfList<GodotCollisionObject3D> pending_shape_update_list;
+
+	void _update_shapes();
+
+protected:
+	void _update_shapes_with_motion(const Vector3 &p_motion);
+	void _unregister_shapes();
+
+	_FORCE_INLINE_ void _set_transform(const Transform3D &p_transform, bool p_update_shapes = true) {
+#ifdef DEBUG_ENABLED
+
+		ERR_FAIL_COND_MSG(p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2, "Object went too far away (more than '" + itos(MAX_OBJECT_DISTANCE) + "' units from origin).");
+#endif
+
+		transform = p_transform;
+		if (p_update_shapes) {
+			_update_shapes();
+		}
+	}
+	_FORCE_INLINE_ void _set_inv_transform(const Transform3D &p_transform) { inv_transform = p_transform; }
+	void _set_static(bool p_static);
+
+	virtual void _shapes_changed() = 0;
+	void _set_space(GodotSpace3D *p_space);
+
+	bool ray_pickable = true;
+
+	GodotCollisionObject3D(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; }
+
+	void _shape_changed() override;
+
+	_FORCE_INLINE_ Type get_type() const { return type; }
+	void add_shape(GodotShape3D *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false);
+	void set_shape(int p_index, GodotShape3D *p_shape);
+	void set_shape_transform(int p_index, const Transform3D &p_transform);
+	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
+	_FORCE_INLINE_ GodotShape3D *get_shape(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].shape;
+	}
+	_FORCE_INLINE_ const Transform3D &get_shape_transform(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].xform;
+	}
+	_FORCE_INLINE_ const Transform3D &get_shape_inv_transform(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].xform_inv;
+	}
+	_FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].aabb_cache;
+	}
+	_FORCE_INLINE_ real_t get_shape_area(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].area_cache;
+	}
+
+	_FORCE_INLINE_ const Transform3D &get_transform() const { return transform; }
+	_FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; }
+	_FORCE_INLINE_ GodotSpace3D *get_space() const { return space; }
+
+	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
+	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
+
+	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_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_mask(uint32_t p_mask) {
+		collision_mask = p_mask;
+		_shape_changed();
+	}
+	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
+
+	_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; }
+
+	_FORCE_INLINE_ bool collides_with(GodotCollisionObject3D *p_other) const {
+		return p_other->collision_layer & collision_mask;
+	}
+
+	_FORCE_INLINE_ bool interacts_with(const GodotCollisionObject3D *p_other) const {
+		return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
+	}
+
+	void remove_shape(GodotShape3D *p_shape) override;
+	void remove_shape(int p_index);
+
+	virtual void set_space(GodotSpace3D *p_space) = 0;
+
+	_FORCE_INLINE_ bool is_static() const { return _static; }
+
+	virtual ~GodotCollisionObject3D() {}
+};

modules/godot_physics_3d/godot_collision_solver_3d.cpp

@@ -0,0 +1,589 @@
+/**************************************************************************/
+/*  godot_collision_solver_3d.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_3d.h"
+
+#include "godot_collision_solver_3d_sat.h"
+#include "godot_soft_body_3d.h"
+
+#include "gjk_epa.h"
+
+#define collision_solver sat_calculate_penetration
+//#define collision_solver gjk_epa_calculate_penetration
+
+bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
+	const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A);
+	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
+		return false;
+	}
+	Plane p = p_transform_A.xform(world_boundary->get_plane());
+
+	static const int max_supports = 16;
+	Vector3 supports[max_supports];
+	int support_count;
+	GodotShape3D::FeatureType support_type = GodotShape3D::FeatureType::FEATURE_POINT;
+	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
+
+	if (support_type == GodotShape3D::FEATURE_CIRCLE) {
+		ERR_FAIL_COND_V(support_count != 3, false);
+
+		Vector3 circle_pos = supports[0];
+		Vector3 circle_axis_1 = supports[1] - circle_pos;
+		Vector3 circle_axis_2 = supports[2] - circle_pos;
+
+		// Use 3 equidistant points on the circle.
+		for (int i = 0; i < 3; ++i) {
+			Vector3 vertex_pos = circle_pos;
+			vertex_pos += circle_axis_1 * Math::cos(2.0 * Math::PI * i / 3.0);
+			vertex_pos += circle_axis_2 * Math::sin(2.0 * Math::PI * i / 3.0);
+			supports[i] = vertex_pos;
+		}
+	}
+
+	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]);
+		if (p.distance_to(supports[i]) >= 0) {
+			continue;
+		}
+		found = true;
+
+		Vector3 support_A = p.project(supports[i]);
+
+		if (p_result_callback) {
+			if (p_swap_result) {
+				Vector3 normal = (support_A - supports[i]).normalized();
+				p_result_callback(supports[i], 0, support_A, 0, normal, p_userdata);
+			} else {
+				Vector3 normal = (supports[i] - support_A).normalized();
+				p_result_callback(support_A, 0, supports[i], 0, normal, p_userdata);
+			}
+		}
+	}
+
+	return found;
+}
+
+bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
+	const GodotSeparationRayShape3D *ray = static_cast<const GodotSeparationRayShape3D *>(p_shape_A);
+
+	Vector3 from = p_transform_A.origin;
+	Vector3 to = from + p_transform_A.basis.get_column(2) * (ray->get_length() + p_margin);
+	Vector3 support_A = to;
+
+	Transform3D ai = p_transform_B.affine_inverse();
+
+	from = ai.xform(from);
+	to = ai.xform(to);
+
+	Vector3 p, n;
+	int fi = -1;
+	if (!p_shape_B->intersect_segment(from, to, p, n, fi, true)) {
+		return false;
+	}
+
+	// Discard contacts when the ray is fully contained inside the shape.
+	if (n == Vector3()) {
+		return false;
+	}
+
+	// Discard contacts in the wrong direction.
+	if (n.dot(from - to) < CMP_EPSILON) {
+		return false;
+	}
+
+	Vector3 support_B = p_transform_B.xform(p);
+	if (ray->get_slide_on_slope()) {
+		Vector3 global_n = ai.basis.xform_inv(n).normalized();
+		support_B = support_A + (support_B - support_A).length() * global_n;
+	}
+
+	if (p_result_callback) {
+		Vector3 normal = (support_B - support_A).normalized();
+		if (p_swap_result) {
+			p_result_callback(support_B, 0, support_A, 0, -normal, p_userdata);
+		} else {
+			p_result_callback(support_A, 0, support_B, 0, normal, p_userdata);
+		}
+	}
+	return true;
+}
+
+struct _SoftBodyContactCollisionInfo {
+	int node_index = 0;
+	GodotCollisionSolver3D::CallbackResult result_callback = nullptr;
+	void *userdata = nullptr;
+	bool swap_result = false;
+	int contact_count = 0;
+};
+
+void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
+	_SoftBodyContactCollisionInfo &cinfo = *(static_cast<_SoftBodyContactCollisionInfo *>(p_userdata));
+
+	++cinfo.contact_count;
+
+	if (!cinfo.result_callback) {
+		return;
+	}
+
+	if (cinfo.swap_result) {
+		cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, -normal, cinfo.userdata);
+	} else {
+		cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, normal, cinfo.userdata);
+	}
+}
+
+struct _SoftBodyQueryInfo {
+	GodotSoftBody3D *soft_body = nullptr;
+	const GodotShape3D *shape_A = nullptr;
+	const GodotShape3D *shape_B = nullptr;
+	Transform3D transform_A;
+	Transform3D node_transform;
+	_SoftBodyContactCollisionInfo contact_info;
+#ifdef DEBUG_ENABLED
+	int node_query_count = 0;
+	int convex_query_count = 0;
+#endif
+};
+
+bool GodotCollisionSolver3D::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) {
+	_SoftBodyQueryInfo &query_cinfo = *(static_cast<_SoftBodyQueryInfo *>(p_userdata));
+
+	Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index);
+
+	Transform3D transform_B;
+	transform_B.origin = query_cinfo.node_transform.xform(node_position);
+
+	query_cinfo.contact_info.node_index = p_node_index;
+	bool collided = solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info);
+
+#ifdef DEBUG_ENABLED
+	++query_cinfo.node_query_count;
+#endif
+
+	// Stop at first collision if contacts are not needed.
+	return (collided && !query_cinfo.contact_info.result_callback);
+}
+
+bool GodotCollisionSolver3D::soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex) {
+	_SoftBodyQueryInfo &query_cinfo = *(static_cast<_SoftBodyQueryInfo *>(p_userdata));
+
+	query_cinfo.shape_A = p_convex;
+
+	// Calculate AABB for internal soft body query (in world space).
+	AABB shape_aabb;
+	for (int i = 0; i < 3; i++) {
+		Vector3 axis;
+		axis[i] = 1.0;
+
+		real_t smin, smax;
+		p_convex->project_range(axis, query_cinfo.transform_A, smin, smax);
+
+		shape_aabb.position[i] = smin;
+		shape_aabb.size[i] = smax - smin;
+	}
+
+	shape_aabb.grow_by(query_cinfo.soft_body->get_collision_margin());
+
+	query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo);
+
+	bool collided = (query_cinfo.contact_info.contact_count > 0);
+
+#ifdef DEBUG_ENABLED
+	++query_cinfo.convex_query_count;
+#endif
+
+	// Stop at first collision if contacts are not needed.
+	return (collided && !query_cinfo.contact_info.result_callback);
+}
+
+bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
+	const GodotSoftBodyShape3D *soft_body_shape_B = static_cast<const GodotSoftBodyShape3D *>(p_shape_B);
+
+	GodotSoftBody3D *soft_body = soft_body_shape_B->get_soft_body();
+	const Transform3D &world_to_local = soft_body->get_inv_transform();
+
+	const real_t collision_margin = soft_body->get_collision_margin();
+
+	GodotSphereShape3D sphere_shape;
+	sphere_shape.set_data(collision_margin);
+
+	_SoftBodyQueryInfo query_cinfo;
+	query_cinfo.contact_info.result_callback = p_result_callback;
+	query_cinfo.contact_info.userdata = p_userdata;
+	query_cinfo.contact_info.swap_result = p_swap_result;
+	query_cinfo.soft_body = soft_body;
+	query_cinfo.node_transform = p_transform_B * world_to_local;
+	query_cinfo.shape_A = p_shape_A;
+	query_cinfo.transform_A = p_transform_A;
+	query_cinfo.shape_B = &sphere_shape;
+
+	if (p_shape_A->is_concave()) {
+		// In case of concave shape, query convex shapes first.
+		const GodotConcaveShape3D *concave_shape_A = static_cast<const GodotConcaveShape3D *>(p_shape_A);
+
+		AABB soft_body_aabb = soft_body->get_bounds();
+		soft_body_aabb.grow_by(collision_margin);
+
+		// Calculate AABB for internal concave shape query (in local space).
+		AABB local_aabb;
+		for (int i = 0; i < 3; i++) {
+			Vector3 axis(p_transform_A.basis.get_column(i));
+			real_t axis_scale = 1.0 / axis.length();
+
+			real_t smin = soft_body_aabb.position[i];
+			real_t smax = smin + soft_body_aabb.size[i];
+
+			smin *= axis_scale;
+			smax *= axis_scale;
+
+			local_aabb.position[i] = smin;
+			local_aabb.size[i] = smax - smin;
+		}
+
+		concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo, true);
+	} else {
+		AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb());
+		shape_aabb.grow_by(collision_margin);
+
+		soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo);
+	}
+
+	return (query_cinfo.contact_info.contact_count > 0);
+}
+
+struct _ConcaveCollisionInfo {
+	const Transform3D *transform_A = nullptr;
+	const GodotShape3D *shape_A = nullptr;
+	const Transform3D *transform_B = nullptr;
+	GodotCollisionSolver3D::CallbackResult result_callback = nullptr;
+	void *userdata = nullptr;
+	bool swap_result = false;
+	bool collided = false;
+	int aabb_tests = 0;
+	int collisions = 0;
+	bool tested = false;
+	real_t margin_A = 0.0f;
+	real_t margin_B = 0.0f;
+	Vector3 close_A;
+	Vector3 close_B;
+};
+
+bool GodotCollisionSolver3D::concave_callback(void *p_userdata, GodotShape3D *p_convex) {
+	_ConcaveCollisionInfo &cinfo = *(static_cast<_ConcaveCollisionInfo *>(p_userdata));
+	cinfo.aabb_tests++;
+
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, nullptr, 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 GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) {
+	const GodotConcaveShape3D *concave_B = static_cast<const GodotConcaveShape3D *>(p_shape_B);
+
+	_ConcaveCollisionInfo cinfo;
+	cinfo.transform_A = &p_transform_A;
+	cinfo.shape_A = p_shape_A;
+	cinfo.transform_B = &p_transform_B;
+	cinfo.result_callback = p_result_callback;
+	cinfo.userdata = p_userdata;
+	cinfo.swap_result = p_swap_result;
+	cinfo.collided = false;
+	cinfo.collisions = 0;
+	cinfo.margin_A = p_margin_A;
+	cinfo.margin_B = p_margin_B;
+
+	cinfo.aabb_tests = 0;
+
+	Transform3D rel_transform = p_transform_A;
+	rel_transform.origin -= p_transform_B.origin;
+
+	//quickly compute a local AABB
+
+	AABB local_aabb;
+	for (int i = 0; i < 3; i++) {
+		Vector3 axis(p_transform_B.basis.get_column(i));
+		real_t axis_scale = 1.0 / axis.length();
+		axis *= axis_scale;
+
+		real_t smin = 0.0, smax = 0.0;
+		p_shape_A->project_range(axis, rel_transform, smin, smax);
+		smin -= p_margin_A;
+		smax += p_margin_A;
+		smin *= axis_scale;
+		smax *= axis_scale;
+
+		local_aabb.position[i] = smin;
+		local_aabb.size[i] = smax - smin;
+	}
+
+	concave_B->cull(local_aabb, concave_callback, &cinfo, false);
+
+	return cinfo.collided;
+}
+
+bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
+	PhysicsServer3D::ShapeType type_A = p_shape_A->get_type();
+	PhysicsServer3D::ShapeType type_B = p_shape_B->get_type();
+	bool concave_A = p_shape_A->is_concave();
+	bool concave_B = p_shape_B->is_concave();
+
+	bool swap = false;
+
+	if (type_A > type_B) {
+		SWAP(type_A, type_B);
+		SWAP(concave_A, concave_B);
+		swap = true;
+	}
+
+	if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
+		if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
+			WARN_PRINT_ONCE("Collisions between world boundaries are not supported.");
+			return false;
+		}
+		if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) {
+			WARN_PRINT_ONCE("Collisions between world boundaries and rays are not supported.");
+			return false;
+		}
+		if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
+			WARN_PRINT_ONCE("Collisions between world boundaries and soft bodies 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_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_result_callback, p_userdata, false, p_margin_B);
+		}
+
+	} else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) {
+		if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) {
+			WARN_PRINT_ONCE("Collisions between rays are not supported.");
+			return false;
+		}
+
+		if (swap) {
+			return solve_separation_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B);
+		} else {
+			return solve_separation_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A);
+		}
+
+	} else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
+		if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) {
+			WARN_PRINT_ONCE("Collisions between soft bodies are not supported.");
+			return false;
+		}
+
+		if (swap) {
+			return solve_soft_body(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
+		} else {
+			return solve_soft_body(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
+		}
+
+	} 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_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B);
+		} else {
+			return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B);
+		}
+
+	} else {
+		return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B);
+	}
+}
+
+bool GodotCollisionSolver3D::concave_distance_callback(void *p_userdata, GodotShape3D *p_convex) {
+	_ConcaveCollisionInfo &cinfo = *(static_cast<_ConcaveCollisionInfo *>(p_userdata));
+	cinfo.aabb_tests++;
+
+	Vector3 close_A, close_B;
+	cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B);
+
+	if (cinfo.collided) {
+		// No need to process any more result.
+		return true;
+	}
+
+	if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) {
+		cinfo.close_A = close_A;
+		cinfo.close_B = close_B;
+		cinfo.tested = true;
+	}
+
+	cinfo.collisions++;
+	return false;
+}
+
+bool GodotCollisionSolver3D::solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
+	const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A);
+	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
+		return false;
+	}
+	Plane p = p_transform_A.xform(world_boundary->get_plane());
+
+	static const int max_supports = 16;
+	Vector3 supports[max_supports];
+	int support_count;
+	GodotShape3D::FeatureType support_type;
+	Vector3 support_direction = p_transform_B.basis.xform_inv(-p.normal).normalized();
+
+	p_shape_B->get_supports(support_direction, max_supports, supports, support_count, support_type);
+
+	if (support_count == 0) { // This is a poor man's way to detect shapes that don't implement get_supports, such as GodotMotionShape3D.
+		Vector3 support_B = p_transform_B.xform(p_shape_B->get_support(support_direction));
+		r_point_A = p.project(support_B);
+		r_point_B = support_B;
+		bool collided = p.distance_to(support_B) <= 0;
+		return collided;
+	}
+
+	if (support_type == GodotShape3D::FEATURE_CIRCLE) {
+		ERR_FAIL_COND_V(support_count != 3, false);
+
+		Vector3 circle_pos = supports[0];
+		Vector3 circle_axis_1 = supports[1] - circle_pos;
+		Vector3 circle_axis_2 = supports[2] - circle_pos;
+
+		// Use 3 equidistant points on the circle.
+		for (int i = 0; i < 3; ++i) {
+			Vector3 vertex_pos = circle_pos;
+			vertex_pos += circle_axis_1 * Math::cos(2.0 * Math::PI * i / 3.0);
+			vertex_pos += circle_axis_2 * Math::sin(2.0 * Math::PI * i / 3.0);
+			supports[i] = vertex_pos;
+		}
+	}
+
+	bool collided = false;
+	Vector3 closest;
+	real_t closest_d = 0;
+
+	for (int i = 0; i < support_count; i++) {
+		supports[i] = p_transform_B.xform(supports[i]);
+		real_t d = p.distance_to(supports[i]);
+		if (i == 0 || d < closest_d) {
+			closest = supports[i];
+			closest_d = d;
+			if (d <= 0) {
+				collided = true;
+			}
+		}
+	}
+
+	r_point_A = p.project(closest);
+	r_point_B = closest;
+
+	return collided;
+}
+
+bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) {
+	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
+		Vector3 a, b;
+		bool col = solve_distance_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b);
+		r_point_A = b;
+		r_point_B = a;
+		return !col;
+
+	} else if (p_shape_B->is_concave()) {
+		if (p_shape_A->is_concave()) {
+			return false;
+		}
+
+		const GodotConcaveShape3D *concave_B = static_cast<const GodotConcaveShape3D *>(p_shape_B);
+
+		_ConcaveCollisionInfo cinfo;
+		cinfo.transform_A = &p_transform_A;
+		cinfo.shape_A = p_shape_A;
+		cinfo.transform_B = &p_transform_B;
+		cinfo.result_callback = nullptr;
+		cinfo.userdata = nullptr;
+		cinfo.swap_result = false;
+		cinfo.collided = false;
+		cinfo.collisions = 0;
+		cinfo.aabb_tests = 0;
+		cinfo.tested = false;
+
+		Transform3D rel_transform = p_transform_A;
+		rel_transform.origin -= p_transform_B.origin;
+
+		//quickly compute a local AABB
+
+		bool use_cc_hint = p_concave_hint != AABB();
+		AABB cc_hint_aabb;
+		if (use_cc_hint) {
+			cc_hint_aabb = p_concave_hint;
+			cc_hint_aabb.position -= p_transform_B.origin;
+		}
+
+		AABB local_aabb;
+		for (int i = 0; i < 3; i++) {
+			Vector3 axis(p_transform_B.basis.get_column(i));
+			real_t axis_scale = ((real_t)1.0) / axis.length();
+			axis *= axis_scale;
+
+			real_t smin, smax;
+
+			if (use_cc_hint) {
+				cc_hint_aabb.project_range_in_plane(Plane(axis), smin, smax);
+			} else {
+				p_shape_A->project_range(axis, rel_transform, smin, smax);
+			}
+
+			smin *= axis_scale;
+			smax *= axis_scale;
+
+			local_aabb.position[i] = smin;
+			local_aabb.size[i] = smax - smin;
+		}
+
+		concave_B->cull(local_aabb, concave_distance_callback, &cinfo, false);
+		if (!cinfo.collided) {
+			r_point_A = cinfo.close_A;
+			r_point_B = cinfo.close_B;
+		}
+
+		return !cinfo.collided;
+	} else {
+		return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis..
+	}
+}

modules/godot_physics_3d/godot_collision_solver_3d.h

@@ -0,0 +1,54 @@
+/**************************************************************************/
+/*  godot_collision_solver_3d.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_3d.h"
+
+class GodotCollisionSolver3D {
+public:
+	typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
+
+private:
+	static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata);
+	static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
+	static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex);
+	static bool concave_callback(void *p_userdata, GodotShape3D *p_convex);
+	static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
+	static bool solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
+	static bool solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
+	static bool solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0);
+	static bool concave_distance_callback(void *p_userdata, GodotShape3D *p_convex);
+	static bool solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
+
+public:
+	static bool solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
+	static bool solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr);
+};

modules/godot_physics_3d/godot_collision_solver_3d_sat.h

@@ -0,0 +1,35 @@
+/**************************************************************************/
+/*  godot_collision_solver_3d_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_3d.h"
+
+bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0);

modules/godot_physics_3d/godot_constraint_3d.h

@@ -0,0 +1,78 @@
+/**************************************************************************/
+/*  godot_constraint_3d.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
+
+class GodotBody3D;
+class GodotSoftBody3D;
+
+class GodotConstraint3D {
+	GodotBody3D **_body_ptr;
+	int _body_count;
+	uint64_t island_step;
+	int priority;
+	bool disabled_collisions_between_bodies;
+
+	RID self;
+
+protected:
+	GodotConstraint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) {
+		_body_ptr = p_body_ptr;
+		_body_count = p_body_count;
+		island_step = 0;
+		priority = 1;
+		disabled_collisions_between_bodies = true;
+	}
+
+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_ GodotBody3D **get_body_ptr() const { return _body_ptr; }
+	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
+
+	virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const { return nullptr; }
+	virtual int get_soft_body_count() const { return 0; }
+
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
+	_FORCE_INLINE_ int get_priority() const { return priority; }
+
+	_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 ~GodotConstraint3D() {}
+};

modules/godot_physics_3d/godot_joint_3d.h

@@ -0,0 +1,98 @@
+/**************************************************************************/
+/*  godot_joint_3d.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_3d.h"
+#include "godot_constraint_3d.h"
+
+class GodotJoint3D : public GodotConstraint3D {
+protected:
+	bool dynamic_A = false;
+	bool dynamic_B = false;
+
+	void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
+		if (Math::abs(n.z) > Math::SQRT12) {
+			// choose p in y-z plane
+			real_t a = n[1] * n[1] + n[2] * n[2];
+			real_t k = 1.0 / Math::sqrt(a);
+			p = Vector3(0, -n[2] * k, n[1] * k);
+			// set q = n x p
+			q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]);
+		} else {
+			// choose p in x-y plane
+			real_t a = n.x * n.x + n.y * n.y;
+			real_t k = 1.0 / Math::sqrt(a);
+			p = Vector3(-n.y * k, n.x * k, 0);
+			// set q = n x p
+			q = Vector3(-n.z * p.y, n.z * p.x, a * k);
+		}
+	}
+
+	_FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
+		real_t coeff_1 = Math::PI / 4.0f;
+		real_t coeff_2 = 3.0f * coeff_1;
+		real_t abs_y = Math::abs(y);
+		real_t angle;
+		if (x >= 0.0f) {
+			real_t r = (x - abs_y) / (x + abs_y);
+			angle = coeff_1 - coeff_1 * r;
+		} else {
+			real_t r = (x + abs_y) / (abs_y - x);
+			angle = coeff_2 - coeff_1 * r;
+		}
+		return (y < 0.0f) ? -angle : angle;
+	}
+
+public:
+	virtual bool setup(real_t p_step) override { return false; }
+	virtual bool pre_solve(real_t p_step) override { return true; }
+	virtual void solve(real_t p_step) override {}
+
+	void copy_settings_from(GodotJoint3D *p_joint) {
+		set_self(p_joint->get_self());
+		set_priority(p_joint->get_priority());
+		disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies());
+	}
+
+	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; }
+	_FORCE_INLINE_ GodotJoint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) :
+			GodotConstraint3D(p_body_ptr, p_body_count) {
+	}
+
+	virtual ~GodotJoint3D() {
+		for (int i = 0; i < get_body_count(); i++) {
+			GodotBody3D *body = get_body_ptr()[i];
+			if (body) {
+				body->remove_constraint(this);
+			}
+		}
+	}
+};

modules/godot_physics_3d/godot_physics_server_3d.h

@@ -0,0 +1,394 @@
+/**************************************************************************/
+/*  godot_physics_server_3d.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_joint_3d.h"
+#include "godot_shape_3d.h"
+#include "godot_space_3d.h"
+#include "godot_step_3d.h"
+
+#include "core/templates/rid_owner.h"
+#include "servers/physics_server_3d.h"
+
+class GodotPhysicsServer3D : public PhysicsServer3D {
+	GDCLASS(GodotPhysicsServer3D, PhysicsServer3D);
+
+	friend class GodotPhysicsDirectSpaceState3D;
+	bool active = true;
+
+	int island_count = 0;
+	int active_objects = 0;
+	int collision_pairs = 0;
+
+	bool using_threads = false;
+	bool doing_sync = false;
+	bool flushing_queries = false;
+
+	GodotStep3D *stepper = nullptr;
+	HashSet<GodotSpace3D *> active_spaces;
+
+	mutable RID_PtrOwner<GodotShape3D, true> shape_owner;
+	mutable RID_PtrOwner<GodotSpace3D, true> space_owner;
+	mutable RID_PtrOwner<GodotArea3D, true> area_owner;
+	mutable RID_PtrOwner<GodotBody3D, true> body_owner{ 65536, 1048576 };
+	mutable RID_PtrOwner<GodotSoftBody3D, true> soft_body_owner;
+	mutable RID_PtrOwner<GodotJoint3D, true> joint_owner;
+
+	//void _clear_query(QuerySW *p_query);
+	friend class GodotCollisionObject3D;
+	SelfList<GodotCollisionObject3D>::List pending_shape_update_list;
+	void _update_shapes();
+
+	static GodotPhysicsServer3D *godot_singleton;
+
+public:
+	struct CollCbkData {
+		int max;
+		int amount;
+		Vector3 *ptr = nullptr;
+	};
+
+	static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
+
+	virtual RID world_boundary_shape_create() override;
+	virtual RID separation_ray_shape_create() override;
+	virtual RID sphere_shape_create() override;
+	virtual RID box_shape_create() override;
+	virtual RID capsule_shape_create() override;
+	virtual RID cylinder_shape_create() override;
+	virtual RID convex_polygon_shape_create() override;
+	virtual RID concave_polygon_shape_create() override;
+	virtual RID heightmap_shape_create() override;
+	virtual RID custom_shape_create() override;
+
+	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 void shape_set_margin(RID p_shape, real_t p_margin) override;
+	virtual real_t shape_get_margin(RID p_shape) const override;
+
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const 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;
+
+	// this function only works on physics process, errors and returns null otherwise
+	virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override;
+
+	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override;
+	virtual Vector<Vector3> space_get_contacts(RID p_space) const override;
+	virtual int space_get_contact_count(RID p_space) const 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 Transform3D &p_transform = Transform3D(), 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 Transform3D &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 Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const 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_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) 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_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override;
+	virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override;
+
+	virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override;
+	virtual Transform3D area_get_transform(RID p_area) const override;
+
+	virtual void area_set_ray_pickable(RID p_area, bool p_enable) 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_monitorable(RID p_area, bool p_monitorable) 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;
+
+	/* 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 Transform3D &p_transform = Transform3D(), 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 Transform3D &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 Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override;
+
+	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) 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_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_set_enable_continuous_collision_detection(RID p_body, bool p_enable) override;
+	virtual bool body_is_continuous_collision_detection_enabled(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_user_flags(RID p_body, uint32_t p_flags) override;
+	virtual uint32_t body_get_user_flags(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 Vector3 &p_impulse) override;
+	virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override;
+	virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override;
+
+	virtual void soft_body_apply_point_impulse(RID p_body, int p_point_index, const Vector3 &p_impulse) override;
+	virtual void soft_body_apply_point_force(RID p_body, int p_point_index, const Vector3 &p_force) override;
+	virtual void soft_body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override;
+	virtual void soft_body_apply_central_force(RID p_body, const Vector3 &p_force) override;
+
+	virtual void body_apply_central_force(RID p_body, const Vector3 &p_force) override;
+	virtual void body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void body_apply_torque(RID p_body, const Vector3 &p_torque) override;
+
+	virtual void body_add_constant_central_force(RID p_body, const Vector3 &p_force) override;
+	virtual void body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void body_add_constant_torque(RID p_body, const Vector3 &p_torque) override;
+
+	virtual void body_set_constant_force(RID p_body, const Vector3 &p_force) override;
+	virtual Vector3 body_get_constant_force(RID p_body) const override;
+
+	virtual void body_set_constant_torque(RID p_body, const Vector3 &p_torque) override;
+	virtual Vector3 body_get_constant_torque(RID p_body) const override;
+
+	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override;
+
+	virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) override;
+	virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const 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 void body_set_ray_pickable(RID p_body, bool p_enable) 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 PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override;
+
+	/* SOFT BODY */
+
+	virtual RID soft_body_create() override;
+
+	virtual void soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) override;
+
+	virtual void soft_body_set_space(RID p_body, RID p_space) override;
+	virtual RID soft_body_get_space(RID p_body) const override;
+
+	virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override;
+	virtual uint32_t soft_body_get_collision_layer(RID p_body) const override;
+
+	virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override;
+	virtual uint32_t soft_body_get_collision_mask(RID p_body) const override;
+
+	virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override;
+	virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override;
+	virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
+
+	virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
+	virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override;
+
+	virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override;
+
+	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override;
+
+	virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override;
+	virtual int soft_body_get_simulation_precision(RID p_body) const override;
+
+	virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override;
+	virtual real_t soft_body_get_total_mass(RID p_body) const override;
+
+	virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override;
+	virtual real_t soft_body_get_linear_stiffness(RID p_body) const override;
+
+	virtual void soft_body_set_shrinking_factor(RID p_body, real_t p_shrinking_factor) override;
+	virtual real_t soft_body_get_shrinking_factor(RID p_body) const override;
+
+	virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override;
+	virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override;
+
+	virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override;
+	virtual real_t soft_body_get_damping_coefficient(RID p_body) const override;
+
+	virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override;
+	virtual real_t soft_body_get_drag_coefficient(RID p_body) const override;
+
+	virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override;
+
+	virtual AABB soft_body_get_bounds(RID p_body) const override;
+
+	virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override;
+	virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override;
+
+	virtual void soft_body_remove_all_pinned_points(RID p_body) override;
+	virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override;
+	virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override;
+
+	/* JOINT API */
+
+	virtual RID joint_create() override;
+
+	virtual void joint_clear(RID p_joint) override; //resets type
+
+	virtual void joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) 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 pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) override;
+	virtual Vector3 pin_joint_get_local_a(RID p_joint) const override;
+
+	virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override;
+	virtual Vector3 pin_joint_get_local_b(RID p_joint) const override;
+
+	virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) override;
+	virtual void joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override;
+
+	virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override;
+	virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const override;
+
+	virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override;
+	virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override;
+
+	virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A
+
+	virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override;
+	virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override;
+
+	virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A
+
+	virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override;
+	virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override;
+
+	virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A
+
+	virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value) override;
+	virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override;
+
+	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override;
+	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override;
+
+	virtual JointType joint_get_type(RID p_joint) const override;
+
+	virtual void joint_set_solver_priority(RID p_joint, int p_priority) override;
+	virtual int joint_get_solver_priority(RID p_joint) const override;
+
+	virtual void joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) override;
+	virtual bool joint_is_disabled_collisions_between_bodies(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 int space_get_last_process_info(RID p_space, ProcessInfo p_info) 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;
+
+	GodotPhysicsServer3D(bool p_using_threads = false);
+	~GodotPhysicsServer3D() {}
+};

modules/godot_physics_3d/godot_shape_3d.h

@@ -0,0 +1,511 @@
+/**************************************************************************/
+/*  godot_shape_3d.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/geometry_3d.h"
+#include "core/templates/local_vector.h"
+#include "servers/physics_server_3d.h"
+
+class GodotShape3D;
+
+class GodotShapeOwner3D {
+public:
+	virtual void _shape_changed() = 0;
+	virtual void remove_shape(GodotShape3D *p_shape) = 0;
+
+	virtual ~GodotShapeOwner3D() {}
+};
+
+class GodotShape3D {
+	RID self;
+	AABB aabb;
+	bool configured = false;
+	real_t custom_bias = 0.0;
+
+	HashMap<GodotShapeOwner3D *, int> owners;
+
+protected:
+	void configure(const AABB &p_aabb);
+
+public:
+	enum FeatureType {
+		FEATURE_POINT,
+		FEATURE_EDGE,
+		FEATURE_FACE,
+		FEATURE_CIRCLE,
+	};
+
+	virtual real_t get_volume() const { return aabb.get_volume(); }
+
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	virtual PhysicsServer3D::ShapeType get_type() const = 0;
+
+	_FORCE_INLINE_ const AABB &get_aabb() const { return aabb; }
+	_FORCE_INLINE_ bool is_configured() const { return configured; }
+
+	virtual bool is_concave() const { return false; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const = 0;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const = 0;
+	virtual bool intersect_point(const Vector3 &p_point) const = 0;
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) 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(GodotShapeOwner3D *p_owner);
+	void remove_owner(GodotShapeOwner3D *p_owner);
+	bool is_owner(GodotShapeOwner3D *p_owner) const;
+	const HashMap<GodotShapeOwner3D *, int> &get_owners() const;
+
+	GodotShape3D() {}
+	virtual ~GodotShape3D();
+};
+
+class GodotConcaveShape3D : public GodotShape3D {
+public:
+	virtual bool is_concave() const override { return true; }
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
+
+	// Returns true to stop the query.
+	typedef bool (*QueryCallback)(void *p_userdata, GodotShape3D *p_convex);
+
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const = 0;
+
+	GodotConcaveShape3D() {}
+};
+
+class GodotWorldBoundaryShape3D : public GodotShape3D {
+	Plane plane;
+
+	void _setup(const Plane &p_plane);
+
+public:
+	Plane get_plane() const;
+
+	virtual real_t get_volume() const override { return Math::INF; }
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_WORLD_BOUNDARY; }
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
+
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotWorldBoundaryShape3D();
+};
+
+class GodotSeparationRayShape3D : public GodotShape3D {
+	real_t length = 1.0;
+	bool slide_on_slope = false;
+
+	void _setup(real_t p_length, bool p_slide_on_slope);
+
+public:
+	real_t get_length() const;
+	bool get_slide_on_slope() const;
+
+	virtual real_t get_volume() const override { return 0.0; }
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SEPARATION_RAY; }
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
+
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotSeparationRayShape3D();
+};
+
+class GodotSphereShape3D : public GodotShape3D {
+	real_t radius = 0.0;
+
+	void _setup(real_t p_radius);
+
+public:
+	real_t get_radius() const;
+
+	virtual real_t get_volume() const override { return 4.0 / 3.0 * Math::PI * radius * radius * radius; }
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SPHERE; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotSphereShape3D();
+};
+
+class GodotBoxShape3D : public GodotShape3D {
+	Vector3 half_extents;
+	void _setup(const Vector3 &p_half_extents);
+
+public:
+	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
+	virtual real_t get_volume() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; }
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_BOX; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotBoxShape3D();
+};
+
+class GodotCapsuleShape3D : public GodotShape3D {
+	real_t height = 0.0;
+	real_t radius = 0.0;
+
+	void _setup(real_t p_height, real_t p_radius);
+
+public:
+	_FORCE_INLINE_ real_t get_height() const { return height; }
+	_FORCE_INLINE_ real_t get_radius() const { return radius; }
+
+	virtual real_t get_volume() const override { return 4.0 / 3.0 * Math::PI * radius * radius * radius + (height - radius * 2.0) * Math::PI * radius * radius; }
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CAPSULE; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotCapsuleShape3D();
+};
+
+class GodotCylinderShape3D : public GodotShape3D {
+	real_t height = 0.0;
+	real_t radius = 0.0;
+
+	void _setup(real_t p_height, real_t p_radius);
+
+public:
+	_FORCE_INLINE_ real_t get_height() const { return height; }
+	_FORCE_INLINE_ real_t get_radius() const { return radius; }
+
+	virtual real_t get_volume() const override { return height * Math::PI * radius * radius; }
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CYLINDER; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotCylinderShape3D();
+};
+
+struct GodotConvexPolygonShape3D : public GodotShape3D {
+	Geometry3D::MeshData mesh;
+	LocalVector<int> extreme_vertices;
+	LocalVector<LocalVector<int>> vertex_neighbors;
+
+	void _setup(const Vector<Vector3> &p_vertices);
+
+public:
+	const Geometry3D::MeshData &get_mesh() const { return mesh; }
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotConvexPolygonShape3D();
+};
+
+struct _Volume_BVH;
+struct GodotFaceShape3D;
+
+struct GodotConcavePolygonShape3D : public GodotConcaveShape3D {
+	// always a trimesh
+
+	struct Face {
+		Vector3 normal;
+		int indices[3] = {};
+	};
+
+	Vector<Face> faces;
+	Vector<Vector3> vertices;
+
+	struct BVH {
+		AABB aabb;
+		int left = 0;
+		int right = 0;
+
+		int face_index = 0;
+	};
+
+	Vector<BVH> bvh;
+
+	struct _CullParams {
+		AABB aabb;
+		QueryCallback callback = nullptr;
+		void *userdata = nullptr;
+		const Face *faces = nullptr;
+		const Vector3 *vertices = nullptr;
+		const BVH *bvh = nullptr;
+		GodotFaceShape3D *face = nullptr;
+	};
+
+	struct _SegmentCullParams {
+		Vector3 from;
+		Vector3 to;
+		Vector3 dir;
+		const Face *faces = nullptr;
+		const Vector3 *vertices = nullptr;
+		const BVH *bvh = nullptr;
+		GodotFaceShape3D *face = nullptr;
+
+		Vector3 result;
+		Vector3 normal;
+		int face_index = -1;
+		real_t min_d = 1e20;
+		int collisions = 0;
+	};
+
+	bool backface_collision = false;
+
+	void _cull_segment(int p_idx, _SegmentCullParams *p_params) const;
+	bool _cull(int p_idx, _CullParams *p_params) const;
+
+	void _fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx);
+
+	void _setup(const Vector<Vector3> &p_faces, bool p_backface_collision);
+
+public:
+	Vector<Vector3> get_faces() const;
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotConcavePolygonShape3D();
+};
+
+struct GodotHeightMapShape3D : public GodotConcaveShape3D {
+	Vector<real_t> heights;
+	int width = 0;
+	int depth = 0;
+	Vector3 local_origin;
+
+	// Accelerator.
+	struct Range {
+		real_t min = 0.0;
+		real_t max = 0.0;
+	};
+	LocalVector<Range> bounds_grid;
+	int bounds_grid_width = 0;
+	int bounds_grid_depth = 0;
+
+	static const int BOUNDS_CHUNK_SIZE = 16;
+
+	_FORCE_INLINE_ const Range &_get_bounds_chunk(int p_x, int p_z) const {
+		return bounds_grid[(p_z * bounds_grid_width) + p_x];
+	}
+
+	_FORCE_INLINE_ real_t _get_height(int p_x, int p_z) const {
+		return heights[(p_z * width) + p_x];
+	}
+
+	_FORCE_INLINE_ void _get_point(int p_x, int p_z, Vector3 &r_point) const {
+		r_point.x = p_x - 0.5 * (width - 1.0);
+		r_point.y = _get_height(p_x, p_z);
+		r_point.z = p_z - 0.5 * (depth - 1.0);
+	}
+
+	void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const;
+
+	void _build_accelerator();
+
+	template <typename ProcessFunction>
+	bool _intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const;
+
+	void _setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
+
+public:
+	Vector<real_t> get_heights() const;
+	int get_width() const;
+	int get_depth() const;
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_HEIGHTMAP; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override;
+	virtual Variant get_data() const override;
+
+	GodotHeightMapShape3D();
+};
+
+//used internally
+struct GodotFaceShape3D : public GodotShape3D {
+	Vector3 normal; //cache
+	Vector3 vertex[3];
+	bool backface_collision = false;
+	bool invert_backface_collision = false;
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; }
+
+	const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
+	virtual Vector3 get_support(const Vector3 &p_normal) const override;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
+
+	virtual void set_data(const Variant &p_data) override {}
+	virtual Variant get_data() const override { return Variant(); }
+
+	GodotFaceShape3D();
+};
+
+struct GodotMotionShape3D : public GodotShape3D {
+	GodotShape3D *shape = nullptr;
+	Vector3 motion;
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override {
+		Vector3 cast = p_transform.basis.xform(motion);
+		real_t mina, maxa;
+		real_t minb, maxb;
+		Transform3D ofsb = p_transform;
+		ofsb.origin += cast;
+		shape->project_range(p_normal, p_transform, mina, maxa);
+		shape->project_range(p_normal, ofsb, minb, maxb);
+		r_min = MIN(mina, minb);
+		r_max = MAX(maxa, maxb);
+	}
+
+	virtual Vector3 get_support(const Vector3 &p_normal) const override {
+		Vector3 support = shape->get_support(p_normal);
+		if (p_normal.dot(motion) > 0) {
+			support += motion;
+		}
+		return support;
+	}
+
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override { return false; }
+	virtual bool intersect_point(const Vector3 &p_point) const override { return false; }
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override { return p_point; }
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); }
+
+	virtual void set_data(const Variant &p_data) override {}
+	virtual Variant get_data() const override { return Variant(); }
+
+	GodotMotionShape3D() { configure(AABB()); }
+};

modules/godot_physics_3d/godot_soft_body_3d.h

@@ -0,0 +1,280 @@
+/**************************************************************************/
+/*  godot_soft_body_3d.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_3d.h"
+#include "godot_collision_object_3d.h"
+
+#include "core/math/aabb.h"
+#include "core/math/dynamic_bvh.h"
+#include "core/math/vector3.h"
+#include "core/templates/hash_set.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/vset.h"
+
+class GodotConstraint3D;
+
+class GodotSoftBody3D : public GodotCollisionObject3D {
+	RID soft_mesh;
+
+	struct Node {
+		Vector3 s; // Source position
+		Vector3 x; // Position
+		Vector3 q; // Previous step position/Test position
+		Vector3 f; // Force accumulator
+		Vector3 v; // Velocity
+		Vector3 bv; // Biased Velocity
+		Vector3 n; // Normal
+		real_t area = 0.0; // Area
+		real_t im = 0.0; // 1/mass
+		DynamicBVH::ID leaf; // Leaf data
+		uint32_t index = 0;
+	};
+
+	struct Link {
+		Vector3 c3; // gradient
+		Node *n[2] = { nullptr, nullptr }; // Node pointers
+		real_t rl = 0.0; // Rest length
+		real_t c0 = 0.0; // (ima+imb)*kLST
+		real_t c1 = 0.0; // rl^2
+		real_t c2 = 0.0; // |gradient|^2/c0
+	};
+
+	struct Face {
+		Vector3 centroid;
+		Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers
+		Vector3 normal; // Normal
+		real_t ra = 0.0; // Rest area
+		DynamicBVH::ID leaf; // Leaf data
+		uint32_t index = 0;
+	};
+
+	LocalVector<Node> nodes;
+	LocalVector<Link> links;
+	LocalVector<Face> faces;
+
+	DynamicBVH node_tree;
+	DynamicBVH face_tree;
+
+	LocalVector<uint32_t> map_visual_to_physics;
+
+	AABB bounds;
+
+	real_t collision_margin = 0.05;
+
+	real_t total_mass = 1.0;
+	real_t inv_total_mass = 1.0;
+
+	int iteration_count = 5;
+	real_t linear_stiffness = 0.5; // [0,1]
+	real_t shrinking_factor = 0.0; // [-1,1]
+	real_t pressure_coefficient = 0.0; // [-inf,+inf]
+	real_t damping_coefficient = 0.01; // [0,1]
+	real_t drag_coefficient = 0.0; // [0,1]
+	LocalVector<int> pinned_vertices;
+
+	SelfList<GodotSoftBody3D> active_list;
+
+	HashSet<GodotConstraint3D *> constraints;
+
+	Vector<AreaCMP> areas;
+
+	VSet<RID> exceptions;
+
+	uint64_t island_step = 0;
+
+	_FORCE_INLINE_ Vector3 _compute_area_windforce(const GodotArea3D *p_area, const Face *p_face);
+
+public:
+	GodotSoftBody3D();
+
+	const AABB &get_bounds() const { return bounds; }
+
+	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
+	Variant get_state(PhysicsServer3D::BodyState p_state) const;
+
+	_FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; }
+	_FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
+
+	_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_area(GodotArea3D *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(GodotArea3D *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);
+			}
+		}
+	}
+
+	virtual void set_space(GodotSpace3D *p_space) override;
+
+	void set_mesh(RID p_mesh);
+
+	void update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler);
+
+	Vector3 get_vertex_position(int p_index) const;
+	void set_vertex_position(int p_index, const Vector3 &p_position);
+
+	void pin_vertex(int p_index);
+	void unpin_vertex(int p_index);
+	void unpin_all_vertices();
+	bool is_vertex_pinned(int p_index) const;
+
+	uint32_t get_node_count() const;
+	real_t get_node_inv_mass(uint32_t p_node_index) const;
+	Vector3 get_node_position(uint32_t p_node_index) const;
+	Vector3 get_node_velocity(uint32_t p_node_index) const;
+	Vector3 get_node_biased_velocity(uint32_t p_node_index) const;
+	void apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse);
+	void apply_node_force(uint32_t p_node_index, const Vector3 &p_force);
+	void apply_central_impulse(const Vector3 &p_impulse);
+	void apply_central_force(const Vector3 &p_force);
+	void apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse);
+
+	uint32_t get_face_count() const;
+	void get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const;
+	Vector3 get_face_normal(uint32_t p_face_index) const;
+
+	void set_iteration_count(int p_val);
+	_FORCE_INLINE_ real_t get_iteration_count() const { return iteration_count; }
+
+	void set_total_mass(real_t p_val);
+	_FORCE_INLINE_ real_t get_total_mass() const { return total_mass; }
+	_FORCE_INLINE_ real_t get_total_inv_mass() const { return inv_total_mass; }
+
+	void set_collision_margin(real_t p_val);
+	_FORCE_INLINE_ real_t get_collision_margin() const { return collision_margin; }
+
+	void set_linear_stiffness(real_t p_val);
+	_FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; }
+
+	void set_shrinking_factor(real_t p_val);
+	_FORCE_INLINE_ real_t get_shrinking_factor() const { return shrinking_factor; }
+
+	void set_pressure_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
+
+	void set_damping_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; }
+
+	void set_drag_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; }
+
+	void predict_motion(real_t p_delta);
+	void solve_constraints(real_t p_delta);
+
+	_FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return static_cast<Node *>(p_node)->index; }
+	_FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return static_cast<Face *>(p_face)->index; }
+
+	// Return true to stop the query.
+	// p_index is the node index for AABB query, face index for Ray query.
+	typedef bool (*QueryResultCallback)(uint32_t p_index, void *p_userdata);
+
+	void query_aabb(const AABB &p_aabb, QueryResultCallback p_result_callback, void *p_userdata);
+	void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata);
+
+protected:
+	virtual void _shapes_changed() override;
+
+private:
+	void update_normals_and_centroids();
+	void update_bounds();
+	void update_constants();
+	void update_area();
+	void reset_link_rest_lengths();
+	void update_link_constants();
+
+	void apply_nodes_transform(const Transform3D &p_transform);
+
+	void add_velocity(const Vector3 &p_velocity);
+
+	void apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas);
+
+	bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices);
+	void generate_bending_constraints(int p_distance);
+	void reoptimize_link_order();
+	void append_link(uint32_t p_node1, uint32_t p_node2);
+	void append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3);
+
+	void solve_links(real_t kst, real_t ti);
+
+	void initialize_face_tree();
+	void update_face_tree(real_t p_delta);
+
+	void initialize_shape(bool p_force_move = true);
+	void deinitialize_shape();
+
+	void destroy();
+};
+
+class GodotSoftBodyShape3D : public GodotShape3D {
+	GodotSoftBody3D *soft_body = nullptr;
+
+public:
+	GodotSoftBody3D *get_soft_body() const { return soft_body; }
+
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SOFT_BODY; }
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { r_min = r_max = 0.0; }
+	virtual Vector3 get_support(const Vector3 &p_normal) const override { return Vector3(); }
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
+
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); }
+
+	virtual void set_data(const Variant &p_data) override {}
+	virtual Variant get_data() const override { return Variant(); }
+
+	void update_bounds();
+
+	GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body);
+	~GodotSoftBodyShape3D() {}
+};

modules/godot_physics_3d/godot_space_3d.h

@@ -0,0 +1,211 @@
+/**************************************************************************/
+/*  godot_space_3d.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_3d.h"
+#include "godot_body_3d.h"
+#include "godot_broad_phase_3d.h"
+#include "godot_collision_object_3d.h"
+#include "godot_soft_body_3d.h"
+
+#include "core/typedefs.h"
+
+class GodotPhysicsDirectSpaceState3D : public PhysicsDirectSpaceState3D {
+	GDCLASS(GodotPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D);
+
+public:
+	GodotSpace3D *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, ShapeRestInfo *r_info = nullptr) override;
+	virtual bool collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) override;
+	virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override;
+	virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override;
+
+	GodotPhysicsDirectSpaceState3D();
+};
+
+class GodotSpace3D {
+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:
+	uint64_t elapsed_time[ELAPSED_TIME_MAX] = {};
+
+	GodotPhysicsDirectSpaceState3D *direct_access = nullptr;
+	RID self;
+
+	GodotBroadPhase3D *broadphase = nullptr;
+	SelfList<GodotBody3D>::List active_list;
+	SelfList<GodotBody3D>::List mass_properties_update_list;
+	SelfList<GodotBody3D>::List state_query_list;
+	SelfList<GodotArea3D>::List monitor_query_list;
+	SelfList<GodotArea3D>::List area_moved_list;
+	SelfList<GodotSoftBody3D>::List active_soft_body_list;
+
+	static void *_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self);
+	static void _broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self);
+
+	HashSet<GodotCollisionObject3D *> objects;
+
+	GodotArea3D *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;
+
+	enum {
+		INTERSECTION_QUERY_MAX = 2048
+	};
+
+	GodotCollisionObject3D *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;
+
+	RID static_global_body;
+
+	Vector<Vector3> contact_debug;
+	int contact_debug_count = 0;
+
+	friend class GodotPhysicsDirectSpaceState3D;
+
+	int _cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb);
+
+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(GodotArea3D *p_area) { area = p_area; }
+	GodotArea3D *get_default_area() const { return area; }
+
+	const SelfList<GodotBody3D>::List &get_active_body_list() const;
+	void body_add_to_active_list(SelfList<GodotBody3D> *p_body);
+	void body_remove_from_active_list(SelfList<GodotBody3D> *p_body);
+	void body_add_to_mass_properties_update_list(SelfList<GodotBody3D> *p_body);
+	void body_remove_from_mass_properties_update_list(SelfList<GodotBody3D> *p_body);
+
+	void body_add_to_state_query_list(SelfList<GodotBody3D> *p_body);
+	void body_remove_from_state_query_list(SelfList<GodotBody3D> *p_body);
+
+	void area_add_to_monitor_query_list(SelfList<GodotArea3D> *p_area);
+	void area_remove_from_monitor_query_list(SelfList<GodotArea3D> *p_area);
+	void area_add_to_moved_list(SelfList<GodotArea3D> *p_area);
+	void area_remove_from_moved_list(SelfList<GodotArea3D> *p_area);
+	const SelfList<GodotArea3D>::List &get_moved_area_list() const;
+
+	const SelfList<GodotSoftBody3D>::List &get_active_soft_body_list() const;
+	void soft_body_add_to_active_list(SelfList<GodotSoftBody3D> *p_soft_body);
+	void soft_body_remove_from_active_list(SelfList<GodotSoftBody3D> *p_soft_body);
+
+	GodotBroadPhase3D *get_broadphase();
+
+	void add_object(GodotCollisionObject3D *p_object);
+	void remove_object(GodotCollisionObject3D *p_object);
+	const HashSet<GodotCollisionObject3D *> &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_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(PhysicsServer3D::SpaceParameter p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::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; }
+
+	GodotPhysicsDirectSpaceState3D *get_direct_state();
+
+	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 Vector3 &p_contact) {
+		if (contact_debug_count < contact_debug.size()) {
+			contact_debug.write[contact_debug_count++] = p_contact;
+		}
+	}
+	_FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; }
+	_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
+
+	void set_static_global_body(RID p_body) { static_global_body = p_body; }
+	RID get_static_global_body() { return static_global_body; }
+
+	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]; }
+
+	bool test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result);
+
+	GodotSpace3D();
+	~GodotSpace3D();
+};

modules/godot_physics_3d/godot_step_3d.cpp

@@ -0,0 +1,418 @@
+/**************************************************************************/
+/*  godot_step_3d.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_3d.h"
+
+#include "godot_joint_3d.h"
+
+#include "core/object/worker_thread_pool.h"
+#include "core/os/os.h"
+
+#define BODY_ISLAND_COUNT_RESERVE 128
+#define BODY_ISLAND_SIZE_RESERVE 512
+#define ISLAND_COUNT_RESERVE 128
+#define ISLAND_SIZE_RESERVE 512
+#define CONSTRAINT_COUNT_RESERVE 1024
+
+void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) {
+	p_body->set_island_step(_step);
+
+	if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
+		// Only rigid bodies are tested for activation.
+		p_body_island.push_back(p_body);
+	}
+
+	for (const KeyValue<GodotConstraint3D *, int> &E : p_body->get_constraint_map()) {
+		GodotConstraint3D *constraint = E.key;
+		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);
+
+		// Find connected rigid bodies.
+		for (int i = 0; i < constraint->get_body_count(); i++) {
+			if (i == E.value) {
+				continue;
+			}
+			GodotBody3D *other_body = constraint->get_body_ptr()[i];
+			if (other_body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
+				continue; // Static bodies don't connect islands.
+			}
+			_populate_island(other_body, p_body_island, p_constraint_island);
+		}
+
+		// Find connected soft bodies.
+		for (int i = 0; i < constraint->get_soft_body_count(); i++) {
+			GodotSoftBody3D *soft_body = constraint->get_soft_body_ptr(i);
+			if (soft_body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			_populate_island_soft_body(soft_body, p_body_island, p_constraint_island);
+		}
+	}
+}
+
+void GodotStep3D::_populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) {
+	p_soft_body->set_island_step(_step);
+
+	for (GodotConstraint3D *E : p_soft_body->get_constraints()) {
+		GodotConstraint3D *constraint = E;
+		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);
+
+		// Find connected rigid bodies.
+		for (int i = 0; i < constraint->get_body_count(); i++) {
+			GodotBody3D *body = constraint->get_body_ptr()[i];
+			if (body->get_island_step() == _step) {
+				continue; // Already processed.
+			}
+			if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
+				continue; // Static bodies don't connect islands.
+			}
+			_populate_island(body, p_body_island, p_constraint_island);
+		}
+	}
+}
+
+void GodotStep3D::_setup_constraint(uint32_t p_constraint_index, void *p_userdata) {
+	GodotConstraint3D *constraint = all_constraints[p_constraint_index];
+	constraint->setup(delta);
+}
+
+void GodotStep3D::_pre_solve_island(LocalVector<GodotConstraint3D *> &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) {
+		GodotConstraint3D *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 GodotStep3D::_solve_island(uint32_t p_island_index, void *p_userdata) {
+	LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[p_island_index];
+
+	int current_priority = 1;
+
+	uint32_t constraint_count = constraint_island.size();
+	while (constraint_count > 0) {
+		for (int i = 0; i < iterations; i++) {
+			// Go through all iterations.
+			for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+				constraint_island[constraint_index]->solve(delta);
+			}
+		}
+
+		// Check priority to keep only higher priority constraints.
+		uint32_t priority_constraint_count = 0;
+		++current_priority;
+		for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+			GodotConstraint3D *constraint = constraint_island[constraint_index];
+			if (constraint->get_priority() >= current_priority) {
+				// Keep this constraint for the next iteration.
+				constraint_island[priority_constraint_count++] = constraint;
+			}
+		}
+		constraint_count = priority_constraint_count;
+	}
+}
+
+void GodotStep3D::_check_suspend(const LocalVector<GodotBody3D *> &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) {
+		GodotBody3D *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) {
+		GodotBody3D *body = p_body_island[body_index];
+
+		bool active = body->is_active();
+
+		if (active == can_sleep) {
+			body->set_active(!can_sleep);
+		}
+	}
+}
+
+void GodotStep3D::step(GodotSpace3D *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<GodotBody3D>::List *body_list = &p_space->get_active_body_list();
+
+	const SelfList<GodotSoftBody3D>::List *soft_body_list = &p_space->get_active_soft_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<GodotBody3D> *b = body_list->first();
+	while (b) {
+		b->self()->integrate_forces(p_delta);
+		b = b->next();
+		active_count++;
+	}
+
+	/* UPDATE SOFT BODY MOTION */
+
+	const SelfList<GodotSoftBody3D> *sb = soft_body_list->first();
+	while (sb) {
+		sb->self()->predict_motion(p_delta);
+		sb = sb->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(GodotSpace3D::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<GodotArea3D>::List &aml = p_space->get_moved_area_list();
+
+	while (aml.first()) {
+		for (GodotConstraint3D *E : aml.first()->self()->get_constraints()) {
+			GodotConstraint3D *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<GodotConstraint3D *> &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<GodotArea3D> *)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) {
+		GodotBody3D *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<GodotBody3D *> &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<GodotConstraint3D *> &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();
+	}
+
+	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */
+
+	sb = soft_body_list->first();
+	while (sb) {
+		GodotSoftBody3D *soft_body = sb->self();
+
+		if (soft_body->get_island_step() != _step) {
+			++body_island_count;
+			if (body_islands.size() < body_island_count) {
+				body_islands.resize(body_island_count);
+			}
+			LocalVector<GodotBody3D *> &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<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+			constraint_island.reserve(ISLAND_SIZE_RESERVE);
+
+			_populate_island_soft_body(soft_body, body_island, constraint_island);
+
+			if (body_island.is_empty()) {
+				--body_island_count;
+			}
+
+			if (constraint_island.is_empty()) {
+				--island_count;
+			}
+		}
+		sb = sb->next();
+	}
+
+	p_space->set_island_count((int)island_count);
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace3D::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, &GodotStep3D::_setup_constraint, nullptr, total_constraint_count, -1, true, SNAME("Physics3DConstraintSetup"));
+	WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace3D::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, &GodotStep3D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics3DConstraintSolveIslands"));
+	WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
+	}
+
+	/* INTEGRATE VELOCITIES */
+
+	b = body_list->first();
+	while (b) {
+		const SelfList<GodotBody3D> *n = b->next();
+		b->self()->integrate_velocities(p_delta);
+		b = n;
+	}
+
+	/* SLEEP / WAKE UP ISLANDS */
+
+	for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
+		_check_suspend(body_islands[island_index]);
+	}
+
+	/* UPDATE SOFT BODY CONSTRAINTS */
+
+	sb = soft_body_list->first();
+	while (sb) {
+		sb->self()->solve_constraints(p_delta);
+		sb = sb->next();
+	}
+
+	{ //profile
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
+	}
+
+	all_constraints.clear();
+
+	p_space->unlock();
+	_step++;
+}
+
+GodotStep3D::GodotStep3D() {
+	body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
+	constraint_islands.reserve(ISLAND_COUNT_RESERVE);
+	all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
+}
+
+GodotStep3D::~GodotStep3D() {
+}

modules/godot_physics_3d/godot_step_3d.h

@@ -0,0 +1,58 @@
+/**************************************************************************/
+/*  godot_step_3d.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_3d.h"
+
+#include "core/templates/local_vector.h"
+
+class GodotStep3D {
+	uint64_t _step = 1;
+
+	int iterations = 0;
+	real_t delta = 0.0;
+
+	LocalVector<LocalVector<GodotBody3D *>> body_islands;
+	LocalVector<LocalVector<GodotConstraint3D *>> constraint_islands;
+	LocalVector<GodotConstraint3D *> all_constraints;
+
+	void _populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island);
+	void _populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island);
+	void _setup_constraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
+	void _pre_solve_island(LocalVector<GodotConstraint3D *> &p_constraint_island) const;
+	void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr);
+	void _check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const;
+
+public:
+	void step(GodotSpace3D *p_space, real_t p_delta);
+	GodotStep3D();
+	~GodotStep3D();
+};

modules/godot_physics_3d/joints/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_3d/joints/godot_cone_twist_joint_3d.cpp

@@ -0,0 +1,326 @@
+/**************************************************************************/
+/*  godot_cone_twist_joint_3d.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.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+#include "godot_cone_twist_joint_3d.h"
+
+GodotConeTwistJoint3D::GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
+		GodotJoint3D(_arr, 2) {
+	A = rbA;
+	B = rbB;
+
+	m_rbAFrame = rbAFrame;
+	m_rbBFrame = rbBFrame;
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+bool GodotConeTwistJoint3D::setup(real_t p_timestep) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	m_appliedImpulse = real_t(0.);
+
+	//set bias, sign, clear accumulator
+	m_swingCorrection = real_t(0.);
+	m_twistLimitSign = real_t(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+	m_accTwistLimitImpulse = real_t(0.);
+	m_accSwingLimitImpulse = real_t(0.);
+
+	if (!m_angularOnly) {
+		Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
+		Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
+		Vector3 relPos = pivotBInW - pivotAInW;
+
+		Vector3 normal[3];
+		if (Math::is_zero_approx(relPos.length_squared())) {
+			normal[0] = Vector3(real_t(1.0), 0, 0);
+		} else {
+			normal[0] = relPos.normalized();
+		}
+
+		plane_space(normal[0], normal[1], normal[2]);
+
+		for (int i = 0; i < 3; i++) {
+			memnew_placement(
+					&m_jac[i],
+					GodotJacobianEntry3D(
+							A->get_principal_inertia_axes().transposed(),
+							B->get_principal_inertia_axes().transposed(),
+							pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+							pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+							normal[i],
+							A->get_inv_inertia(),
+							A->get_inv_mass(),
+							B->get_inv_inertia(),
+							B->get_inv_mass()));
+		}
+	}
+
+	Vector3 b1Axis1, b1Axis2, b1Axis3;
+	Vector3 b2Axis1;
+
+	b1Axis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0));
+	b2Axis1 = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(0));
+
+	real_t swing1 = real_t(0.), swing2 = real_t(0.);
+
+	real_t swx = real_t(0.), swy = real_t(0.);
+	real_t thresh = real_t(10.);
+	real_t fact;
+
+	// Get Frame into world space
+	if (m_swingSpan1 >= real_t(0.05f)) {
+		b1Axis2 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1));
+		//swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
+		swx = b2Axis1.dot(b1Axis1);
+		swy = b2Axis1.dot(b1Axis2);
+		swing1 = atan2fast(swy, swx);
+		fact = (swy * swy + swx * swx) * thresh * thresh;
+		fact = fact / (fact + real_t(1.0));
+		swing1 *= fact;
+	}
+
+	if (m_swingSpan2 >= real_t(0.05f)) {
+		b1Axis3 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2));
+		//swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
+		swx = b2Axis1.dot(b1Axis1);
+		swy = b2Axis1.dot(b1Axis3);
+		swing2 = atan2fast(swy, swx);
+		fact = (swy * swy + swx * swx) * thresh * thresh;
+		fact = fact / (fact + real_t(1.0));
+		swing2 *= fact;
+	}
+
+	real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
+	real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
+	real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq;
+
+	if (EllipseAngle > 1.0f) {
+		m_swingCorrection = EllipseAngle - 1.0f;
+		m_solveSwingLimit = true;
+
+		// Calculate necessary axis & factors
+		m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
+		m_swingAxis.normalize();
+
+		real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
+		m_swingAxis *= swingAxisSign;
+
+		m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + B->compute_angular_impulse_denominator(m_swingAxis));
+	}
+
+	// Twist limits
+	if (m_twistSpan >= real_t(0.)) {
+		Vector3 b2Axis22 = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1));
+		Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1);
+		Vector3 TwistRef = rotationArc.xform(b2Axis22);
+		real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
+
+		real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.);
+		if (twist <= -m_twistSpan * lockedFreeFactor) {
+			m_twistCorrection = -(twist + m_twistSpan);
+			m_solveTwistLimit = true;
+
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+			m_twistAxis *= -1.0f;
+
+			m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis));
+
+		} else if (twist > m_twistSpan * lockedFreeFactor) {
+			m_twistCorrection = (twist - m_twistSpan);
+			m_solveTwistLimit = true;
+
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+
+			m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis));
+		}
+	}
+
+	return true;
+}
+
+void GodotConeTwistJoint3D::solve(real_t p_timestep) {
+	Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
+	Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
+
+	real_t tau = real_t(0.3);
+
+	//linear part
+	if (!m_angularOnly) {
+		Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+		Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+
+		Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
+		Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
+		Vector3 vel = vel1 - vel2;
+
+		for (int i = 0; i < 3; i++) {
+			const Vector3 &normal = m_jac[i].m_linearJointAxis;
+			real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
+
+			real_t rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv;
+			m_appliedImpulse += impulse;
+			Vector3 impulse_vector = normal * impulse;
+			if (dynamic_A) {
+				A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+			}
+			if (dynamic_B) {
+				B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+			}
+		}
+	}
+
+	{
+		///solve angular part
+		const Vector3 &angVelA = A->get_angular_velocity();
+		const Vector3 &angVelB = B->get_angular_velocity();
+
+		// solve swing limit
+		if (m_solveSwingLimit) {
+			real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / p_timestep) * m_biasFactor);
+			real_t impulseMag = amplitude * m_kSwing;
+
+			// Clamp the accumulated impulse
+			real_t temp = m_accSwingLimitImpulse;
+			m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0));
+			impulseMag = m_accSwingLimitImpulse - temp;
+
+			Vector3 impulse = m_swingAxis * impulseMag;
+
+			if (dynamic_A) {
+				A->apply_torque_impulse(impulse);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-impulse);
+			}
+		}
+
+		// solve twist limit
+		if (m_solveTwistLimit) {
+			real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / p_timestep) * m_biasFactor);
+			real_t impulseMag = amplitude * m_kTwist;
+
+			// Clamp the accumulated impulse
+			real_t temp = m_accTwistLimitImpulse;
+			m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0));
+			impulseMag = m_accTwistLimitImpulse - temp;
+
+			Vector3 impulse = m_twistAxis * impulseMag;
+
+			if (dynamic_A) {
+				A->apply_torque_impulse(impulse);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-impulse);
+			}
+		}
+	}
+}
+
+void GodotConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
+			m_swingSpan1 = p_value;
+			m_swingSpan2 = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: {
+			m_twistSpan = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: {
+			m_biasFactor = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: {
+			m_limitSoftness = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: {
+			m_relaxationFactor = p_value;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+real_t GodotConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
+			return m_swingSpan1;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: {
+			return m_twistSpan;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: {
+			return m_biasFactor;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: {
+			return m_limitSoftness;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: {
+			return m_relaxationFactor;
+		} break;
+		case PhysicsServer3D::CONE_TWIST_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return 0;
+}

modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.h

@@ -0,0 +1,139 @@
+/**************************************************************************/
+/*  godot_cone_twist_joint_3d.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
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+GodotConeTwistJoint3D is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.h"
+
+// GodotConeTwistJoint3D can be used to simulate ragdoll joints (upper arm, leg etc).
+class GodotConeTwistJoint3D : public GodotJoint3D {
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
+	};
+
+	GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
+
+	real_t m_appliedImpulse = 0.0;
+	Transform3D m_rbAFrame;
+	Transform3D m_rbBFrame;
+
+	real_t m_limitSoftness = 0.0;
+	real_t m_biasFactor = 0.3;
+	real_t m_relaxationFactor = 1.0;
+
+	real_t m_swingSpan1 = Math::TAU / 8.0;
+	real_t m_swingSpan2 = 0.0;
+	real_t m_twistSpan = 0.0;
+
+	Vector3 m_swingAxis;
+	Vector3 m_twistAxis;
+
+	real_t m_kSwing = 0.0;
+	real_t m_kTwist = 0.0;
+
+	real_t m_twistLimitSign = 0.0;
+	real_t m_swingCorrection = 0.0;
+	real_t m_twistCorrection = 0.0;
+
+	real_t m_accSwingLimitImpulse = 0.0;
+	real_t m_accTwistLimitImpulse = 0.0;
+
+	bool m_angularOnly = false;
+	bool m_solveTwistLimit = false;
+	bool m_solveSwingLimit = false;
+
+public:
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame);
+
+	void setAngularOnly(bool angularOnly) {
+		m_angularOnly = angularOnly;
+	}
+
+	void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) {
+		m_swingSpan1 = _swingSpan1;
+		m_swingSpan2 = _swingSpan2;
+		m_twistSpan = _twistSpan;
+
+		m_limitSoftness = _softness;
+		m_biasFactor = _biasFactor;
+		m_relaxationFactor = _relaxationFactor;
+	}
+
+	inline int getSolveTwistLimit() {
+		return m_solveTwistLimit;
+	}
+
+	inline int getSolveSwingLimit() {
+		return m_solveTwistLimit;
+	}
+
+	inline real_t getTwistLimitSign() {
+		return m_twistLimitSign;
+	}
+
+	void set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const;
+};

modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.cpp

@@ -0,0 +1,675 @@
+/**************************************************************************/
+/*  godot_generic_6dof_joint_3d.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.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+2007-09-09
+GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+#include "godot_generic_6dof_joint_3d.h"
+
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+//////////////////////////// GodotG6DOFRotationalLimitMotor3D ////////////////////////////////////
+
+int GodotG6DOFRotationalLimitMotor3D::testLimitValue(real_t test_value) {
+	if (m_loLimit > m_hiLimit) {
+		m_currentLimit = 0; //Free from violation
+		return 0;
+	}
+
+	if (test_value < m_loLimit) {
+		m_currentLimit = 1; //low limit violation
+		m_currentLimitError = test_value - m_loLimit;
+		return 1;
+	} else if (test_value > m_hiLimit) {
+		m_currentLimit = 2; //High limit violation
+		m_currentLimitError = test_value - m_hiLimit;
+		return 2;
+	};
+
+	m_currentLimit = 0; //Free from violation
+	return 0;
+}
+
+real_t GodotG6DOFRotationalLimitMotor3D::solveAngularLimits(
+		real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
+		GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
+	if (!needApplyTorques()) {
+		return 0.0f;
+	}
+
+	real_t target_velocity = m_targetVelocity;
+	real_t maxMotorForce = m_maxMotorForce;
+
+	//current error correction
+	if (m_currentLimit != 0) {
+		target_velocity = -m_ERP * m_currentLimitError / (timeStep);
+		maxMotorForce = m_maxLimitForce;
+	}
+
+	maxMotorForce *= timeStep;
+
+	// current velocity difference
+	Vector3 vel_diff = body0->get_angular_velocity();
+	if (body1) {
+		vel_diff -= body1->get_angular_velocity();
+	}
+
+	real_t rel_vel = axis.dot(vel_diff);
+
+	// correction velocity
+	real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
+
+	if (Math::is_zero_approx(motor_relvel)) {
+		return 0.0f; //no need for applying force
+	}
+
+	// correction impulse
+	real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
+
+	// clip correction impulse
+	real_t clippedMotorImpulse;
+
+	///@todo: should clip against accumulated impulse
+	if (unclippedMotorImpulse > 0.0f) {
+		clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
+	} else {
+		clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
+	}
+
+	// sort with accumulated impulses
+	real_t lo = real_t(-1e30);
+	real_t hi = real_t(1e30);
+
+	real_t oldaccumImpulse = m_accumulatedImpulse;
+	real_t sum = oldaccumImpulse + clippedMotorImpulse;
+	m_accumulatedImpulse = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum);
+
+	clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+
+	Vector3 motorImp = clippedMotorImpulse * axis;
+
+	if (p_body0_dynamic) {
+		body0->apply_torque_impulse(motorImp);
+	}
+	if (body1 && p_body1_dynamic) {
+		body1->apply_torque_impulse(-motorImp);
+	}
+
+	return clippedMotorImpulse;
+}
+
+//////////////////////////// GodotG6DOFTranslationalLimitMotor3D ////////////////////////////////////
+
+real_t GodotG6DOFTranslationalLimitMotor3D::solveLinearAxis(
+		real_t timeStep,
+		real_t jacDiagABInv,
+		GodotBody3D *body1, const Vector3 &pointInA,
+		GodotBody3D *body2, const Vector3 &pointInB,
+		bool p_body1_dynamic, bool p_body2_dynamic,
+		int limit_index,
+		const Vector3 &axis_normal_on_a,
+		const Vector3 &anchorPos) {
+	///find relative velocity
+	//    Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
+	//    Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
+	Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
+	Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
+
+	Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
+	Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
+	Vector3 vel = vel1 - vel2;
+
+	real_t rel_vel = axis_normal_on_a.dot(vel);
+
+	/// apply displacement correction
+
+	//positional error (zeroth order error)
+	real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+	real_t lo = real_t(-1e30);
+	real_t hi = real_t(1e30);
+
+	real_t minLimit = m_lowerLimit[limit_index];
+	real_t maxLimit = m_upperLimit[limit_index];
+
+	//handle the limits
+	if (minLimit < maxLimit) {
+		{
+			if (depth > maxLimit) {
+				depth -= maxLimit;
+				lo = real_t(0.);
+
+			} else {
+				if (depth < minLimit) {
+					depth -= minLimit;
+					hi = real_t(0.);
+				} else {
+					return 0.0f;
+				}
+			}
+		}
+	}
+
+	real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv;
+
+	real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
+	real_t sum = oldNormalImpulse + normalImpulse;
+	m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum);
+	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
+
+	Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
+	if (p_body1_dynamic) {
+		body1->apply_impulse(impulse_vector, rel_pos1);
+	}
+	if (p_body2_dynamic) {
+		body2->apply_impulse(-impulse_vector, rel_pos2);
+	}
+	return normalImpulse;
+}
+
+//////////////////////////// GodotGeneric6DOFJoint3D ////////////////////////////////////
+
+GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) :
+		GodotJoint3D(_arr, 2),
+		m_frameInA(frameInA),
+		m_frameInB(frameInB),
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
+	A = rbA;
+	B = rbB;
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+void GodotGeneric6DOFJoint3D::calculateAngleInfo() {
+	Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis;
+
+	m_calculatedAxisAngleDiff = relative_frame.get_euler(EulerOrder::XYZ);
+
+	// in euler angle mode we do not actually constrain the angular velocity
+	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
+	//
+	//    to get			constrain w2-w1 along		...not
+	//    ------			---------------------		------
+	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+	//    d(angle[1])/dt = 0	ax[1]
+	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+	//
+	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+	// to prove the result for angle[0], write the expression for angle[0] from
+	// GetInfo1 then take the derivative. to prove this for angle[2] it is
+	// easier to take the euler rate expression for d(angle[2])/dt with respect
+	// to the components of w and set that to 0.
+
+	Vector3 axis0 = m_calculatedTransformB.basis.get_column(0);
+	Vector3 axis2 = m_calculatedTransformA.basis.get_column(2);
+
+	m_calculatedAxis[1] = axis2.cross(axis0);
+	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+
+	/*
+	if(m_debugDrawer)
+	{
+		char buff[300];
+		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
+		m_calculatedAxisAngleDiff[0],
+		m_calculatedAxisAngleDiff[1],
+		m_calculatedAxisAngleDiff[2]);
+		m_debugDrawer->reportErrorWarning(buff);
+	}
+	*/
+}
+
+void GodotGeneric6DOFJoint3D::calculateTransforms() {
+	m_calculatedTransformA = A->get_transform() * m_frameInA;
+	m_calculatedTransformB = B->get_transform() * m_frameInB;
+
+	calculateAngleInfo();
+}
+
+void GodotGeneric6DOFJoint3D::buildLinearJacobian(
+		GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
+		const Vector3 &pivotAInW, const Vector3 &pivotBInW) {
+	memnew_placement(
+			&jacLinear,
+			GodotJacobianEntry3D(
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+					pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+					normalWorld,
+					A->get_inv_inertia(),
+					A->get_inv_mass(),
+					B->get_inv_inertia(),
+					B->get_inv_mass()));
+}
+
+void GodotGeneric6DOFJoint3D::buildAngularJacobian(
+		GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) {
+	memnew_placement(
+			&jacAngular,
+			GodotJacobianEntry3D(
+					jointAxisW,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
+}
+
+bool GodotGeneric6DOFJoint3D::testAngularLimitMotor(int axis_index) {
+	real_t angle = m_calculatedAxisAngleDiff[axis_index];
+
+	//test limits
+	m_angularLimits[axis_index].testLimitValue(angle);
+	return m_angularLimits[axis_index].needApplyTorques();
+}
+
+bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	// Clear accumulated impulses for the next simulation step
+	m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.));
+	int i;
+	for (i = 0; i < 3; i++) {
+		m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
+	}
+	//calculates transform
+	calculateTransforms();
+
+	//  const Vector3& pivotAInW = m_calculatedTransformA.origin;
+	//  const Vector3& pivotBInW = m_calculatedTransformB.origin;
+	calcAnchorPos();
+	Vector3 pivotAInW = m_AnchorPos;
+	Vector3 pivotBInW = m_AnchorPos;
+
+	// not used here
+	//    Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+	//    Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+
+	Vector3 normalWorld;
+	//linear part
+	for (i = 0; i < 3; i++) {
+		if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
+			if (m_useLinearReferenceFrameA) {
+				normalWorld = m_calculatedTransformA.basis.get_column(i);
+			} else {
+				normalWorld = m_calculatedTransformB.basis.get_column(i);
+			}
+
+			buildLinearJacobian(
+					m_jacLinear[i], normalWorld,
+					pivotAInW, pivotBInW);
+		}
+	}
+
+	// angular part
+	for (i = 0; i < 3; i++) {
+		//calculates error angle
+		if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) {
+			normalWorld = getAxis(i);
+			// Create angular atom
+			buildAngularJacobian(m_jacAng[i], normalWorld);
+		}
+	}
+
+	return true;
+}
+
+void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) {
+	m_timeStep = p_timestep;
+
+	//calculateTransforms();
+
+	int i;
+
+	// linear
+
+	Vector3 pointInA = m_calculatedTransformA.origin;
+	Vector3 pointInB = m_calculatedTransformB.origin;
+
+	real_t jacDiagABInv;
+	Vector3 linear_axis;
+	for (i = 0; i < 3; i++) {
+		if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
+			jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
+
+			if (m_useLinearReferenceFrameA) {
+				linear_axis = m_calculatedTransformA.basis.get_column(i);
+			} else {
+				linear_axis = m_calculatedTransformB.basis.get_column(i);
+			}
+
+			m_linearLimits.solveLinearAxis(
+					m_timeStep,
+					jacDiagABInv,
+					A, pointInA,
+					B, pointInB,
+					dynamic_A, dynamic_B,
+					i, linear_axis, m_AnchorPos);
+		}
+	}
+
+	// angular
+	Vector3 angular_axis;
+	real_t angularJacDiagABInv;
+	for (i = 0; i < 3; i++) {
+		if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) {
+			// get axis
+			angular_axis = getAxis(i);
+
+			angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
+
+			m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B);
+		}
+	}
+}
+
+void GodotGeneric6DOFJoint3D::updateRHS(real_t timeStep) {
+	(void)timeStep;
+}
+
+Vector3 GodotGeneric6DOFJoint3D::getAxis(int axis_index) const {
+	return m_calculatedAxis[axis_index];
+}
+
+real_t GodotGeneric6DOFJoint3D::getAngle(int axis_index) const {
+	return m_calculatedAxisAngleDiff[axis_index];
+}
+
+void GodotGeneric6DOFJoint3D::calcAnchorPos() {
+	real_t imA = A->get_inv_mass();
+	real_t imB = B->get_inv_mass();
+	real_t weight;
+	if (imB == real_t(0.0)) {
+		weight = real_t(1.0);
+	} else {
+		weight = imA / (imA + imB);
+	}
+	const Vector3 &pA = m_calculatedTransformA.origin;
+	const Vector3 &pB = m_calculatedTransformB.origin;
+	m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
+}
+
+void GodotGeneric6DOFJoint3D::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
+	ERR_FAIL_INDEX(p_axis, 3);
+	switch (p_param) {
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
+			m_linearLimits.m_lowerLimit[p_axis] = p_value;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
+			m_linearLimits.m_upperLimit[p_axis] = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
+			m_linearLimits.m_limitSoftness[p_axis] = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: {
+			m_linearLimits.m_restitution[p_axis] = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: {
+			m_linearLimits.m_damping[p_axis] = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
+			m_angularLimits[p_axis].m_loLimit = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
+			m_angularLimits[p_axis].m_hiLimit = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+			m_angularLimits[p_axis].m_limitSoftness = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: {
+			m_angularLimits[p_axis].m_damping = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: {
+			m_angularLimits[p_axis].m_bounce = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
+			m_angularLimits[p_axis].m_maxLimitForce = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: {
+			m_angularLimits[p_axis].m_ERP = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
+			m_angularLimits[p_axis].m_targetVelocity = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
+			m_angularLimits[p_axis].m_maxLimitForce = p_value;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+real_t GodotGeneric6DOFJoint3D::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
+	ERR_FAIL_INDEX_V(p_axis, 3, 0);
+	switch (p_param) {
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
+			return m_linearLimits.m_lowerLimit[p_axis];
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
+			return m_linearLimits.m_upperLimit[p_axis];
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
+			return m_linearLimits.m_limitSoftness[p_axis];
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: {
+			return m_linearLimits.m_restitution[p_axis];
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: {
+			return m_linearLimits.m_damping[p_axis];
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
+			return m_angularLimits[p_axis].m_loLimit;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
+			return m_angularLimits[p_axis].m_hiLimit;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+			return m_angularLimits[p_axis].m_limitSoftness;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: {
+			return m_angularLimits[p_axis].m_damping;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: {
+			return m_angularLimits[p_axis].m_bounce;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
+			return m_angularLimits[p_axis].m_maxLimitForce;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: {
+			return m_angularLimits[p_axis].m_ERP;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
+			return m_angularLimits[p_axis].m_targetVelocity;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
+			return m_angularLimits[p_axis].m_maxMotorForce;
+
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+	return 0;
+}
+
+void GodotGeneric6DOFJoint3D::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
+	ERR_FAIL_INDEX(p_axis, 3);
+
+	switch (p_flag) {
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
+			m_linearLimits.enable_limit[p_axis] = p_value;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
+			m_angularLimits[p_axis].m_enableLimit = p_value;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
+			m_angularLimits[p_axis].m_enableMotor = p_value;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+bool GodotGeneric6DOFJoint3D::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
+	ERR_FAIL_INDEX_V(p_axis, 3, false);
+	switch (p_flag) {
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
+			return m_linearLimits.enable_limit[p_axis];
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
+			return m_angularLimits[p_axis].m_enableLimit;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
+			return m_angularLimits[p_axis].m_enableMotor;
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: {
+			// Not implemented in GodotPhysics3D backend
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return false;
+}

modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.h

@@ -0,0 +1,319 @@
+/**************************************************************************/
+/*  godot_generic_6dof_joint_3d.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
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.h"
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+2007-09-09
+GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+//! Rotation Limit structure for generic joints
+class GodotG6DOFRotationalLimitMotor3D {
+public:
+	//! limit_parameters
+	//!@{
+	real_t m_loLimit = -1e30; //!< joint limit
+	real_t m_hiLimit = 1e30; //!< joint limit
+	real_t m_targetVelocity = 0.0; //!< target motor velocity
+	real_t m_maxMotorForce = 0.1; //!< max force on motor
+	real_t m_maxLimitForce = 300.0; //!< max force on limit
+	real_t m_damping = 1.0; //!< Damping.
+	real_t m_limitSoftness = 0.5; //! Relaxation factor
+	real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit
+	real_t m_bounce = 0.0; //!< restitution factor
+	bool m_enableMotor = false;
+	bool m_enableLimit = false;
+
+	//!@}
+
+	//! temp_variables
+	//!@{
+	real_t m_currentLimitError = 0.0; //!< How much is violated this limit
+	int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit
+	real_t m_accumulatedImpulse = 0.0;
+	//!@}
+
+	GodotG6DOFRotationalLimitMotor3D() {}
+
+	bool isLimited() {
+		return (m_loLimit < m_hiLimit);
+	}
+
+	// Need apply correction.
+	bool needApplyTorques() {
+		return (m_enableMotor || m_currentLimit != 0);
+	}
+
+	// Calculates m_currentLimit and m_currentLimitError.
+	int testLimitValue(real_t test_value);
+
+	// Apply the correction impulses for two bodies.
+	real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic);
+};
+
+class GodotG6DOFTranslationalLimitMotor3D {
+public:
+	Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits
+	Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits
+	Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0);
+	//! Linear_Limit_parameters
+	//!@{
+	Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit
+	Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit
+	Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit
+	//!@}
+	bool enable_limit[3] = { true, true, true };
+
+	//! Test limit
+	/*!
+	 * - free means upper < lower,
+	 * - locked means upper == lower
+	 * - limited means upper > lower
+	 * - limitIndex: first 3 are linear, next 3 are angular
+	 */
+	inline bool isLimited(int limitIndex) {
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+
+	real_t solveLinearAxis(
+			real_t timeStep,
+			real_t jacDiagABInv,
+			GodotBody3D *body1, const Vector3 &pointInA,
+			GodotBody3D *body2, const Vector3 &pointInB,
+			bool p_body1_dynamic, bool p_body2_dynamic,
+			int limit_index,
+			const Vector3 &axis_normal_on_a,
+			const Vector3 &anchorPos);
+};
+
+class GodotGeneric6DOFJoint3D : public GodotJoint3D {
+protected:
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
+	};
+
+	//! relative_frames
+	//!@{
+	Transform3D m_frameInA; //!< the constraint space w.r.t body A
+	Transform3D m_frameInB; //!< the constraint space w.r.t body B
+	//!@}
+
+	//! Jacobians
+	//!@{
+	GodotJacobianEntry3D m_jacLinear[3]; //!< 3 orthogonal linear constraints
+	GodotJacobianEntry3D m_jacAng[3]; //!< 3 orthogonal angular constraints
+	//!@}
+
+	//! Linear_Limit_parameters
+	//!@{
+	GodotG6DOFTranslationalLimitMotor3D m_linearLimits;
+	//!@}
+
+	//! hinge_parameters
+	//!@{
+	GodotG6DOFRotationalLimitMotor3D m_angularLimits[3];
+	//!@}
+
+protected:
+	//! temporal variables
+	//!@{
+	real_t m_timeStep = 0.0;
+	Transform3D m_calculatedTransformA;
+	Transform3D m_calculatedTransformB;
+	Vector3 m_calculatedAxisAngleDiff;
+	Vector3 m_calculatedAxis[3];
+
+	Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes
+
+	bool m_useLinearReferenceFrameA = false;
+
+	//!@}
+
+	GodotGeneric6DOFJoint3D(GodotGeneric6DOFJoint3D const &) = delete;
+	void operator=(GodotGeneric6DOFJoint3D const &) = delete;
+
+	void buildLinearJacobian(
+			GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
+			const Vector3 &pivotAInW, const Vector3 &pivotBInW);
+
+	void buildAngularJacobian(GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW);
+
+	//! calcs the euler angles between the two bodies.
+	void calculateAngleInfo();
+
+public:
+	GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA);
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	// Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies.
+	void calculateTransforms();
+
+	// Gets the global transform of the offset for body A.
+	const Transform3D &getCalculatedTransformA() const {
+		return m_calculatedTransformA;
+	}
+
+	// Gets the global transform of the offset for body B.
+	const Transform3D &getCalculatedTransformB() const {
+		return m_calculatedTransformB;
+	}
+
+	const Transform3D &getFrameOffsetA() const {
+		return m_frameInA;
+	}
+
+	const Transform3D &getFrameOffsetB() const {
+		return m_frameInB;
+	}
+
+	Transform3D &getFrameOffsetA() {
+		return m_frameInA;
+	}
+
+	Transform3D &getFrameOffsetB() {
+		return m_frameInB;
+	}
+
+	// Performs Jacobian calculation, and also calculates angle differences and axis.
+	void updateRHS(real_t timeStep);
+
+	// Get the rotation axis in global coordinates.
+	Vector3 getAxis(int axis_index) const;
+
+	// Get the relative Euler angle.
+	real_t getAngle(int axis_index) const;
+
+	// Calculates angular correction and returns true if limit needs to be corrected.
+	bool testAngularLimitMotor(int axis_index);
+
+	void setLinearLowerLimit(const Vector3 &linearLower) {
+		m_linearLimits.m_lowerLimit = linearLower;
+	}
+
+	void setLinearUpperLimit(const Vector3 &linearUpper) {
+		m_linearLimits.m_upperLimit = linearUpper;
+	}
+
+	void setAngularLowerLimit(const Vector3 &angularLower) {
+		m_angularLimits[0].m_loLimit = angularLower.x;
+		m_angularLimits[1].m_loLimit = angularLower.y;
+		m_angularLimits[2].m_loLimit = angularLower.z;
+	}
+
+	void setAngularUpperLimit(const Vector3 &angularUpper) {
+		m_angularLimits[0].m_hiLimit = angularUpper.x;
+		m_angularLimits[1].m_hiLimit = angularUpper.y;
+		m_angularLimits[2].m_hiLimit = angularUpper.z;
+	}
+
+	// Retrieves the angular limit information.
+	GodotG6DOFRotationalLimitMotor3D *getRotationalLimitMotor(int index) {
+		return &m_angularLimits[index];
+	}
+
+	// Retrieves the limit information.
+	GodotG6DOFTranslationalLimitMotor3D *getTranslationalLimitMotor() {
+		return &m_linearLimits;
+	}
+
+	// First 3 are linear, next 3 are angular.
+	void setLimit(int axis, real_t lo, real_t hi) {
+		if (axis < 3) {
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		} else {
+			m_angularLimits[axis - 3].m_loLimit = lo;
+			m_angularLimits[axis - 3].m_hiLimit = hi;
+		}
+	}
+
+	//! Test limit
+	/*!
+	 * - free means upper < lower,
+	 * - locked means upper == lower
+	 * - limited means upper > lower
+	 * - limitIndex: first 3 are linear, next 3 are angular
+	 */
+	bool isLimited(int limitIndex) {
+		if (limitIndex < 3) {
+			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex - 3].isLimited();
+	}
+
+	const GodotBody3D *getRigidBodyA() const {
+		return A;
+	}
+	const GodotBody3D *getRigidBodyB() const {
+		return B;
+	}
+
+	virtual void calcAnchorPos(); // overridable
+
+	void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value);
+	real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const;
+
+	void set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value);
+	bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const;
+};

modules/godot_physics_3d/joints/godot_hinge_joint_3d.cpp

@@ -0,0 +1,441 @@
+/**************************************************************************/
+/*  godot_hinge_joint_3d.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.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "godot_hinge_joint_3d.h"
+
+GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB) :
+		GodotJoint3D(_arr, 2) {
+	A = rbA;
+	B = rbB;
+
+	m_rbAFrame = frameA;
+	m_rbBFrame = frameB;
+	// flip axis
+	m_rbBFrame.basis[0][2] *= real_t(-1.);
+	m_rbBFrame.basis[1][2] *= real_t(-1.);
+	m_rbBFrame.basis[2][2] *= real_t(-1.);
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
+		const Vector3 &axisInA, const Vector3 &axisInB) :
+		GodotJoint3D(_arr, 2) {
+	A = rbA;
+	B = rbB;
+
+	m_rbAFrame.origin = pivotInA;
+
+	// since no frame is given, assume this to be zero angle and just pick rb transform axis
+	Vector3 rbAxisA1 = rbA->get_transform().basis.get_column(0);
+
+	Vector3 rbAxisA2;
+	real_t projection = axisInA.dot(rbAxisA1);
+	if (projection >= 1.0f - CMP_EPSILON) {
+		rbAxisA1 = -rbA->get_transform().basis.get_column(2);
+		rbAxisA2 = rbA->get_transform().basis.get_column(1);
+	} else if (projection <= -1.0f + CMP_EPSILON) {
+		rbAxisA1 = rbA->get_transform().basis.get_column(2);
+		rbAxisA2 = rbA->get_transform().basis.get_column(1);
+	} else {
+		rbAxisA2 = axisInA.cross(rbAxisA1);
+		rbAxisA1 = rbAxisA2.cross(axisInA);
+	}
+
+	m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x,
+			rbAxisA1.y, rbAxisA2.y, axisInA.y,
+			rbAxisA1.z, rbAxisA2.z, axisInA.z);
+
+	Quaternion rotationArc = Quaternion(axisInA, axisInB);
+	Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1);
+	Vector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+
+	m_rbBFrame.origin = pivotInB;
+	m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x,
+			rbAxisB1.y, rbAxisB2.y, -axisInB.y,
+			rbAxisB1.z, rbAxisB2.z, -axisInB.z);
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+bool GodotHingeJoint3D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	m_appliedImpulse = real_t(0.);
+
+	if (!m_angularOnly) {
+		Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
+		Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
+		Vector3 relPos = pivotBInW - pivotAInW;
+
+		Vector3 normal[3];
+		if (Math::is_zero_approx(relPos.length_squared())) {
+			normal[0] = Vector3(real_t(1.0), 0, 0);
+		} else {
+			normal[0] = relPos.normalized();
+		}
+
+		plane_space(normal[0], normal[1], normal[2]);
+
+		for (int i = 0; i < 3; i++) {
+			memnew_placement(
+					&m_jac[i],
+					GodotJacobianEntry3D(
+							A->get_principal_inertia_axes().transposed(),
+							B->get_principal_inertia_axes().transposed(),
+							pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+							pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+							normal[i],
+							A->get_inv_inertia(),
+							A->get_inv_mass(),
+							B->get_inv_inertia(),
+							B->get_inv_mass()));
+		}
+	}
+
+	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
+	//these two jointAxis require equal angular velocities for both bodies
+
+	//this is unused for now, it's a todo
+	Vector3 jointAxis0local;
+	Vector3 jointAxis1local;
+
+	plane_space(m_rbAFrame.basis.get_column(2), jointAxis0local, jointAxis1local);
+
+	Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local);
+	Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local);
+	Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2));
+
+	memnew_placement(
+			&m_jacAng[0],
+			GodotJacobianEntry3D(
+					jointAxis0,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
+
+	memnew_placement(
+			&m_jacAng[1],
+			GodotJacobianEntry3D(
+					jointAxis1,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
+
+	memnew_placement(
+			&m_jacAng[2],
+			GodotJacobianEntry3D(
+					hingeAxisWorld,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
+
+	// Compute limit information
+	real_t hingeAngle = get_hinge_angle();
+
+	//set bias, sign, clear accumulator
+	m_correction = real_t(0.);
+	m_limitSign = real_t(0.);
+	m_solveLimit = false;
+	m_accLimitImpulse = real_t(0.);
+
+	if (m_useLimit && m_lowerLimit <= m_upperLimit) {
+		if (hingeAngle <= m_lowerLimit) {
+			m_correction = (m_lowerLimit - hingeAngle);
+			m_limitSign = 1.0f;
+			m_solveLimit = true;
+		} else if (hingeAngle >= m_upperLimit) {
+			m_correction = m_upperLimit - hingeAngle;
+			m_limitSign = -1.0f;
+			m_solveLimit = true;
+		}
+	}
+
+	//Compute K = J*W*J' for hinge axis
+	Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2));
+	m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
+
+	return true;
+}
+
+void GodotHingeJoint3D::solve(real_t p_step) {
+	Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
+	Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
+
+	//real_t tau = real_t(0.3);
+
+	//linear part
+	if (!m_angularOnly) {
+		Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+		Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+
+		Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
+		Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
+		Vector3 vel = vel1 - vel2;
+
+		for (int i = 0; i < 3; i++) {
+			const Vector3 &normal = m_jac[i].m_linearJointAxis;
+			real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
+
+			real_t rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv;
+			m_appliedImpulse += impulse;
+			Vector3 impulse_vector = normal * impulse;
+			if (dynamic_A) {
+				A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+			}
+			if (dynamic_B) {
+				B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+			}
+		}
+	}
+
+	{
+		///solve angular part
+
+		// get axes in world space
+		Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2));
+		Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(2));
+
+		const Vector3 &angVelA = A->get_angular_velocity();
+		const Vector3 &angVelB = B->get_angular_velocity();
+
+		Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+		Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+		Vector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+		Vector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+		Vector3 velrelOrthog = angAorthog - angBorthog;
+		{
+			//solve orthogonal angular velocity correction
+			real_t relaxation = real_t(1.);
+			real_t len = velrelOrthog.length();
+			if (len > real_t(0.00001)) {
+				Vector3 normal = velrelOrthog.normalized();
+				real_t denom = A->compute_angular_impulse_denominator(normal) +
+						B->compute_angular_impulse_denominator(normal);
+				// scale for mass and relaxation
+				velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor;
+			}
+
+			//solve angular positional correction
+			Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step);
+			real_t len2 = angularError.length();
+			if (len2 > real_t(0.00001)) {
+				Vector3 normal2 = angularError.normalized();
+				real_t denom2 = A->compute_angular_impulse_denominator(normal2) +
+						B->compute_angular_impulse_denominator(normal2);
+				angularError *= (real_t(1.) / denom2) * relaxation;
+			}
+
+			if (dynamic_A) {
+				A->apply_torque_impulse(-velrelOrthog + angularError);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(velrelOrthog - angularError);
+			}
+
+			// solve limit
+			if (m_solveLimit) {
+				real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign;
+
+				real_t impulseMag = amplitude * m_kHinge;
+
+				// Clamp the accumulated impulse
+				real_t temp = m_accLimitImpulse;
+				m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0));
+				impulseMag = m_accLimitImpulse - temp;
+
+				Vector3 impulse = axisA * impulseMag * m_limitSign;
+				if (dynamic_A) {
+					A->apply_torque_impulse(impulse);
+				}
+				if (dynamic_B) {
+					B->apply_torque_impulse(-impulse);
+				}
+			}
+		}
+
+		//apply motor
+		if (m_enableAngularMotor) {
+			//todo: add limits too
+			Vector3 angularLimit(0, 0, 0);
+
+			Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+			real_t projRelVel = velrel.dot(axisA);
+
+			real_t desiredMotorVel = m_motorTargetVelocity;
+			real_t motor_relvel = desiredMotorVel - projRelVel;
+
+			real_t unclippedMotorImpulse = m_kHinge * motor_relvel;
+			//todo: should clip against accumulated impulse
+			real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+			Vector3 motorImp = clippedMotorImpulse * axisA;
+
+			if (dynamic_A) {
+				A->apply_torque_impulse(motorImp + angularLimit);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-motorImp - angularLimit);
+			}
+		}
+	}
+}
+
+/*
+void	HingeJointSW::updateRHS(real_t	timeStep)
+{
+	(void)timeStep;
+}
+
+*/
+
+real_t GodotHingeJoint3D::get_hinge_angle() {
+	const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0));
+	const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1));
+	const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1));
+
+	return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+}
+
+void GodotHingeJoint3D::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::HINGE_JOINT_BIAS:
+			tau = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER:
+			m_upperLimit = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER:
+			m_lowerLimit = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS:
+			m_biasFactor = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS:
+			m_limitSoftness = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION:
+			m_relaxationFactor = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
+			m_motorTargetVelocity = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE:
+			m_maxMotorImpulse = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+real_t GodotHingeJoint3D::get_param(PhysicsServer3D::HingeJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::HINGE_JOINT_BIAS:
+			return tau;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER:
+			return m_upperLimit;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER:
+			return m_lowerLimit;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS:
+			return m_biasFactor;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS:
+			return m_limitSoftness;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION:
+			return m_relaxationFactor;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
+			return m_motorTargetVelocity;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE:
+			return m_maxMotorImpulse;
+		case PhysicsServer3D::HINGE_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return 0;
+}
+
+void GodotHingeJoint3D::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
+	switch (p_flag) {
+		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
+			m_useLimit = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR:
+			m_enableAngularMotor = p_value;
+			break;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+bool GodotHingeJoint3D::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
+	switch (p_flag) {
+		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
+			return m_useLimit;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR:
+			return m_enableAngularMotor;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return false;
+}

modules/godot_physics_3d/joints/godot_hinge_joint_3d.h

@@ -0,0 +1,113 @@
+/**************************************************************************/
+/*  godot_hinge_joint_3d.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
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.h"
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+class GodotHingeJoint3D : public GodotJoint3D {
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = {};
+	};
+
+	GodotJacobianEntry3D m_jac[3]; //3 orthogonal linear constraints
+	GodotJacobianEntry3D m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+
+	Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	Transform3D m_rbBFrame;
+
+	real_t m_motorTargetVelocity = 0.0;
+	real_t m_maxMotorImpulse = 0.0;
+
+	real_t m_limitSoftness = 0.9;
+	real_t m_biasFactor = 0.3;
+	real_t m_relaxationFactor = 1.0;
+
+	real_t m_lowerLimit = Math::PI;
+	real_t m_upperLimit = -Math::PI;
+
+	real_t m_kHinge = 0.0;
+
+	real_t m_limitSign = 0.0;
+	real_t m_correction = 0.0;
+
+	real_t m_accLimitImpulse = 0.0;
+
+	real_t tau = 0.3;
+
+	bool m_useLimit = false;
+	bool m_angularOnly = false;
+	bool m_enableAngularMotor = false;
+	bool m_solveLimit = false;
+
+	real_t m_appliedImpulse = 0.0;
+
+public:
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	real_t get_hinge_angle();
+
+	void set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::HingeJointParam p_param) const;
+
+	void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value);
+	bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const;
+
+	GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB);
+	GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
+};

modules/godot_physics_3d/joints/godot_jacobian_entry_3d.h

@@ -0,0 +1,166 @@
+/**************************************************************************/
+/*  godot_jacobian_entry_3d.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
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "core/math/transform_3d.h"
+
+class GodotJacobianEntry3D {
+public:
+	GodotJacobianEntry3D() {}
+	//constraint between two different rigidbodies
+	GodotJacobianEntry3D(
+			const Basis &world2A,
+			const Basis &world2B,
+			const Vector3 &rel_pos1, const Vector3 &rel_pos2,
+			const Vector3 &jointAxis,
+			const Vector3 &inertiaInvA,
+			const real_t massInvA,
+			const Vector3 &inertiaInvB,
+			const real_t massInvB) :
+			m_linearJointAxis(jointAxis) {
+		m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis));
+		m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
+
+		ERR_FAIL_COND(m_Adiag <= real_t(0.0));
+	}
+
+	//angular constraint between two different rigidbodies
+	GodotJacobianEntry3D(const Vector3 &jointAxis,
+			const Basis &world2A,
+			const Basis &world2B,
+			const Vector3 &inertiaInvA,
+			const Vector3 &inertiaInvB) :
+			m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) {
+		m_aJ = world2A.xform(jointAxis);
+		m_bJ = world2B.xform(-jointAxis);
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+		ERR_FAIL_COND(m_Adiag <= real_t(0.0));
+	}
+
+	//angular constraint between two different rigidbodies
+	GodotJacobianEntry3D(const Vector3 &axisInA,
+			const Vector3 &axisInB,
+			const Vector3 &inertiaInvA,
+			const Vector3 &inertiaInvB) :
+			m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))),
+			m_aJ(axisInA),
+			m_bJ(-axisInB) {
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+		ERR_FAIL_COND(m_Adiag <= real_t(0.0));
+	}
+
+	//constraint on one rigidbody
+	GodotJacobianEntry3D(
+			const Basis &world2A,
+			const Vector3 &rel_pos1, const Vector3 &rel_pos2,
+			const Vector3 &jointAxis,
+			const Vector3 &inertiaInvA,
+			const real_t massInvA) :
+			m_linearJointAxis(jointAxis) {
+		m_aJ = world2A.xform(rel_pos1.cross(jointAxis));
+		m_bJ = world2A.xform(rel_pos2.cross(-jointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.));
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
+
+		ERR_FAIL_COND(m_Adiag <= real_t(0.0));
+	}
+
+	real_t getDiagonal() const { return m_Adiag; }
+
+	// for two constraints on the same rigidbody (for example vehicle friction)
+	real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA) const {
+		const GodotJacobianEntry3D &jacA = *this;
+		real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
+		real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
+		return lin + ang;
+	}
+
+	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
+	real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA, const real_t massInvB) const {
+		const GodotJacobianEntry3D &jacA = *this;
+		Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
+		Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
+		Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
+		Vector3 lin0 = massInvA * lin;
+		Vector3 lin1 = massInvB * lin;
+		Vector3 sum = ang0 + ang1 + lin0 + lin1;
+		return sum[0] + sum[1] + sum[2];
+	}
+
+	real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) {
+		Vector3 linrel = linvelA - linvelB;
+		Vector3 angvela = angvelA * m_aJ;
+		Vector3 angvelb = angvelB * m_bJ;
+		linrel *= m_linearJointAxis;
+		angvela += angvelb;
+		angvela += linrel;
+		real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2];
+		return rel_vel2 + CMP_EPSILON;
+	}
+	//private:
+
+	Vector3 m_linearJointAxis;
+	Vector3 m_aJ;
+	Vector3 m_bJ;
+	Vector3 m_0MinvJt;
+	Vector3 m_1MinvJt;
+	//Optimization: can be stored in the w/last component of one of the vectors
+	real_t m_Adiag = 1.0;
+};

modules/godot_physics_3d/joints/godot_pin_joint_3d.cpp

@@ -0,0 +1,181 @@
+/**************************************************************************/
+/*  godot_pin_joint_3d.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.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "godot_pin_joint_3d.h"
+
+bool GodotPinJoint3D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	m_appliedImpulse = real_t(0.);
+
+	Vector3 normal(0, 0, 0);
+
+	for (int i = 0; i < 3; i++) {
+		normal[i] = 1;
+		memnew_placement(
+				&m_jac[i],
+				GodotJacobianEntry3D(
+						A->get_principal_inertia_axes().transposed(),
+						B->get_principal_inertia_axes().transposed(),
+						A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
+						B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
+						normal,
+						A->get_inv_inertia(),
+						A->get_inv_mass(),
+						B->get_inv_inertia(),
+						B->get_inv_mass()));
+		normal[i] = 0;
+	}
+
+	return true;
+}
+
+void GodotPinJoint3D::solve(real_t p_step) {
+	Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
+	Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
+
+	Vector3 normal(0, 0, 0);
+
+	//Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity();
+	//Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity();
+
+	for (int i = 0; i < 3; i++) {
+		normal[i] = 1;
+		real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
+
+		Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+		Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+		//this jacobian entry could be reused for all iterations
+
+		Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
+		Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
+		Vector3 vel = vel1 - vel2;
+
+		real_t rel_vel;
+		rel_vel = normal.dot(vel);
+
+		/*
+		//velocity error (first order error)
+		real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA,
+														B->getLinearVelocity(),angvelB);
+	*/
+
+		//positional error (zeroth order error)
+		real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+
+		real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv;
+
+		real_t impulseClamp = m_impulseClamp;
+		if (impulseClamp > 0) {
+			if (impulse < -impulseClamp) {
+				impulse = -impulseClamp;
+			}
+			if (impulse > impulseClamp) {
+				impulse = impulseClamp;
+			}
+		}
+
+		m_appliedImpulse += impulse;
+		Vector3 impulse_vector = normal * impulse;
+		if (dynamic_A) {
+			A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+		}
+		if (dynamic_B) {
+			B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
+		}
+
+		normal[i] = 0;
+	}
+}
+
+void GodotPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::PIN_JOINT_BIAS:
+			m_tau = p_value;
+			break;
+		case PhysicsServer3D::PIN_JOINT_DAMPING:
+			m_damping = p_value;
+			break;
+		case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP:
+			m_impulseClamp = p_value;
+			break;
+	}
+}
+
+real_t GodotPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::PIN_JOINT_BIAS:
+			return m_tau;
+		case PhysicsServer3D::PIN_JOINT_DAMPING:
+			return m_damping;
+		case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP:
+			return m_impulseClamp;
+	}
+
+	return 0;
+}
+
+GodotPinJoint3D::GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b) :
+		GodotJoint3D(_arr, 2) {
+	A = p_body_a;
+	B = p_body_b;
+	m_pivotInA = p_pos_a;
+	m_pivotInB = p_pos_b;
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+GodotPinJoint3D::~GodotPinJoint3D() {
+}

modules/godot_physics_3d/joints/godot_pin_joint_3d.h

@@ -0,0 +1,92 @@
+/**************************************************************************/
+/*  godot_pin_joint_3d.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
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.h"
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+class GodotPinJoint3D : public GodotJoint3D {
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = {};
+	};
+
+	real_t m_tau = 0.3; //bias
+	real_t m_damping = 1.0;
+	real_t m_impulseClamp = 0.0;
+	real_t m_appliedImpulse = 0.0;
+
+	GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
+
+	Vector3 m_pivotInA;
+	Vector3 m_pivotInB;
+
+public:
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; }
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::PinJointParam p_param) const;
+
+	void set_pos_a(const Vector3 &p_pos) { m_pivotInA = p_pos; }
+	void set_pos_b(const Vector3 &p_pos) { m_pivotInB = p_pos; }
+
+	Vector3 get_position_a() { return m_pivotInA; }
+	Vector3 get_position_b() { return m_pivotInB; }
+
+	GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b);
+	~GodotPinJoint3D();
+};

modules/godot_physics_3d/joints/godot_slider_joint_3d.cpp

@@ -0,0 +1,478 @@
+/**************************************************************************/
+/*  godot_slider_joint_3d.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.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+Added by Roman Ponomarev (rponom@gmail.com)
+April 04, 2008
+
+*/
+
+#include "godot_slider_joint_3d.h"
+
+//-----------------------------------------------------------------------------
+
+GodotSliderJoint3D::GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
+		GodotJoint3D(_arr, 2),
+		m_frameInA(frameInA),
+		m_frameInB(frameInB) {
+	A = rbA;
+	B = rbB;
+
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+}
+
+//-----------------------------------------------------------------------------
+
+bool GodotSliderJoint3D::setup(real_t p_step) {
+	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
+
+	if (!dynamic_A && !dynamic_B) {
+		return false;
+	}
+
+	//calculate transforms
+	m_calculatedTransformA = A->get_transform() * m_frameInA;
+	m_calculatedTransformB = B->get_transform() * m_frameInB;
+	m_realPivotAInW = m_calculatedTransformA.origin;
+	m_realPivotBInW = m_calculatedTransformB.origin;
+	m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X
+	m_delta = m_realPivotBInW - m_realPivotAInW;
+	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+	m_relPosA = m_projPivotInW - A->get_transform().origin;
+	m_relPosB = m_realPivotBInW - B->get_transform().origin;
+	Vector3 normalWorld;
+	int i;
+	//linear part
+	for (i = 0; i < 3; i++) {
+		normalWorld = m_calculatedTransformA.basis.get_column(i);
+		memnew_placement(
+				&m_jacLin[i],
+				GodotJacobianEntry3D(
+						A->get_principal_inertia_axes().transposed(),
+						B->get_principal_inertia_axes().transposed(),
+						m_relPosA - A->get_center_of_mass(),
+						m_relPosB - B->get_center_of_mass(),
+						normalWorld,
+						A->get_inv_inertia(),
+						A->get_inv_mass(),
+						B->get_inv_inertia(),
+						B->get_inv_mass()));
+		m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal();
+		m_depth[i] = m_delta.dot(normalWorld);
+	}
+	testLinLimits();
+	// angular part
+	for (i = 0; i < 3; i++) {
+		normalWorld = m_calculatedTransformA.basis.get_column(i);
+		memnew_placement(
+				&m_jacAng[i],
+				GodotJacobianEntry3D(
+						normalWorld,
+						A->get_principal_inertia_axes().transposed(),
+						B->get_principal_inertia_axes().transposed(),
+						A->get_inv_inertia(),
+						B->get_inv_inertia()));
+	}
+	testAngLimits();
+	Vector3 axisA = m_calculatedTransformA.basis.get_column(0);
+	m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
+	// clear accumulator for motors
+	m_accumulatedLinMotorImpulse = real_t(0.0);
+	m_accumulatedAngMotorImpulse = real_t(0.0);
+
+	return true;
+}
+
+//-----------------------------------------------------------------------------
+
+void GodotSliderJoint3D::solve(real_t p_step) {
+	int i;
+	// linear
+	Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
+	Vector3 velB = B->get_velocity_in_local_point(m_relPosB);
+	Vector3 vel = velA - velB;
+	for (i = 0; i < 3; i++) {
+		const Vector3 &normal = m_jacLin[i].m_linearJointAxis;
+		real_t rel_vel = normal.dot(vel);
+		// calculate positional error
+		real_t depth = m_depth[i];
+		// get parameters
+		real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
+		real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
+		real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
+		// Calculate and apply impulse.
+		real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
+		Vector3 impulse_vector = normal * normalImpulse;
+		if (dynamic_A) {
+			A->apply_impulse(impulse_vector, m_relPosA);
+		}
+		if (dynamic_B) {
+			B->apply_impulse(-impulse_vector, m_relPosB);
+		}
+		if (m_poweredLinMotor && (!i)) { // apply linear motor
+			if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
+				real_t desiredMotorVel = m_targetLinMotorVelocity;
+				real_t motor_relvel = desiredMotorVel + rel_vel;
+				normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
+				// clamp accumulated impulse
+				real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse);
+				if (new_acc > m_maxLinMotorForce) {
+					new_acc = m_maxLinMotorForce;
+				}
+				real_t del = new_acc - m_accumulatedLinMotorImpulse;
+				if (normalImpulse < real_t(0.0)) {
+					normalImpulse = -del;
+				} else {
+					normalImpulse = del;
+				}
+				m_accumulatedLinMotorImpulse = new_acc;
+				// apply clamped impulse
+				impulse_vector = normal * normalImpulse;
+				if (dynamic_A) {
+					A->apply_impulse(impulse_vector, m_relPosA);
+				}
+				if (dynamic_B) {
+					B->apply_impulse(-impulse_vector, m_relPosB);
+				}
+			}
+		}
+	}
+	// angular
+	// get axes in world space
+	Vector3 axisA = m_calculatedTransformA.basis.get_column(0);
+	Vector3 axisB = m_calculatedTransformB.basis.get_column(0);
+
+	const Vector3 &angVelA = A->get_angular_velocity();
+	const Vector3 &angVelB = B->get_angular_velocity();
+
+	Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
+	Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
+
+	Vector3 angAorthog = angVelA - angVelAroundAxisA;
+	Vector3 angBorthog = angVelB - angVelAroundAxisB;
+	Vector3 velrelOrthog = angAorthog - angBorthog;
+	//solve orthogonal angular velocity correction
+	real_t len = velrelOrthog.length();
+	if (len > real_t(0.00001)) {
+		Vector3 normal = velrelOrthog.normalized();
+		real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal);
+		velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+	}
+	//solve angular positional correction
+	Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step);
+	real_t len2 = angularError.length();
+	if (len2 > real_t(0.00001)) {
+		Vector3 normal2 = angularError.normalized();
+		real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2);
+		angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+	}
+	// apply impulse
+	if (dynamic_A) {
+		A->apply_torque_impulse(-velrelOrthog + angularError);
+	}
+	if (dynamic_B) {
+		B->apply_torque_impulse(velrelOrthog - angularError);
+	}
+	real_t impulseMag;
+	//solve angular limits
+	if (m_solveAngLim) {
+		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step;
+		impulseMag *= m_kAngle * m_softnessLimAng;
+	} else {
+		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step;
+		impulseMag *= m_kAngle * m_softnessDirAng;
+	}
+	Vector3 impulse = axisA * impulseMag;
+	if (dynamic_A) {
+		A->apply_torque_impulse(impulse);
+	}
+	if (dynamic_B) {
+		B->apply_torque_impulse(-impulse);
+	}
+	//apply angular motor
+	if (m_poweredAngMotor) {
+		if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) {
+			Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
+			real_t projRelVel = velrel.dot(axisA);
+
+			real_t desiredMotorVel = m_targetAngMotorVelocity;
+			real_t motor_relvel = desiredMotorVel - projRelVel;
+
+			real_t angImpulse = m_kAngle * motor_relvel;
+			// clamp accumulated impulse
+			real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse);
+			if (new_acc > m_maxAngMotorForce) {
+				new_acc = m_maxAngMotorForce;
+			}
+			real_t del = new_acc - m_accumulatedAngMotorImpulse;
+			if (angImpulse < real_t(0.0)) {
+				angImpulse = -del;
+			} else {
+				angImpulse = del;
+			}
+			m_accumulatedAngMotorImpulse = new_acc;
+			// apply clamped impulse
+			Vector3 motorImp = angImpulse * axisA;
+			if (dynamic_A) {
+				A->apply_torque_impulse(motorImp);
+			}
+			if (dynamic_B) {
+				B->apply_torque_impulse(-motorImp);
+			}
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void GodotSliderJoint3D::calculateTransforms() {
+	m_calculatedTransformA = A->get_transform() * m_frameInA;
+	m_calculatedTransformB = B->get_transform() * m_frameInB;
+	m_realPivotAInW = m_calculatedTransformA.origin;
+	m_realPivotBInW = m_calculatedTransformB.origin;
+	m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X
+	m_delta = m_realPivotBInW - m_realPivotAInW;
+	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+	Vector3 normalWorld;
+	int i;
+	//linear part
+	for (i = 0; i < 3; i++) {
+		normalWorld = m_calculatedTransformA.basis.get_column(i);
+		m_depth[i] = m_delta.dot(normalWorld);
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void GodotSliderJoint3D::testLinLimits() {
+	m_solveLinLim = false;
+	m_linPos = m_depth[0];
+	if (m_lowerLinLimit <= m_upperLinLimit) {
+		if (m_depth[0] > m_upperLinLimit) {
+			m_depth[0] -= m_upperLinLimit;
+			m_solveLinLim = true;
+		} else if (m_depth[0] < m_lowerLinLimit) {
+			m_depth[0] -= m_lowerLinLimit;
+			m_solveLinLim = true;
+		} else {
+			m_depth[0] = real_t(0.);
+		}
+	} else {
+		m_depth[0] = real_t(0.);
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void GodotSliderJoint3D::testAngLimits() {
+	m_angDepth = real_t(0.);
+	m_solveAngLim = false;
+	if (m_lowerAngLimit <= m_upperAngLimit) {
+		const Vector3 axisA0 = m_calculatedTransformA.basis.get_column(1);
+		const Vector3 axisA1 = m_calculatedTransformA.basis.get_column(2);
+		const Vector3 axisB0 = m_calculatedTransformB.basis.get_column(1);
+		real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+		if (rot < m_lowerAngLimit) {
+			m_angDepth = rot - m_lowerAngLimit;
+			m_solveAngLim = true;
+		} else if (rot > m_upperAngLimit) {
+			m_angDepth = rot - m_upperAngLimit;
+			m_solveAngLim = true;
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+Vector3 GodotSliderJoint3D::getAncorInA() {
+	Vector3 ancorInA;
+	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis;
+	ancorInA = A->get_transform().inverse().xform(ancorInA);
+	return ancorInA;
+}
+
+//-----------------------------------------------------------------------------
+
+Vector3 GodotSliderJoint3D::getAncorInB() {
+	Vector3 ancorInB;
+	ancorInB = m_frameInB.origin;
+	return ancorInB;
+}
+
+void GodotSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
+			m_upperLinLimit = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
+			m_lowerLinLimit = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
+			m_softnessLimLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
+			m_restitutionLimLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
+			m_dampingLimLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
+			m_softnessDirLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
+			m_restitutionDirLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
+			m_dampingDirLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
+			m_softnessOrthoLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
+			m_restitutionOrthoLin = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
+			m_dampingOrthoLin = p_value;
+			break;
+
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
+			m_upperAngLimit = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
+			m_lowerAngLimit = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
+			m_softnessLimAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
+			m_restitutionLimAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
+			m_dampingLimAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
+			m_softnessDirAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
+			m_restitutionDirAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
+			m_dampingDirAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
+			m_softnessOrthoAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
+			m_restitutionOrthoAng = p_value;
+			break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
+			m_dampingOrthoAng = p_value;
+			break;
+
+		case PhysicsServer3D::SLIDER_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+}
+
+real_t GodotSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
+			return m_upperLinLimit;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
+			return m_lowerLinLimit;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
+			return m_softnessLimLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
+			return m_restitutionLimLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
+			return m_dampingLimLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
+			return m_softnessDirLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
+			return m_restitutionDirLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
+			return m_dampingDirLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
+			return m_softnessOrthoLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
+			return m_restitutionOrthoLin;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
+			return m_dampingOrthoLin;
+
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
+			return m_upperAngLimit;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
+			return m_lowerAngLimit;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
+			return m_softnessLimAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
+			return m_restitutionLimAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
+			return m_dampingLimAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
+			return m_softnessDirAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
+			return m_restitutionDirAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
+			return m_dampingDirAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
+			return m_softnessOrthoAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
+			return m_restitutionOrthoAng;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
+			return m_dampingOrthoAng;
+
+		case PhysicsServer3D::SLIDER_JOINT_MAX:
+			break; // Can't happen, but silences warning
+	}
+
+	return 0;
+}

modules/godot_physics_3d/joints/godot_slider_joint_3d.h

@@ -0,0 +1,243 @@
+/**************************************************************************/
+/*  godot_slider_joint_3d.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
+
+/*
+Adapted to Godot from the Bullet library.
+*/
+
+#include "../godot_joint_3d.h"
+#include "godot_jacobian_entry_3d.h"
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+Added by Roman Ponomarev (rponom@gmail.com)
+April 04, 2008
+
+*/
+
+#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0))
+#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0))
+#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7))
+
+//-----------------------------------------------------------------------------
+
+class GodotSliderJoint3D : public GodotJoint3D {
+protected:
+	union {
+		struct {
+			GodotBody3D *A;
+			GodotBody3D *B;
+		};
+
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
+	};
+
+	Transform3D m_frameInA;
+	Transform3D m_frameInB;
+
+	// linear limits
+	real_t m_lowerLinLimit = 1.0;
+	real_t m_upperLinLimit = -1.0;
+	// angular limits
+	real_t m_lowerAngLimit = 0.0;
+	real_t m_upperAngLimit = 0.0;
+	// softness, restitution and damping for different cases
+	// DirLin - moving inside linear limits
+	// LimLin - hitting linear limit
+	// DirAng - moving inside angular limits
+	// LimAng - hitting angular limit
+	// OrthoLin, OrthoAng - against constraint axis
+	real_t m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingDirLin = 0.0;
+	real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingDirAng = 0.0;
+	real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+
+	// for interlal use
+	bool m_solveLinLim = false;
+	bool m_solveAngLim = false;
+
+	GodotJacobianEntry3D m_jacLin[3] = {};
+	real_t m_jacLinDiagABInv[3] = {};
+
+	GodotJacobianEntry3D m_jacAng[3] = {};
+
+	real_t m_timeStep = 0.0;
+	Transform3D m_calculatedTransformA;
+	Transform3D m_calculatedTransformB;
+
+	Vector3 m_sliderAxis;
+	Vector3 m_realPivotAInW;
+	Vector3 m_realPivotBInW;
+	Vector3 m_projPivotInW;
+	Vector3 m_delta;
+	Vector3 m_depth;
+	Vector3 m_relPosA;
+	Vector3 m_relPosB;
+
+	real_t m_linPos = 0.0;
+
+	real_t m_angDepth = 0.0;
+	real_t m_kAngle = 0.0;
+
+	bool m_poweredLinMotor = false;
+	real_t m_targetLinMotorVelocity = 0.0;
+	real_t m_maxLinMotorForce = 0.0;
+	real_t m_accumulatedLinMotorImpulse = 0.0;
+
+	bool m_poweredAngMotor = false;
+	real_t m_targetAngMotorVelocity = 0.0;
+	real_t m_maxAngMotorForce = 0.0;
+	real_t m_accumulatedAngMotorImpulse = 0.0;
+
+public:
+	// constructors
+	GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
+	//SliderJointSW();
+	// overrides
+
+	// access
+	const GodotBody3D *getRigidBodyA() const { return A; }
+	const GodotBody3D *getRigidBodyB() const { return B; }
+	const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; }
+	const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; }
+	const Transform3D &getFrameOffsetA() const { return m_frameInA; }
+	const Transform3D &getFrameOffsetB() const { return m_frameInB; }
+	Transform3D &getFrameOffsetA() { return m_frameInA; }
+	Transform3D &getFrameOffsetB() { return m_frameInB; }
+	real_t getLowerLinLimit() { return m_lowerLinLimit; }
+	void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; }
+	real_t getUpperLinLimit() { return m_upperLinLimit; }
+	void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; }
+	real_t getLowerAngLimit() { return m_lowerAngLimit; }
+	void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; }
+	real_t getUpperAngLimit() { return m_upperAngLimit; }
+	void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; }
+
+	real_t getSoftnessDirLin() { return m_softnessDirLin; }
+	real_t getRestitutionDirLin() { return m_restitutionDirLin; }
+	real_t getDampingDirLin() { return m_dampingDirLin; }
+	real_t getSoftnessDirAng() { return m_softnessDirAng; }
+	real_t getRestitutionDirAng() { return m_restitutionDirAng; }
+	real_t getDampingDirAng() { return m_dampingDirAng; }
+	real_t getSoftnessLimLin() { return m_softnessLimLin; }
+	real_t getRestitutionLimLin() { return m_restitutionLimLin; }
+	real_t getDampingLimLin() { return m_dampingLimLin; }
+	real_t getSoftnessLimAng() { return m_softnessLimAng; }
+	real_t getRestitutionLimAng() { return m_restitutionLimAng; }
+	real_t getDampingLimAng() { return m_dampingLimAng; }
+	real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; }
+	real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
+	real_t getDampingOrthoLin() { return m_dampingOrthoLin; }
+	real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; }
+	real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
+	real_t getDampingOrthoAng() { return m_dampingOrthoAng; }
+	void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; }
+	void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
+	void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; }
+	void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; }
+	void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
+	void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; }
+	void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; }
+	void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
+	void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; }
+	void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; }
+	void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
+	void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; }
+	void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
+	void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
+	void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
+	void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
+	void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
+	void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
+	void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
+	bool getPoweredLinMotor() { return m_poweredLinMotor; }
+	void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
+	real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
+	void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
+	real_t getMaxLinMotorForce() { return m_maxLinMotorForce; }
+	void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
+	bool getPoweredAngMotor() { return m_poweredAngMotor; }
+	void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
+	real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
+	void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
+	real_t getMaxAngMotorForce() { return m_maxAngMotorForce; }
+	real_t getLinearPos() { return m_linPos; }
+
+	// access for ODE solver
+	bool getSolveLinLimit() { return m_solveLinLim; }
+	real_t getLinDepth() { return m_depth[0]; }
+	bool getSolveAngLimit() { return m_solveAngLim; }
+	real_t getAngDepth() { return m_angDepth; }
+	// shared code used by ODE solver
+	void calculateTransforms();
+	void testLinLimits();
+	void testAngLimits();
+	// access for PE Solver
+	Vector3 getAncorInA();
+	Vector3 getAncorInB();
+
+	void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer3D::SliderJointParam p_param) const;
+
+	virtual bool setup(real_t p_step) override;
+	virtual void solve(real_t p_step) override;
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
+};

modules/godot_physics_3d/register_types.cpp

@@ -0,0 +1,61 @@
+/**************************************************************************/
+/*  register_types.cpp                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "register_types.h"
+
+#include "godot_physics_server_3d.h"
+#include "servers/physics_server_3d.h"
+#include "servers/physics_server_3d_wrap_mt.h"
+
+static PhysicsServer3D *_createGodotPhysics3DCallback() {
+#ifdef THREADS_ENABLED
+	bool using_threads = GLOBAL_GET("physics/3d/run_on_separate_thread");
+#else
+	bool using_threads = false;
+#endif
+
+	PhysicsServer3D *physics_server_3d = memnew(GodotPhysicsServer3D(using_threads));
+
+	return memnew(PhysicsServer3DWrapMT(physics_server_3d, using_threads));
+}
+
+void initialize_godot_physics_3d_module(ModuleInitializationLevel p_level) {
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		return;
+	}
+	PhysicsServer3DManager::get_singleton()->register_server("GodotPhysics3D", callable_mp_static(_createGodotPhysics3DCallback));
+	PhysicsServer3DManager::get_singleton()->set_default_server("GodotPhysics3D");
+}
+
+void uninitialize_godot_physics_3d_module(ModuleInitializationLevel p_level) {
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		return;
+	}
+}

modules/godot_physics_3d/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_3d_module(ModuleInitializationLevel p_level);
+void uninitialize_godot_physics_3d_module(ModuleInitializationLevel p_level);