1 files changed, 39 insertions, 18 deletions

diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h
index b05f65f268..061d66a085 100644
--- a/servers/physics_3d/shape_3d_sw.h
+++ b/servers/physics_3d/shape_3d_sw.h
@@ -32,6 +32,7 @@
 #define SHAPE_SW_H
 
 #include "core/math/geometry_3d.h"
+#include "core/templates/local_vector.h"
 #include "servers/physics_server_3d.h"
 
 class Shape3DSW;
@@ -47,8 +48,8 @@ public:
 class Shape3DSW {
 	RID self;
 	AABB aabb;
-	bool configured;
-	real_t custom_bias;
+	bool configured = false;
+	real_t custom_bias = 0.0;
 
 	Map<ShapeOwner3DSW *, int> owners;
 
@@ -94,7 +95,7 @@ public:
 	bool is_owner(ShapeOwner3DSW *p_owner) const;
 	const Map<ShapeOwner3DSW *, int> &get_owners() const;
 
-	Shape3DSW();
+	Shape3DSW() {}
 	virtual ~Shape3DSW();
 };
 
@@ -111,7 +112,7 @@ public:
 	ConcaveShape3DSW() {}
 };
 
-class PlaneShape3DSW : public Shape3DSW {
+class WorldBoundaryShape3DSW : public Shape3DSW {
 	Plane plane;
 
 	void _setup(const Plane &p_plane);
@@ -120,7 +121,7 @@ public:
 	Plane get_plane() const;
 
 	virtual real_t get_area() const override { return INFINITY; }
-	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_PLANE; }
+	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; }
@@ -133,12 +134,12 @@ public:
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	PlaneShape3DSW();
+	WorldBoundaryShape3DSW();
 };
 
 class SeparationRayShape3DSW : public Shape3DSW {
-	real_t length;
-	bool slide_on_slope;
+	real_t length = 1.0;
+	bool slide_on_slope = false;
 
 	void _setup(real_t p_length, bool p_slide_on_slope);
 
@@ -165,7 +166,7 @@ public:
 };
 
 class SphereShape3DSW : public Shape3DSW {
-	real_t radius;
+	real_t radius = 0.0;
 
 	void _setup(real_t p_radius);
 
@@ -217,8 +218,8 @@ public:
 };
 
 class CapsuleShape3DSW : public Shape3DSW {
-	real_t height;
-	real_t radius;
+	real_t height = 0.0;
+	real_t radius = 0.0;
 
 	void _setup(real_t p_height, real_t p_radius);
 
@@ -246,8 +247,8 @@ public:
 };
 
 class CylinderShape3DSW : public Shape3DSW {
-	real_t height;
-	real_t radius;
+	real_t height = 0.0;
+	real_t radius = 0.0;
 
 	void _setup(real_t p_height, real_t p_radius);
 
@@ -307,7 +308,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
 
 	struct Face {
 		Vector3 normal;
-		int indices[3];
+		int indices[3] = {};
 	};
 
 	Vector<Face> faces;
@@ -315,10 +316,10 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
 
 	struct BVH {
 		AABB aabb;
-		int left;
-		int right;
+		int left = 0;
+		int right = 0;
 
-		int face_index;
+		int face_index = 0;
 	};
 
 	Vector<BVH> bvh;
@@ -385,6 +386,21 @@ struct HeightMapShape3DSW : public ConcaveShape3DSW {
 	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];
 	}
@@ -397,6 +413,11 @@ struct HeightMapShape3DSW : public ConcaveShape3DSW {
 
 	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:
@@ -448,7 +469,7 @@ struct FaceShape3DSW : public Shape3DSW {
 };
 
 struct MotionShape3DSW : public Shape3DSW {
-	Shape3DSW *shape;
+	Shape3DSW *shape = nullptr;
 	Vector3 motion;
 
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; }