diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index cb3dcc96dee..2f42e9a52b3 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/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) { diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h index b02556d7f8f..690e2f3616c 100644 --- a/servers/physics/collision_solver_sw.h +++ 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: diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp index d5119f285e2..6ee68504dc1 100644 --- a/servers/physics/shape_sw.cpp +++ b/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; + } } } } diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h index ca22d0da145..579db6422d6 100644 --- a/servers/physics/shape_sw.h +++ b/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; diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index fe8ca1dfe2d..df3e5f66650 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/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) { diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index 4f12ca9e882..96a94329528 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ 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); diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp index 90893f04d79..3c547b342e9 100644 --- a/servers/physics_2d/shape_2d_sw.cpp +++ b/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 { diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index c228f2f1933..7b2a0328ad4 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -472,10 +472,11 @@ public: class ConcaveShape2DSW : public Shape2DSW { public: - virtual bool is_concave() const { return true; } - typedef void (*Callback)(void *p_userdata, Shape2DSW *p_convex); + // Returns true to stop the query. + 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 };