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

Optimize area detection and intersect_shape queries with concave shapes

Browse source 
Whenever contact points are not needed, collision checks with concave
shapes (triangle mesh and heightmap) stop at the first colliding
triangle.
This commit is contained in:
PouleyKetchoupp 2021-05-07 19:16:04 -07:00
parent 90a35dac48
commit cc43c2ea16
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;