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

Merge pull request #53453 from nekomatata/fix-rayshape-snap-3.x

Browse source
This commit is contained in:
Rémi Verschelde 2021-10-06 08:37:57 +02:00 committed by GitHub
commit abb1413e0f
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 40 additions and 35 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
servers/physics/collision_solver_sw.cpp
View file

@ -89,11 +89,11 @@ bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Trans
return found;
}
bool CollisionSolverSW::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) {
bool CollisionSolverSW::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, real_t p_margin) {
const RayShapeSW *ray = static_cast<const RayShapeSW *>(p_shape_A);
Vector3 from = p_transform_A.origin;
Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length();
Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin);
Vector3 support_A = to;
Transform ai = p_transform_B.affine_inverse();
@ -232,9 +232,9 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &
}
if (swap) {
return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B);
} else {
return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A);
}
} else if (concave_B) {

2
servers/physics/collision_solver_sw.h
View file

@ -40,7 +40,7 @@ public:
private:
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_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, real_t p_margin = 0);
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 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);

28
servers/physics/space_sw.cpp
View file

@ -593,7 +593,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
for (int i = 0; i < p_result_max; i++) {
//reset results
r_results[i].collision_depth = 0;
r_results[i].collision_depth = -1.0;
}
int rays_found = 0;
@ -668,18 +668,28 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
Vector3 a = sr[k * 2 + 0];
Vector3 b = sr[k * 2 + 1];
recover_motion += (b - a) / cbk.amount;
// Compute plane on b towards a.
Vector3 n = (a - b).normalized();
float d = n.dot(b);
// Compute depth on recovered motion.
float depth = n.dot(a + recover_motion) - d;
// Apply recovery without margin.
float separation_depth = depth - p_margin;
if (separation_depth > 0.0) {
// Only recover if there is penetration.
recover_motion -= n * separation_depth;
}
float depth = a.distance_to(b);
if (depth > result.collision_depth) {
result.collision_depth = depth;
result.collision_point = b;
result.collision_normal = (b - a).normalized();
result.collision_normal = -n;
result.collision_local_shape = j;
result.collider = col_obj->get_self();
result.collider_id = col_obj->get_instance_id();
result.collider_shape = shape_idx;
//result.collider_metadata = col_obj->get_shape_metadata(shape_idx);
if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) {
BodySW *body = (BodySW *)col_obj;
@ -704,14 +714,6 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
} while (recover_attempts);
}
//optimize results (remove non colliding)
for (int i = 0; i < rays_found; i++) {
if (r_results[i].collision_depth == 0) {
rays_found--;
SWAP(r_results[i], r_results[rays_found]);
}
}
r_recover_motion = body_transform.origin - p_transform.origin;
return rays_found;
}

8
servers/physics_2d/collision_solver_2d_sw.cpp
View file

@ -73,14 +73,14 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Tr
return found;
}
bool CollisionSolver2DSW::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) {
bool CollisionSolver2DSW::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, real_t p_margin) {
const RayShape2DSW *ray = static_cast<const RayShape2DSW *>(p_shape_A);
if (p_shape_B->get_type() == Physics2DServer::SHAPE_RAY) {
return false;
}
Vector2 from = p_transform_A.get_origin();
Vector2 to = from + p_transform_A[1] * ray->get_length();
Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin);
if (p_motion_A != Vector2()) {
//not the best but should be enough
Vector2 normal = (to - from).normalized();
@ -226,9 +226,9 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p
}
if (swap) {
return solve_raycast(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis);
return solve_raycast(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis, p_margin_B);
} else {
return solve_raycast(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis);
return solve_raycast(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis, p_margin_A);
}
} else if (concave_B) {

2
servers/physics_2d/collision_solver_2d_sw.h
View file

@ -41,7 +41,7 @@ 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 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);
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, real_t p_margin = 0);
public:
static bool solve(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, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);

27
servers/physics_2d/space_2d_sw.cpp
View file

@ -565,7 +565,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
for (int i = 0; i < p_result_max; i++) {
//reset results
r_results[i].collision_depth = 0;
r_results[i].collision_depth = -1.0;
}
int rays_found = 0;
@ -665,13 +665,24 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
Vector2 a = sr[k * 2 + 0];
Vector2 b = sr[k * 2 + 1];
recover_motion += (b - a) / cbk.amount;
// Compute plane on b towards a.
Vector2 n = (a - b).normalized();
float d = n.dot(b);
// Compute depth on recovered motion.
float depth = n.dot(a + recover_motion) - d;
// Apply recovery without margin.
float separation_depth = depth - p_margin;
if (separation_depth > 0.0) {
// Only recover if there is penetration.
recover_motion -= n * separation_depth;
}
float depth = a.distance_to(b);
if (depth > result.collision_depth) {
result.collision_depth = depth;
result.collision_point = b;
result.collision_normal = (b - a).normalized();
result.collision_normal = -n;
result.collision_local_shape = j;
result.collider_shape = shape_idx;
result.collider = col_obj->get_self();
@ -701,14 +712,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
} while (recover_attempts);
}
//optimize results (remove non colliding)
for (int i = 0; i < rays_found; i++) {
if (r_results[i].collision_depth == 0) {
rays_found--;
SWAP(r_results[i], r_results[rays_found]);
}
}
r_recover_motion = body_transform.elements[2] - p_transform.elements[2];
return rays_found;
}