maxmustermann/godot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Merge pull request #48550 from nekomatata/concave-shape-optimization

Browse source 
Optimize area detection and intersect_shape queries with concave shapes
This commit is contained in:
Camille Mohr-Daurat 2021-08-27 10:55:18 -07:00 committed by GitHub
parent 33773a24dc cc43c2ea16
commit 569d87878e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 66 additions and 41 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

10
servers/physics_2d/collision_solver_2d_sw.cpp
View file

@ -149,20 +149,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 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {

2
servers/physics_2d/collision_solver_2d_sw.h
View file

@ -39,7 +39,7 @@ public:
private:
static bool solve_static_world_margin(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 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
static bool solve_separation_ray(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 *r_sep_axis = nullptr, real_t p_margin = 0);

6
servers/physics_2d/shape_2d_sw.cpp
View file

@ -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).orthogonal().normalized());
p_callback(p_userdata, &ss);
if (p_callback(p_userdata, &ss)) {
return;
}
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {

8
servers/physics_2d/shape_2d_sw.h
View file

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

30
servers/physics_3d/collision_solver_3d_sw.cpp
View file

@ -178,17 +178,17 @@ bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *
transform_B.origin = query_cinfo.node_transform.xform(node_position);
query_cinfo.contact_info.node_index = p_node_index;
solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info);
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
// Continue with the query.
return false;
// Stop at first collision if contacts are not needed.
return (collided && !query_cinfo.contact_info.result_callback);
}
void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) {
bool CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) {
_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
query_cinfo.shape_A = p_convex;
@ -210,9 +210,14 @@ void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW
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 CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
@ -286,17 +291,20 @@ struct _ConcaveCollisionInfo {
Vector3 close_A, close_B;
};
void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) {
bool CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *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 CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *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) {
@ -413,19 +421,18 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
}
}
void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) {
bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *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;
@ -433,6 +440,7 @@ void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW
}
cinfo.collisions++;
return false;
}
bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {

6
servers/physics_3d/collision_solver_3d_sw.h
View file

@ -40,13 +40,13 @@ public:
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, void *p_userdata);
static void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex);
static void concave_callback(void *p_userdata, Shape3DSW *p_convex);
static bool soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex);
static bool concave_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *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 Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *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 void concave_distance_callback(void *p_userdata, Shape3DSW *p_convex);
static bool concave_distance_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
public:

31
servers/physics_3d/shape_3d_sw.cpp
View file

@ -1382,11 +1382,11 @@ Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) co
return Vector3();
}
void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const {
bool ConcavePolygonShape3DSW::_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) {
@ -1396,20 +1396,27 @@ void ConcavePolygonShape3DSW::_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 ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
// make matrix local to concave
if (faces.size() == 0) {
return;
@ -1875,7 +1882,7 @@ void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, i
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 {
void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
if (heights.is_empty()) {
return;
}
@ -1911,13 +1918,17 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, voi
_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);
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[2], face.vertex[1]).normal;
p_callback(p_userdata, &face);
if (p_callback(p_userdata, &face)) {
return;
}
}
}
}

14
servers/physics_3d/shape_3d_sw.h
View file

@ -101,10 +101,12 @@ public:
class ConcaveShape3DSW : public Shape3DSW {
public:
virtual bool is_concave() const override { return true; }
typedef void (*Callback)(void *p_userdata, Shape3DSW *p_convex);
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 void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0;
// Returns true to stop the query.
typedef bool (*QueryCallback)(void *p_userdata, Shape3DSW *p_convex);
virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
ConcaveShape3DSW() {}
};
@ -323,7 +325,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
struct _CullParams {
AABB aabb;
Callback callback = nullptr;
QueryCallback callback = nullptr;
void *userdata = nullptr;
const Face *faces = nullptr;
const Vector3 *vertices = nullptr;
@ -349,7 +351,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
bool backface_collision = false;
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);
@ -367,7 +369,7 @@ public:
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, Callback p_callback, void *p_userdata) const override;
virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override;
virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
@ -410,7 +412,7 @@ public:
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, Callback p_callback, void *p_userdata) const override;
virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override;
virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;