Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix Rayshape not working properly in move_and_slide and move_and_slide_with_snap #45005

Closed
wants to merge 1 commit into from
Closed

Fix Rayshape not working properly in move_and_slide and move_and_slide_with_snap #45005

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions servers/physics_2d/collision_solver_2d_sw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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() == PhysicsServer2D::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();
Expand Down Expand Up @@ -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) {
Expand Down
2 changes: 1 addition & 1 deletion servers/physics_2d/collision_solver_2d_sw.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ class CollisionSolver2DSW {
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 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);
Expand Down
27 changes: 15 additions & 12 deletions servers/physics_2d/space_2d_sw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -543,7 +543,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;
Expand All @@ -561,6 +561,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t

do {
Vector2 recover_motion;
int recover_count = 0;

bool collided = false;

Expand All @@ -579,6 +580,8 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t

Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);

Vector2 ray_normal = -body_shape_xform[1];

for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
Expand Down Expand Up @@ -638,17 +641,23 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
if (ray_index != -1) {
PhysicsServer2D::SeparationResult &result = r_results[ray_index];

recover_count += cbk.amount;
for (int k = 0; k < cbk.amount; k++) {
Vector2 a = sr[k * 2 + 0];
Vector2 b = sr[k * 2 + 1];
Vector2 separation = (b - a);

recover_motion += (b - a) / cbk.amount;
// Apply recovery without margin.
float depth = separation.length();
float separation_depth = depth - p_margin;
if (separation_depth > 0.0) {
recover_motion += separation * (separation_depth / 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 = ray_normal;
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Always using raycast normal as a result, to make sure it only does separation, as described in this article:
https://godotengine.org/article/godot-31-will-get-many-improvements-kinematicbody

result.collision_local_shape = j;
result.collider_shape = shape_idx;
result.collider = col_obj->get_self();
Expand All @@ -671,21 +680,15 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
break;
}

recover_motion /= recover_count;

body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion;

recover_attempts--;
} while (recover_attempts);
}

//optimize results (remove non colliding)
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This part is not needed anymore, since we're considering hits at depth 0 as a collision now, in order to stabilize floor tests.

As a side note, this block was not working correctly, as it would skip one element after each swap.
Proper implementation would have been:

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]);
		i--; // because we need to check the element we just moved
	}
}

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;
}
Expand Down
8 changes: 4 additions & 4 deletions servers/physics_3d/collision_solver_3d_sw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,11 +72,11 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T
return found;
}

bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
const RayShape3DSW *ray = static_cast<const RayShape3DSW *>(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();
Expand Down Expand Up @@ -212,9 +212,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
}

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) {
Expand Down
2 changes: 1 addition & 1 deletion servers/physics_3d/collision_solver_3d_sw.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class CollisionSolver3DSW {
private:
static void concave_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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 Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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, Shape3DSW *p_convex);
static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
Expand Down
27 changes: 15 additions & 12 deletions servers/physics_3d/space_3d_sw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -574,7 +574,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra

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;
Expand All @@ -592,6 +592,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra

do {
Vector3 recover_motion;
int recover_count = 0;

bool collided = false;

Expand All @@ -610,6 +611,8 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra

Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);

Vector3 ray_normal = -body_shape_xform.basis.get_axis(2);

for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
Expand Down Expand Up @@ -645,17 +648,23 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra
if (ray_index != -1) {
PhysicsServer3D::SeparationResult &result = r_results[ray_index];

recover_count += cbk.amount;
for (int k = 0; k < cbk.amount; k++) {
Vector3 a = sr[k * 2 + 0];
Vector3 b = sr[k * 2 + 1];
Vector3 separation = (b - a);

recover_motion += (b - a) / cbk.amount;
// Apply recovery without margin.
float depth = separation.length();
float separation_depth = depth - p_margin;
if (separation_depth > 0.0) {
recover_motion += separation * (separation_depth / 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 = ray_normal;
result.collision_local_shape = j;
result.collider = col_obj->get_self();
result.collider_id = col_obj->get_instance_id();
Expand All @@ -679,21 +688,15 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra
break;
}

recover_motion /= recover_count;

body_transform.origin += recover_motion;
body_aabb.position += recover_motion;

recover_attempts--;
} 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;
}
Expand Down