16 files changed, 514 insertions, 180 deletions

diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 64ba0cb09d..d54345821d 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -693,15 +693,16 @@ void Body3DSW::call_queries() {
 
 		Variant v = dbs;
 
-		Object *obj = ObjectDB::get_instance(fi_callback->id);
+		Object *obj = fi_callback->callable.get_object();
 		if (!obj) {
-			set_force_integration_callback(ObjectID(), StringName());
+			set_force_integration_callback(Callable());
 		} else {
 			const Variant *vp[2] = { &v, &fi_callback->udata };
 
 			Callable::CallError ce;
 			int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2;
-			obj->call(fi_callback->method, vp, argc, ce);
+			Variant rv;
+			fi_callback->callable.call(vp, argc, rv, ce);
 		}
 	}
 }
@@ -725,16 +726,15 @@ bool Body3DSW::sleep_test(real_t p_step) {
 	}
 }
 
-void Body3DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) {
+void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
 	if (fi_callback) {
 		memdelete(fi_callback);
 		fi_callback = nullptr;
 	}
 
-	if (p_id.is_valid()) {
+	if (p_callable.get_object()) {
 		fi_callback = memnew(ForceIntegrationCallback);
-		fi_callback->id = p_id;
-		fi_callback->method = p_method;
+		fi_callback->callable = p_callable;
 		fi_callback->udata = p_udata;
 	}
 }
@@ -761,8 +761,6 @@ Body3DSW::Body3DSW() :
 	omit_force_integration = false;
 	//applied_torque=0;
 	island_step = 0;
-	island_next = nullptr;
-	island_list_next = nullptr;
 	first_time_kinematic = false;
 	first_integration = false;
 	_set_static(false);
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index e87ff2364b..9afb8cd56f 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -127,16 +127,13 @@ class Body3DSW : public CollisionObject3DSW {
 	int contact_count;
 
 	struct ForceIntegrationCallback {
-		ObjectID id;
-		StringName method;
+		Callable callable;
 		Variant udata;
 	};
 
 	ForceIntegrationCallback *fi_callback;
 
 	uint64_t island_step;
-	Body3DSW *island_next;
-	Body3DSW *island_list_next;
 
 	_FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area3DSW *p_area);
 
@@ -145,7 +142,7 @@ class Body3DSW : public CollisionObject3DSW {
 	friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose
 
 public:
-	void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
+	void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
 
 	void set_kinematic_margin(real_t p_margin);
 	_FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; }
@@ -189,12 +186,6 @@ public:
 	_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_ Body3DSW *get_island_next() const { return island_next; }
-	_FORCE_INLINE_ void set_island_next(Body3DSW *p_next) { island_next = p_next; }
-
-	_FORCE_INLINE_ Body3DSW *get_island_list_next() const { return island_list_next; }
-	_FORCE_INLINE_ void set_island_list_next(Body3DSW *p_next) { island_list_next = p_next; }
-
 	_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
 	_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); }
 	const Map<Constraint3DSW *, int> &get_constraint_map() const { return constraint_map; }
diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp
index 36114c0c91..28c854466f 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/body_pair_3d_sw.cpp
@@ -281,6 +281,8 @@ bool BodyPair3DSW::setup(real_t p_step) {
 
 	real_t inv_dt = 1.0 / p_step;
 
+	bool do_process = false;
+
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
 		c.active = false;
@@ -323,6 +325,7 @@ bool BodyPair3DSW::setup(real_t p_step) {
 		}
 
 		c.active = true;
+		do_process = true;
 
 		// Precompute normal mass, tangent mass, and bias.
 		Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal));
@@ -350,7 +353,7 @@ bool BodyPair3DSW::setup(real_t p_step) {
 		}
 	}
 
-	return true;
+	return do_process;
 }
 
 void BodyPair3DSW::solve(real_t p_step) {
@@ -594,6 +597,8 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 
 	real_t inv_dt = 1.0 / p_step;
 
+	bool do_process = false;
+
 	uint32_t contact_count = contacts.size();
 	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
 		Contact &c = contacts[contact_index];
@@ -614,6 +619,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 		}
 
 		c.active = true;
+		do_process = true;
 
 #ifdef DEBUG_ENABLED
 
@@ -645,7 +651,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 		c.depth = depth;
 
 		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
