asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge pull request #48551 from nekomatata/concave-shape-optimization-3.x

Browse source 
[3.x] Optimize area detection and intersect_shape queries with concave shapes
This commit is contained in:
Camille Mohr-Daurat 2021-09-06 09:36:27 -07:00 committed by GitHub
commit 095dea7b71
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 57 additions and 36 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

18
servers/physics/collision_solver_sw.cpp
View file

@ -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) {

4
servers/physics/collision_solver_sw.h
View file

@ -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:

30
servers/physics/shape_sw.cpp
View file

@ -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;
}
}
}
}

14
servers/physics/shape_sw.h
View file

@ -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;

10
servers/physics_2d/collision_solver_2d_sw.cpp
View file

@ -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) {

2
servers/physics_2d/collision_solver_2d_sw.h
View file

@ -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);

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

9
servers/physics_2d/shape_2d_sw.h
View file

@ -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
};