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

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 18:31:41 -07:00
parent 48d7eff3e3
commit ea0015a8a6
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
};