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

Merge pull request #71707 from peastman/scale

Browse source 
Collision detection supports uniform scaling
This commit is contained in:
Rémi Verschelde 2023-01-21 16:53:00 +01:00
commit 06464cd15e
No known key found for this signature in database
GPG key ID: C3336907360768E1
1 changed files with 23 additions and 21 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

44
servers/physics_3d/godot_collision_solver_3d_sat.cpp
View file

@ -827,9 +827,9 @@ static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D
// Perform an analytic sphere collision between the two spheres // Perform an analytic sphere collision between the two spheres
analytic_sphere_collision<withMargin>( analytic_sphere_collision<withMargin>(
p_transform_a.origin, p_transform_a.origin,
sphere_A->get_radius(), sphere_A->get_radius() * p_transform_a.basis[0].length(),
p_transform_b.origin, p_transform_b.origin,
sphere_B->get_radius(), sphere_B->get_radius() * p_transform_b.basis[0].length(),
p_collector, p_collector,
p_margin_a, p_margin_a,
p_margin_b); p_margin_b);
@ -842,7 +842,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
// Find the point on the box nearest to the center of the sphere. // Find the point on the box nearest to the center of the sphere.
Vector3 center = p_transform_b.xform_inv(p_transform_a.origin); Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin);
Vector3 extents = box_B->get_half_extents(); Vector3 extents = box_B->get_half_extents();
Vector3 nearest(MIN(MAX(center.x, -extents.x), extents.x), Vector3 nearest(MIN(MAX(center.x, -extents.x), extents.x),
MIN(MAX(center.y, -extents.y), extents.y), MIN(MAX(center.y, -extents.y), extents.y),
@ -853,7 +853,8 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
Vector3 delta = nearest - p_transform_a.origin; Vector3 delta = nearest - p_transform_a.origin;
real_t length = delta.length(); real_t length = delta.length();
if (length > sphere_A->get_radius() + p_margin_a + p_margin_b) { real_t radius = sphere_A->get_radius() * p_transform_a.basis[0].length();
if (length > radius + p_margin_a + p_margin_b) {
return; return;
} }
p_collector->collided = true; p_collector->collided = true;
@ -867,7 +868,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
} else { } else {
axis = delta / length; axis = delta / length;
} }
Vector3 point_a = p_transform_a.origin + (sphere_A->get_radius() + p_margin_a) * axis; Vector3 point_a = p_transform_a.origin + (radius + p_margin_a) * axis;
Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest); Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
p_collector->call(point_a, point_b, axis); p_collector->call(point_a, point_b, axis);
} }
@ -877,11 +878,12 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D
const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a); const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b); const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
real_t capsule_B_radius = capsule_B->get_radius(); real_t scale_A = p_transform_a.basis[0].length();
real_t scale_B = p_transform_b.basis[0].length();
// Construct the capsule segment (ball-center to ball-center) // Construct the capsule segment (ball-center to ball-center)
Vector3 capsule_segment[2]; Vector3 capsule_segment[2];
Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B_radius); Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
capsule_segment[0] = p_transform_b.origin + capsule_axis; capsule_segment[0] = p_transform_b.origin + capsule_axis;
capsule_segment[1] = p_transform_b.origin - capsule_axis; capsule_segment[1] = p_transform_b.origin - capsule_axis;
@ -891,9 +893,9 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D
// Perform an analytic sphere collision between the sphere and the sphere-collider in the capsule // Perform an analytic sphere collision between the sphere and the sphere-collider in the capsule
analytic_sphere_collision<withMargin>( analytic_sphere_collision<withMargin>(
p_transform_a.origin, p_transform_a.origin,
sphere_A->get_radius(), sphere_A->get_radius() * scale_A,
capsule_closest, capsule_closest,
capsule_B_radius, capsule_B->get_radius() * scale_B,
p_collector, p_collector,
p_margin_a, p_margin_a,
p_margin_b); p_margin_b);
@ -903,12 +905,12 @@ template <bool withMargin>
static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radius_b, real_t p_height_b, const Transform3D &p_transform_a, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radius_b, real_t p_height_b, const Transform3D &p_transform_a, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
// Find the point on the cylinder nearest to the center of the sphere. // Find the point on the cylinder nearest to the center of the sphere.
Vector3 center = p_transform_b.xform_inv(p_transform_a.origin); Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin);
Vector3 nearest = center; Vector3 nearest = center;
real_t radius = p_radius_b; real_t scale_A = p_transform_a.basis[0].length();
real_t r = Math::sqrt(center.x * center.x + center.z * center.z); real_t r = Math::sqrt(center.x * center.x + center.z * center.z);
if (r > radius) { if (r > p_radius_b) {
real_t scale = radius / r; real_t scale = p_radius_b / r;
nearest.x *= scale; nearest.x *= scale;
nearest.z *= scale; nearest.z *= scale;
} }
@ -920,7 +922,7 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu
Vector3 delta = nearest - p_transform_a.origin; Vector3 delta = nearest - p_transform_a.origin;
real_t length = delta.length(); real_t length = delta.length();
if (length > p_radius_a + p_margin_a + p_margin_b) { if (length > p_radius_a * scale_A + p_margin_a + p_margin_b) {
return; return;
} }
p_collector->collided = true; p_collector->collided = true;
@ -934,7 +936,7 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu
} else { } else {
axis = delta / length; axis = delta / length;
} }
Vector3 point_a = p_transform_a.origin + (p_radius_a + p_margin_a) * axis; Vector3 point_a = p_transform_a.origin + (p_radius_a * scale_A + p_margin_a) * axis;
Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest); Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
p_collector->call(point_a, point_b, axis); p_collector->call(point_a, point_b, axis);
} }
@ -1632,14 +1634,14 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3
const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a); const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b); const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
real_t capsule_A_radius = capsule_A->get_radius(); real_t scale_A = p_transform_a.basis[0].length();
real_t capsule_B_radius = capsule_B->get_radius(); real_t scale_B = p_transform_b.basis[0].length();
// Get the closest points between the capsule segments // Get the closest points between the capsule segments
Vector3 capsule_A_closest; Vector3 capsule_A_closest;
Vector3 capsule_B_closest; Vector3 capsule_B_closest;
Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A_radius); Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B_radius); Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
Geometry3D::get_closest_points_between_segments( Geometry3D::get_closest_points_between_segments(
p_transform_a.origin + capsule_A_axis, p_transform_a.origin + capsule_A_axis,
p_transform_a.origin - capsule_A_axis, p_transform_a.origin - capsule_A_axis,
@ -1651,9 +1653,9 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3
// Perform the analytic collision between the two closest capsule spheres // Perform the analytic collision between the two closest capsule spheres
analytic_sphere_collision<withMargin>( analytic_sphere_collision<withMargin>(
capsule_A_closest, capsule_A_closest,
capsule_A_radius, capsule_A->get_radius() * scale_A,
capsule_B_closest, capsule_B_closest,
capsule_B_radius, capsule_B->get_radius() * scale_B,
p_collector, p_collector,
p_margin_a, p_margin_a,
p_margin_b); p_margin_b);