Skip to content

Commit

Permalink
Optimize area detection and intersect_shape queries with concave shapes
Browse files Browse the repository at this point in the history 
Whenever contact points are not needed, collision checks with concave
shapes (triangle mesh and heightmap) stop at the first colliding
triangle.
  • Loading branch information
@pouleyKetchoupp
pouleyKetchoupp committed May 8, 2021
1 parent 48d7eff commit ea0015a
Show file tree
Hide file tree
Showing 8 changed files with 57 additions and 36 deletions.
18 changes: 11 additions & 7 deletions servers/physics/collision_solver_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -250,26 +253,27 @@ 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;
cinfo.tested = true;
}

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) {
Expand Down
4 changes: 2 additions & 2 deletions servers/physics/collision_solver_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,11 @@ class CollisionSolverSW {
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:
Expand Down
30 changes: 21 additions & 9 deletions servers/physics/shape_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
}
}
}
}
Expand Down
14 changes: 8 additions & 6 deletions servers/physics/shape_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,11 +112,13 @@ class ShapeSW : public RID_Data {

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() {}
};
Expand Down Expand Up @@ -335,7 +337,7 @@ struct ConcavePolygonShapeSW : public ConcaveShapeSW {

struct _CullParams {
AABB aabb;
Callback callback;
QueryCallback callback;
void *userdata;
const Face *faces;
const Vector3 *vertices;
Expand All @@ -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);

Expand All @@ -376,7 +378,7 @@ struct ConcavePolygonShapeSW : public ConcaveShapeSW {
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;

Expand Down Expand Up @@ -421,7 +423,7 @@ struct HeightMapShapeSW : public ConcaveShapeSW {
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;

Expand Down
10 changes: 5 additions & 5 deletions servers/physics_2d/collision_solver_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
2 changes: 1 addition & 1 deletion servers/physics_2d/collision_solver_2d_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ class CollisionSolver2DSW {

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

Expand Down
6 changes: 4 additions & 2 deletions servers/physics_2d/shape_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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 {
Expand Down
9 changes: 5 additions & 4 deletions servers/physics_2d/shape_2d_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -472,10 +472,11 @@ class ConvexPolygonShape2DSW : public Shape2DSW {

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 {
Expand Down Expand Up @@ -525,7 +526,7 @@ class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
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
};
Expand Down

0 comments on commit ea0015a

Please sign in to comment.