diff options
Diffstat (limited to 'servers/physics_3d/godot_collision_solver_3d_sat.cpp')
| -rw-r--r-- | servers/physics_3d/godot_collision_solver_3d_sat.cpp | 44 |
1 files changed, 23 insertions, 21 deletions
diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp index 66d1811abb..2cb29b3dd0 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp +++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp @@ -827,9 +827,9 @@ static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D // Perform an analytic sphere collision between the two spheres analytic_sphere_collision<withMargin>( p_transform_a.origin, - sphere_A->get_radius(), + sphere_A->get_radius() * p_transform_a.basis[0].length(), p_transform_b.origin, - sphere_B->get_radius(), + sphere_B->get_radius() * p_transform_b.basis[0].length(), p_collector, p_margin_a, 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. - 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 nearest(MIN(MAX(center.x, -extents.x), extents.x), 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; 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; } p_collector->collided = true; @@ -867,7 +868,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_ } else { 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); 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 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) 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[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 analytic_sphere_collision<withMargin>( p_transform_a.origin, - sphere_A->get_radius(), + sphere_A->get_radius() * scale_A, capsule_closest, - capsule_B_radius, + capsule_B->get_radius() * scale_B, p_collector, p_margin_a, 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) { // 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; - 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); - if (r > radius) { - real_t scale = radius / r; + if (r > p_radius_b) { + real_t scale = p_radius_b / r; nearest.x *= 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; 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; } p_collector->collided = true; @@ -934,7 +936,7 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu } else { 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); 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_B = static_cast<const GodotCapsuleShape3D *>(p_b); - real_t capsule_A_radius = capsule_A->get_radius(); - 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(); // Get the closest points between the capsule segments Vector3 capsule_A_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_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B_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->get_radius()); Geometry3D::get_closest_points_between_segments( 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 analytic_sphere_collision<withMargin>( capsule_A_closest, - capsule_A_radius, + capsule_A->get_radius() * scale_A, capsule_B_closest, - capsule_B_radius, + capsule_B->get_radius() * scale_B, p_collector, p_margin_a, p_margin_b); |