-		body->apply_impulse(c.rA + body->get_center_of_mass(), -j_vec);
+		body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass());
 		soft_body->apply_node_impulse(c.index_B, j_vec);
 		c.acc_bias_impulse = 0;
 		c.acc_bias_impulse_center_of_mass = 0;
@@ -661,7 +667,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 		}
 	}
 
-	return true;
+	return do_process;
 }
 
 void BodySoftBodyPair3DSW::solve(real_t p_step) {
@@ -691,7 +697,7 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-			body->apply_bias_impulse(c.rA + body->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step);
+			body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), MAX_BIAS_ROTATION / p_step);
 			soft_body->apply_node_bias_impulse(c.index_B, jb);
 
 			crbA = body->get_biased_angular_velocity().cross(c.rA);
@@ -706,8 +712,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 				Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
 
-				body->apply_bias_impulse(body->get_center_of_mass(), -jb_com, 0.0f);
-				soft_body->apply_node_bias_impulse(c.index_B, -jb_com);
+				body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f);
+				soft_body->apply_node_bias_impulse(c.index_B, jb_com);
 			}
 
 			c.active = true;
@@ -726,7 +732,7 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
 
-			body->apply_impulse(c.rA + body->get_center_of_mass(), -j);
+			body->apply_impulse(-j, c.rA + body->get_center_of_mass());
 			soft_body->apply_node_impulse(c.index_B, j);
 
 			c.active = true;
@@ -767,7 +773,7 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			jt = c.acc_tangent_impulse - jtOld;
 
-			body->apply_impulse(c.rA + body->get_center_of_mass(), -jt);
+			body->apply_impulse(-jt, c.rA + body->get_center_of_mass());
 			soft_body->apply_node_impulse(c.index_B, jt);
 
 			c.active = true;
diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h
index 2571335c43..16a31e167d 100644
--- a/servers/physics_3d/constraint_3d_sw.h
+++ b/servers/physics_3d/constraint_3d_sw.h
@@ -37,8 +37,6 @@ class Constraint3DSW {
 	Body3DSW **_body_ptr;
 	int _body_count;
 	uint64_t island_step;
-	Constraint3DSW *island_next;
-	Constraint3DSW *island_list_next;
 	int priority;
 	bool disabled_collisions_between_bodies;
 
@@ -60,12 +58,6 @@ public:
 	_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_ Constraint3DSW *get_island_next() const { return island_next; }
-	_FORCE_INLINE_ void set_island_next(Constraint3DSW *p_next) { island_next = p_next; }
-
-	_FORCE_INLINE_ Constraint3DSW *get_island_list_next() const { return island_list_next; }
-	_FORCE_INLINE_ void set_island_list_next(Constraint3DSW *p_next) { island_list_next = p_next; }
-
 	_FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; }
 	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
 
diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
index 9c4493f4a2..167f797bfe 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
@@ -109,6 +109,10 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans
 }
 
 bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
+	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+		return false;
+	}
+
 	m_appliedImpulse = real_t(0.);
 
 	//set bias, sign, clear accumulator
diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
index 13b389251f..a86e8b4e76 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
@@ -303,6 +303,10 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
 }
 
 bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
+	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+		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;
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
index 2b9f0038b4..90b82f4680 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
@@ -155,6 +155,10 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo
 }
 
 bool HingeJoint3DSW::setup(real_t p_step) {
+	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+		return false;
+	}
+
 	m_appliedImpulse = real_t(0.);
 
 	if (!m_angularOnly) {
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
index 9f708ce151..75d87992d1 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
@@ -50,6 +50,10 @@ subject to the following restrictions:
 #include "pin_joint_3d_sw.h"
 
 bool PinJoint3DSW::setup(real_t p_step) {
+	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+		return false;
+	}
+
 	m_appliedImpulse = real_t(0.);
 
 	Vector3 normal(0, 0, 0);
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp
index 0adc471797..2e1ee8e770 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp
@@ -127,6 +127,10 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &
 //-----------------------------------------------------------------------------
 
 bool SliderJoint3DSW::setup(real_t p_step) {
+	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+		return false;
+	}
+
 	//calculate transforms
 	m_calculatedTransformA = A->get_transform() * m_frameInA;
 	m_calculatedTransformB = B->get_transform() * m_frameInB;
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index 3d0063b0fa..c08e2b5794 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -857,10 +857,10 @@ int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const {
 	return body->get_max_contacts_reported();
 }
 
-void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) {
+void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
 	Body3DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
-	body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method, p_udata);
+	body->set_force_integration_callback(p_callable, p_udata);
 }
 
 void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) {
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h
index f92652bfad..0b42f1d605 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/servers/physics_3d/physics_server_3d_sw.h
@@ -241,7 +241,7 @@ public:
 	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_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) 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;
 
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h
index 49ae60db92..69d0fcf3ed 100644
--- a/servers/physics_3d/physics_server_3d_wrap_mt.h
+++ b/servers/physics_3d/physics_server_3d_wrap_mt.h
@@ -249,7 +249,7 @@ public:
 	FUNC2(body_set_omit_force_integration, RID, bool);
 	FUNC1RC(bool, body_is_omitting_force_integration, RID);
 
