Optimize area detection and intersect_shape queries with concave shapes

Whenever contact points are not needed, collision checks with concave
shapes (triangle mesh and heightmap) stop at the first colliding
triangle.

servers/physics/collision_solver_sw.cpp

@@ -138,17 +138,20 @@ struct _ConcaveCollisionInfo {
 	Vector3 close_A, close_B;
 };
 
-void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
+bool CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
 	_ConcaveCollisionInfo &cinfo = *(_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;
+		return false;
 	}
 
 	cinfo.collided = true;
 	cinfo.collisions++;
+
+	// Stop at first collision if contacts are not needed.
+	return !cinfo.result_callback;
 }
 
 bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) {
@@ -250,19 +253,18 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &
 	}
 }
 
-void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) {
+bool CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) {
 	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
 	cinfo.aabb_tests++;
-	if (cinfo.collided) {
-		return;
-	}
 
 	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) {
-		return;
+		// 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;
@@ -270,6 +272,8 @@ void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_c
 	}
 
 	cinfo.collisions++;
+
+	return false;
 }
 
 bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {

servers/physics/collision_solver_sw.h

@@ -38,11 +38,11 @@ public:
 	typedef void (*CallbackResult)(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
 
 private:
-	static void concave_callback(void *p_userdata, ShapeSW *p_convex);
+	static bool concave_callback(void *p_userdata, ShapeSW *p_convex);
 	static bool solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
 	static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
 	static bool solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &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 void concave_distance_callback(void *p_userdata, ShapeSW *p_convex);
+	static bool concave_distance_callback(void *p_userdata, ShapeSW *p_convex);
 	static bool solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
 
 public:

servers/physics/shape_sw.cpp

@@ -1383,11 +1383,11 @@ Vector3 ConcavePolygonShapeSW::get_closest_point_to(const Vector3 &p_point) cons
 	return Vector3();
 }
 
-void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const {
+bool ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const {
 	const BVH *bvh = &p_params->bvh[p_idx];
 
 	if (!p_params->aabb.intersects(bvh->aabb)) {
-		return;
+		return false;
 	}
 
 	if (bvh->face_index >= 0) {
@@ -1397,20 +1397,28 @@ void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const {
 		face->vertex[0] = p_params->vertices[f->indices[0]];
 		face->vertex[1] = p_params->vertices[f->indices[1]];
 		face->vertex[2] = p_params->vertices[f->indices[2]];
-		p_params->callback(p_params->userdata, face);
+		if (p_params->callback(p_params->userdata, face)) {
+			return true;
+		}
 
 	} else {
 		if (bvh->left >= 0) {
-			_cull(bvh->left, p_params);
+			if (_cull(bvh->left, p_params)) {
+				return true;
+			}
 		}
 
 		if (bvh->right >= 0) {
-			_cull(bvh->right, p_params);
+			if (_cull(bvh->right, p_params)) {
+				return true;
+			}
 		}
 	}
+
+	return false;
 }
 
-void ConcavePolygonShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
+void ConcavePolygonShapeSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
 	// make matrix local to concave
 	if (faces.size() == 0) {
 		return;
@@ -1873,7 +1881,7 @@ void HeightMapShapeSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int
 	r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5);
 }
 
-void HeightMapShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
+void HeightMapShapeSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
 	if (heights.empty()) {
 		return;
 	}
@@ -1908,13 +1916,17 @@ void HeightMapShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void
 			_get_point(x + 1, z, face.vertex[1]);
 			_get_point(x, z + 1, face.vertex[2]);
 			face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
-			p_callback(p_userdata, &face);
+			if (p_callback(p_userdata, &face)) {
+				return;
+			}
 
 			// 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[1], face.vertex[2]).normal;
-			p_callback(p_userdata, &face);
+			if (p_callback(p_userdata, &face)) {
+				return;
+			}
 		}
 	}
 }

servers/physics/shape_sw.h

@@ -112,11 +112,13 @@ public:
 
 class ConcaveShapeSW : public ShapeSW {
 public:
+	// Returns true to stop the query.
+	typedef bool (*QueryCallback)(void *p_userdata, ShapeSW *p_convex);
+
 	virtual bool is_concave() const { return true; }
-	typedef void (*Callback)(void *p_userdata, ShapeSW *p_convex);
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; }
 
-	virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
 
 	ConcaveShapeSW() {}
 };
@@ -335,7 +337,7 @@ struct ConcavePolygonShapeSW : public ConcaveShapeSW {
 
 	struct _CullParams {
 		AABB aabb;
-		Callback callback;
+		QueryCallback callback;
 		void *userdata;
 		const Face *faces;
 		const Vector3 *vertices;
@@ -358,7 +360,7 @@ struct ConcavePolygonShapeSW : public ConcaveShapeSW {
 	};
 
 	void _cull_segment(int p_idx, _SegmentCullParams *p_params) const;
-	void _cull(int p_idx, _CullParams *p_params) const;
+	bool _cull(int p_idx, _CullParams *p_params) const;
 
 	void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx);
 
@@ -376,7 +378,7 @@ public:
 	virtual bool intersect_point(const Vector3 &p_point) const;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
-	virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -421,7 +423,7 @@ public:
 	virtual bool intersect_point(const Vector3 &p_point) const;
 
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
-	virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 

servers/physics_2d/collision_solver_2d_sw.cpp

@@ -133,20 +133,20 @@ struct _ConcaveCollisionInfo2D {
 	Vector2 *sep_axis;
 };
 
-void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) {
+bool CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) {
 	_ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata);
 	cinfo.aabb_tests++;
-	if (!cinfo.result_callback && cinfo.collided) {
-		return; //already collided and no contacts requested, don't test anymore
-	}
 
 	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B);
 	if (!collided) {
-		return;
+		return false;
 	}
 
 	cinfo.collided = true;
 	cinfo.collisions++;
+
+	// Stop at first collision if contacts are not needed.
+	return !cinfo.result_callback;
 }
 
 bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) {

servers/physics_2d/collision_solver_2d_sw.h

@@ -39,7 +39,7 @@ public:
 
 private:
 	static bool solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
-	static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
+	static bool concave_callback(void *p_userdata, Shape2DSW *p_convex);
 	static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
 	static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr);
 

servers/physics_2d/shape_2d_sw.cpp

@@ -921,7 +921,7 @@ Variant ConcavePolygonShape2DSW::get_data() const {
 	return rsegments;
 }
 
-void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const {
+void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
 	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
 
 	enum {
@@ -969,7 +969,9 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac
 
 						SegmentShape2DSW ss(a, b, (b - a).tangent().normalized());
 
-						p_callback(p_userdata, &ss);
+						if (p_callback(p_userdata, &ss)) {
+							return;
+						}
 						stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
 
 					} else {

servers/physics_2d/shape_2d_sw.h

@@ -472,10 +472,11 @@ public:
 
 class ConcaveShape2DSW : public Shape2DSW {
 public:
-	virtual bool is_concave() const { return true; }
+	// Returns true to stop the query.
-	typedef void (*Callback)(void *p_userdata, Shape2DSW *p_convex);
+	typedef bool (*QueryCallback)(void *p_userdata, Shape2DSW *p_convex);
 
-	virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const = 0;
+	virtual bool is_concave() const { return true; }
+	virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
 };
 
 class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
@@ -525,7 +526,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const;
+	virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const;
 
 	DEFAULT_PROJECT_RANGE_CAST
 };