-	FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &);
+	FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &);
 
 	FUNC2(body_set_ray_pickable, RID, bool);
 
diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp
index 4c14cb3162..ccd37ca742 100644
--- a/servers/physics_3d/shape_3d_sw.cpp
+++ b/servers/physics_3d/shape_3d_sw.cpp
@@ -30,10 +30,28 @@
 
 #include "shape_3d_sw.h"
 
+#include "core/io/image.h"
 #include "core/math/geometry_3d.h"
 #include "core/math/quick_hull.h"
 #include "core/templates/sort_array.h"
 
+// HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape.
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+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.
+*/
+
 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002
 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998
 
@@ -1617,7 +1635,7 @@ ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() {
 
 /* HEIGHT MAP SHAPE */
 
-Vector<real_t> HeightMapShape3DSW::get_heights() const {
+Vector<float> HeightMapShape3DSW::get_heights() const {
 	return heights;
 }
 
@@ -1629,10 +1647,6 @@ int HeightMapShape3DSW::get_depth() const {
 	return depth;
 }
 
-real_t HeightMapShape3DSW::get_cell_size() const {
-	return cell_size;
-}
-
 void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 	//not very useful, but not very used either
 	p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max);
@@ -1643,7 +1657,198 @@ Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const {
 	return get_aabb().get_support(p_normal);
 }
 
+struct _HeightmapSegmentCullParams {
+	Vector3 from;
+	Vector3 to;
+	Vector3 dir;
+
+	Vector3 result;
+	Vector3 normal;
+
+	const HeightMapShape3DSW *heightmap = nullptr;
+	FaceShape3DSW *face = nullptr;
+};
+
+_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) {
+	Vector3 res;
+	Vector3 normal;
+	if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) {
+		p_params.result = res;
+		p_params.normal = normal;
+		return true;
+	}
+
+	return false;
+}
+
+_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, int p_x, int p_z) {
+	// First triangle.
+	p_params.heightmap->_get_point(p_x, p_z, p_params.face->vertex[0]);
+	p_params.heightmap->_get_point(p_x + 1, p_z, p_params.face->vertex[1]);
+	p_params.heightmap->_get_point(p_x, p_z + 1, p_params.face->vertex[2]);
+	p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
+	if (_heightmap_face_cull_segment(p_params)) {
+		return true;
+	}
+
+	// Second triangle.
+	p_params.face->vertex[0] = p_params.face->vertex[1];
+	p_params.heightmap->_get_point(p_x + 1, p_z + 1, p_params.face->vertex[1]);
+	p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
+	if (_heightmap_face_cull_segment(p_params)) {
+		return true;
+	}
+
+	return false;
+}
+
 bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const {
+	if (heights.is_empty()) {
+		return false;
+	}
+
+	Vector3 local_begin = p_begin + local_origin;
+	Vector3 local_end = p_end + local_origin;
+
+	FaceShape3DSW face;
+	face.backface_collision = false;
+
+	_HeightmapSegmentCullParams params;
+	params.from = p_begin;
+	params.to = p_end;
+	params.dir = (p_end - p_begin).normalized();
+	params.heightmap = this;
+	params.face = &face;
+
+	// Quantize the ray begin/end.
+	int begin_x = floor(local_begin.x);
+	int begin_z = floor(local_begin.z);
+	int end_x = floor(local_end.x);
+	int end_z = floor(local_end.z);
+
+	if ((begin_x == end_x) && (begin_z == end_z)) {
+		// Simple case for rays that don't traverse the grid horizontally.
+		// Just perform a test on the given cell.
+		int x = CLAMP(begin_x, 0, width - 2);
+		int z = CLAMP(begin_z, 0, depth - 2);
+		if (_heightmap_cell_cull_segment(params, x, z)) {
+			r_point = params.result;
+			r_normal = params.normal;
+			return true;
+		}
+	} else {
+		// Perform grid query from projected ray.
+		Vector2 ray_dir_proj(local_end.x - local_begin.x, local_end.z - local_begin.z);
+		real_t ray_dist_proj = ray_dir_proj.length();
+
+		if (ray_dist_proj < CMP_EPSILON) {
+			ray_dir_proj = Vector2();
+		} else {
+			ray_dir_proj /= ray_dist_proj;
+		}
+
+		const int x_step = (ray_dir_proj.x > CMP_EPSILON) ? 1 : ((ray_dir_proj.x < -CMP_EPSILON) ? -1 : 0);
+		const int z_step = (ray_dir_proj.y > CMP_EPSILON) ? 1 : ((ray_dir_proj.y < -CMP_EPSILON) ? -1 : 0);
+
+		const real_t infinite = 1e20;
+		const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_proj.x) : infinite;
+		const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_proj.y) : infinite;
+
+		real_t cross_x; // At which value of `param` we will cross a x-axis lane?
+		real_t cross_z; // At which value of `param` we will cross a z-axis lane?
+
+		// X initialization.
+		if (x_step != 0) {
+			if (x_step == 1) {
+				cross_x = (ceil(local_begin.x) - local_begin.x) * delta_x;
+			} else {
+				cross_x = (local_begin.x - floor(local_begin.x)) * delta_x;
+			}
+		} else {
+			cross_x = infinite; // Will never cross on X.
+		}
+
+		// Z initialization.
+		if (z_step != 0) {
+			if (z_step == 1) {
+				cross_z = (ceil(local_begin.z) - local_begin.z) * delta_z;
+			} else {
+				cross_z = (local_begin.z - floor(local_begin.z)) * delta_z;
+			}
+		} else {
+			cross_z = infinite; // Will never cross on Z.
+		}
+
+		int x = floor(local_begin.x);
+		int z = floor(local_begin.z);
+
+		// Workaround cases where the ray starts at an integer position.
+		if (Math::abs(cross_x) < CMP_EPSILON) {
+			cross_x += delta_x;
+			// If going backwards, we should ignore the position we would get by the above flooring,
+			// because the ray is not heading in that direction.
+			if (x_step == -1) {
+				x -= 1;
+			}
+		}
+
+		if (Math::abs(cross_z) < CMP_EPSILON) {
+			cross_z += delta_z;
+			if (z_step == -1) {
+				z -= 1;
+			}
+		}
+
+		// Start inside the grid.
+		int x_start = CLAMP(x, 0, width - 2);
+		int z_start = CLAMP(z, 0, depth - 2);
+
+		// Adjust initial cross values.
+		cross_x += delta_x * x_step * (x_start - x);
+		cross_z += delta_z * z_step * (z_start - z);
+
+		x = x_start;
+		z = z_start;
+
+		if (_heightmap_cell_cull_segment(params, x, z)) {
+			r_point = params.result;
+			r_normal = params.normal;
+			return true;
+		}
+
+		real_t dist = 0.0;
+		while (true) {
+			if (cross_x < cross_z) {
+				// X lane.
+				x += x_step;
+				// Assign before advancing the param,
+				// to be in sync with the initialization step.
+				dist = cross_x;
+				cross_x += delta_x;
+			} else {
+				// Z lane.
+				z += z_step;
+				dist = cross_z;
+				cross_z += delta_z;
+			}
+
+			// Stop when outside the grid.
+			if ((x < 0) || (z < 0) || (x >= width - 1) || (z >= depth - 1)) {
+				break;
+			}
+
+			if (_heightmap_cell_cull_segment(params, x, z)) {
+				r_point = params.result;
+				r_normal = params.normal;
+				return true;
+			}
+
+			if (dist > ray_dist_proj) {
+				break;
+			}
+		}
+	}
+
 	return false;
 }
 
@@ -1655,7 +1860,66 @@ Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
 	return Vector3();
 }
 
+void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const {
+	const AABB &aabb = get_aabb();
+
+	Vector3 pos_local = aabb.position + local_origin;
+
+	Vector3 clamped_point(p_point);
+	clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x);
+	clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y);
+	clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z);
+
+	r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5);
+	r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5);
+	r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5);
+}
+
 void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
+	if (heights.is_empty()) {
+		return;
+	}
+
+	AABB local_aabb = p_local_aabb;
+	local_aabb.position += local_origin;
+
+	// Quantize the aabb, and adjust the start/end ranges.
+	int aabb_min[3];
+	int aabb_max[3];
+	_get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]);
+	_get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]);
+
+	// Expand the min/max quantized values.
+	// This is to catch the case where the input aabb falls between grid points.
+	for (int i = 0; i < 3; ++i) {
+		aabb_min[i]--;
+		aabb_max[i]++;
+	}
+
+	int start_x = MAX(0, aabb_min[0]);
+	int end_x = MIN(width - 1, aabb_max[0]);
+	int start_z = MAX(0, aabb_min[2]);
+	int end_z = MIN(depth - 1, aabb_max[2]);
+
+	FaceShape3DSW face;
+	face.backface_collision = true;
+
+	for (int z = start_z; z < end_z; z++) {
+		for (int x = start_x; x < end_x; x++) {
+			// First triangle.
+			_get_point(x, z, face.vertex[0]);
+			_get_point(x + 1, z, face.vertex[1]);
+			_get_point(x, z + 1, face.vertex[2]);
+			face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal;
+			p_callback(p_userdata, &face);
+
+			// Second triangle.
+			face.vertex[0] = face.vertex[1];
+			_get_point(x + 1, z + 1, face.vertex[1]);
+			face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal;
+			p_callback(p_userdata, &face);
+		}
+	}
 }
 
 Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const {
@@ -1668,58 +1932,102 @@ Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const {
 			(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
 }
 
-void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) {
+void HeightMapShape3DSW::_setup(const Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
 	heights = p_heights;
 	width = p_width;
 	depth = p_depth;
-	cell_size = p_cell_size;
-
-	const real_t *r = heights.ptr();
 
+	// Initialize aabb.
 	AABB aabb;
+	aabb.position = Vector3(0.0, p_min_height, 0.0);
+	aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1);
 
-	for (int i = 0; i < depth; i++) {
-		for (int j = 0; j < width; j++) {
-			real_t h = r[i * width + j];
+	// Initialize origin as the aabb center.
+	local_origin = aabb.position + 0.5 * aabb.size;
+	local_origin.y = 0.0;
 
-			Vector3 pos(j * cell_size, h, i * cell_size);
-			if (i == 0 || j == 0) {
-				aabb.position = pos;
-			} else {
-				aabb.expand_to(pos);
-			}
-		}
-	}
+	aabb.position -= local_origin;
 
 	configure(aabb);
 }
 
 void HeightMapShape3DSW::set_data(const Variant &p_data) {
 	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
+
 	Dictionary d = p_data;
 	ERR_FAIL_COND(!d.has("width"));
 	ERR_FAIL_COND(!d.has("depth"));
-	ERR_FAIL_COND(!d.has("cell_size"));
 	ERR_FAIL_COND(!d.has("heights"));
 
 	int width = d["width"];
 	int depth = d["depth"];
-	real_t cell_size = d["cell_size"];
-	Vector<real_t> heights = d["heights"];
 
-	ERR_FAIL_COND(width <= 0);
-	ERR_FAIL_COND(depth <= 0);
-	ERR_FAIL_COND(cell_size <= CMP_EPSILON);
-	ERR_FAIL_COND(heights.size() != (width * depth));
-	_setup(heights, width, depth, cell_size);
+	ERR_FAIL_COND(width <= 0.0);
+	ERR_FAIL_COND(depth <= 0.0);
+
+	Variant heights_variant = d["heights"];
+	Vector<float> heights_buffer;
+	if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) {
+		// Ready-to-use heights can be passed.
+		heights_buffer = heights_variant;
+	} else if (heights_variant.get_type() == Variant::OBJECT) {
+		// If an image is passed, we have to convert it.
+		// This would be expensive to do with a script, so it's nice to have it here.
+		Ref<Image> image = heights_variant;
+		ERR_FAIL_COND(image.is_null());
+		ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF);
+
+		PackedByteArray im_data = image->get_data();
+		heights_buffer.resize(image->get_width() * image->get_height());
+
+		float *w = heights_buffer.ptrw();
+		float *rp = (float *)im_data.ptr();
+		for (int i = 0; i < heights_buffer.size(); ++i) {
+			w[i] = rp[i];
+		}
+	} else {
+		ERR_FAIL_MSG("Expected PackedFloat32Array or float Image.");
+	}
+
+	// Compute min and max heights or use precomputed values.
+	real_t min_height = 0.0;
+	real_t max_height = 0.0;
+	if (d.has("min_height") && d.has("max_height")) {
+		min_height = d["min_height"];
+		max_height = d["max_height"];
+	} else {
+		int heights_size = heights.size();
+		for (int i = 0; i < heights_size; ++i) {
+			float h = heights[i];
+			if (h < min_height) {
+				min_height = h;
+			} else if (h > max_height) {
+				max_height = h;
+			}
+		}
+	}
+
+	ERR_FAIL_COND(min_height > max_height);
+
+	ERR_FAIL_COND(heights_buffer.size() != (width * depth));
+
+	// If specified, min and max height will be used as precomputed values.
+	_setup(heights_buffer, width, depth, min_height, max_height);
 }
 
 Variant HeightMapShape3DSW::get_data() const {
-	ERR_FAIL_V(Variant());
+	Dictionary d;
+	d["width"] = width;
+	d["depth"] = depth;
+
+	const AABB &aabb = get_aabb();
+	d["min_height"] = aabb.position.y;
+	d["max_height"] = aabb.position.y + aabb.size.y;
+
+	d["heights"] = heights;
+
+	return d;
 }
 
 HeightMapShape3DSW::HeightMapShape3DSW() {
-	width = 0;
-	depth = 0;
-	cell_size = 0;
 }
diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h
index 988e76c699..4d2b6ffbed 100644
--- a/servers/physics_3d/shape_3d_sw.h
+++ b/servers/physics_3d/shape_3d_sw.h
@@ -81,7 +81,7 @@ public:
 
 	virtual PhysicsServer3D::ShapeType get_type() const = 0;
 
-	_FORCE_INLINE_ AABB get_aabb() const { return aabb; }
+	_FORCE_INLINE_ const AABB &get_aabb() const { return aabb; }
 	_FORCE_INLINE_ bool is_configured() const { return configured; }
 
 	virtual bool is_concave() const { return false; }
@@ -389,21 +389,29 @@ public:
 };
 
 struct HeightMapShape3DSW : public ConcaveShape3DSW {
-	Vector<real_t> heights;
-	int width;
-	int depth;
-	real_t cell_size;
+	Vector<float> heights;
+	int width = 0;
+	int depth = 0;
+	Vector3 local_origin;
 
-	//void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
-	//void _cull(int p_idx,_CullParams *p_params) const;
+	_FORCE_INLINE_ float _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 _setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size);
+	void _setup(const Vector<float> &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;
+	Vector<float> get_heights() const;
 	int get_width() const;
 	int get_depth() const;
-	real_t get_cell_size() const;
 
 	virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_HEIGHTMAP; }
 
diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp
index 2133a38670..06f3227eab 100644
--- a/servers/physics_3d/step_3d_sw.cpp
+++ b/servers/physics_3d/step_3d_sw.cpp
@@ -33,19 +33,23 @@
 
 #include "core/os/os.h"
 
-void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island) {
+#define BODY_ISLAND_COUNT_RESERVE 128
+#define BODY_ISLAND_SIZE_RESERVE 512
+#define ISLAND_COUNT_RESERVE 128
+#define ISLAND_SIZE_RESERVE 512
+
+void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
 	p_body->set_island_step(_step);
-	p_body->set_island_next(*p_island);
-	*p_island = p_body;
+	p_body_island.push_back(p_body);
 
-	for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
+	// Faster with reversed iterations.
+	for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().back(); E; E = E->prev()) {
 		Constraint3DSW *c = (Constraint3DSW *)E->key();
 		if (c->get_island_step() == _step) {
 			continue; //already processed
 		}
 		c->set_island_step(_step);
-		c->set_island_next(*p_constraint_island);
-		*p_constraint_island = c;
+		p_constraint_island.push_back(c);
 
 		for (int i = 0; i < c->get_body_count(); i++) {
 			if (i == E->get()) {
@@ -55,87 +59,79 @@ void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constrain
 			if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
 				continue; //no go
 			}
-			_populate_island(c->get_body_ptr()[i], p_island, p_constraint_island);
+			_populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island);
 		}
 	}
 }
 
-void Step3DSW::_setup_island(Constraint3DSW *p_island, real_t p_delta) {
-	Constraint3DSW *ci = p_island;
-	while (ci) {
-		ci->setup(p_delta);
-		//todo remove from island if process fails
-		ci = ci->get_island_next();
+void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta) {
+	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) {
+		Constraint3DSW *constraint = p_constraint_island[constraint_index];
+		if (p_constraint_island[constraint_index]->setup(p_delta)) {
+			// Keep this constraint for solving.
+			p_constraint_island[valid_constraint_count++] = constraint;
+		}
 	}
+	p_constraint_island.resize(valid_constraint_count);
 }
 
-void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta) {
-	int at_priority = 1;
+void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta) {
+	int current_priority = 1;
 
-	while (p_island) {
+	uint32_t constraint_count = p_constraint_island.size();
+	while (constraint_count > 0) {
 		for (int i = 0; i < p_iterations; i++) {
-			Constraint3DSW *ci = p_island;
-			while (ci) {
-				ci->solve(p_delta);
-				ci = ci->get_island_next();
+			// Go through all iterations.
+			for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+				p_constraint_island[constraint_index]->solve(p_delta);
 			}
 		}
 
-		at_priority++;
-
-		{
-			Constraint3DSW *ci = p_island;
-			Constraint3DSW *prev = nullptr;
-			while (ci) {
-				if (ci->get_priority() < at_priority) {
-					if (prev) {
-						prev->set_island_next(ci->get_island_next()); //remove
-					} else {
-						p_island = ci->get_island_next();
-					}
-				} else {
-					prev = ci;
-				}
-
-				ci = ci->get_island_next();
+		// 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) {
+			Constraint3DSW *constraint = p_constraint_island[constraint_index];
+			if (constraint->get_priority() >= current_priority) {
+				// Keep this constraint for the next iteration.
+				p_constraint_island[priority_constraint_count++] = constraint;
 			}
 		}
+		constraint_count = priority_constraint_count;
 	}
 }
 
-void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) {
+void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta) {
 	bool can_sleep = true;
 
-	Body3DSW *b = p_island;
-	while (b) {
-		if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-			b = b->get_island_next();
-			continue; //ignore for static
+	uint32_t body_count = p_body_island.size();
+	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
+		Body3DSW *body = p_body_island[body_index];
+
+		if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
+			continue; // Ignore for static.
 		}
 
-		if (!b->sleep_test(p_delta)) {
+		if (!body->sleep_test(p_delta)) {
 			can_sleep = false;
 		}
-
-		b = b->get_island_next();
 	}
 
-	//put all to sleep or wake up everyoen
+	// Put all to sleep or wake up everyone.
+	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
+		Body3DSW *body = p_body_island[body_index];
 
-	b = p_island;
-	while (b) {
-		if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-			b = b->get_island_next();
-			continue; //ignore for static
+		if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
+			continue; // Ignore for static.
 		}
 
-		bool active = b->is_active();
+		bool active = body->is_active();
 
 		if (active == can_sleep) {
-			b->set_active(!can_sleep);
+			body->set_active(!can_sleep);
 		}
-
-		b = b->get_island_next();
 	}
 }
 
@@ -181,33 +177,43 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	/* GENERATE CONSTRAINT ISLANDS */
 
-	Body3DSW *island_list = nullptr;
-	Constraint3DSW *constraint_island_list = nullptr;
 	b = body_list->first();
 
-	int island_count = 0;
+	uint32_t body_island_count = 0;
+	uint32_t island_count = 0;
 
 	while (b) {
 		Body3DSW *body = b->self();
 
 		if (body->get_island_step() != _step) {
-			Body3DSW *island = nullptr;
-			Constraint3DSW *constraint_island = nullptr;
-			_populate_island(body, &island, &constraint_island);
+			++body_island_count;
+			if (body_islands.size() < body_island_count) {
+				body_islands.resize(body_island_count);
+			}
+			LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1];
+			body_island.clear();
+			body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
 
-			island->set_island_list_next(island_list);
-			island_list = island;
+			++island_count;
+			if (constraint_islands.size() < island_count) {
+				constraint_islands.resize(island_count);
+			}
+			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+			constraint_island.reserve(ISLAND_SIZE_RESERVE);
 
-			if (constraint_island) {
-				constraint_island->set_island_list_next(constraint_island_list);
-				constraint_island_list = constraint_island;
-				island_count++;
+			_populate_island(body, body_island, constraint_island);
+
+			body_islands.push_back(body_island);
+
+			if (constraint_island.is_empty()) {
+				--island_count;
 			}
 		}
 		b = b->next();
 	}
 
-	p_space->set_island_count(island_count);
+	p_space->set_island_count((int)island_count);
 
 	const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list();
 
@@ -218,9 +224,13 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 				continue;
 			}
 			c->set_island_step(_step);
-			c->set_island_next(nullptr);
-			c->set_island_list_next(constraint_island_list);
-			constraint_island_list = c;
+			++island_count;
+			if (constraint_islands.size() < island_count) {
+				constraint_islands.resize(island_count);
+			}
+			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+			constraint_island.push_back(c);
 		}
 		p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
 	}
@@ -233,9 +243,13 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 				continue;
 			}
 			c->set_island_step(_step);
-			c->set_island_next(nullptr);
-			c->set_island_list_next(constraint_island_list);
-			constraint_island_list = c;
+			++island_count;
+			if (constraint_islands.size() < island_count) {
+				constraint_islands.resize(island_count);
+			}
+			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+			constraint_island.clear();
+			constraint_island.push_back(c);
 		}
 		sb = sb->next();
 	}
@@ -248,12 +262,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	/* SETUP CONSTRAINT ISLANDS */
 
-	{
-		Constraint3DSW *ci = constraint_island_list;
-		while (ci) {
-			_setup_island(ci, p_delta);
-			ci = ci->get_island_list_next();
-		}
+	for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
+		_setup_island(constraint_islands[island_index], p_delta);
 	}
 
 	{ //profile
@@ -264,13 +274,10 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	/* SOLVE CONSTRAINT ISLANDS */
 
-	{
-		Constraint3DSW *ci = constraint_island_list;
-		while (ci) {
-			//iterating each island separatedly improves cache efficiency
-			_solve_island(ci, p_iterations, p_delta);
-			ci = ci->get_island_list_next();
-		}
+	for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
+		// Warning: _solve_island modifies the constraint islands for optimization purpose,
+		// their content is not reliable after these calls and shouldn't be used anymore.
+		_solve_island(constraint_islands[island_index], p_iterations, p_delta);
 	}
 
 	{ //profile
@@ -290,12 +297,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	/* SLEEP / WAKE UP ISLANDS */
 
-	{
-		Body3DSW *bi = island_list;
-		while (bi) {
-			_check_suspend(bi, p_delta);
-			bi = bi->get_island_list_next();
-		}
+	for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
+		_check_suspend(body_islands[island_index], p_delta);
 	}
 
 	/* UPDATE SOFT BODY CONSTRAINTS */
@@ -319,4 +322,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 Step3DSW::Step3DSW() {
 	_step = 1;
+
+	body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
+	constraint_islands.reserve(ISLAND_COUNT_RESERVE);
 }
diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h
index 55c48ec0eb..f406c35c3a 100644
--- a/servers/physics_3d/step_3d_sw.h
+++ b/servers/physics_3d/step_3d_sw.h
@@ -33,13 +33,18 @@
 
 #include "space_3d_sw.h"
 
+#include "core/templates/local_vector.h"
+
 class Step3DSW {
 	uint64_t _step;
 
-	void _populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island);
-	void _setup_island(Constraint3DSW *p_island, real_t p_delta);
-	void _solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta);
-	void _check_suspend(Body3DSW *p_island, real_t p_delta);
+	LocalVector<LocalVector<Body3DSW *>> body_islands;
+	LocalVector<LocalVector<Constraint3DSW *>> constraint_islands;
+
+	void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island);
+	void _setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta);
+	void _solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta);
+	void _check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta);
 
 public:
 	void step(Space3DSW *p_space, real_t p_delta, int p_iterations);