Fix positioning due to scaling in hierarchies. (#646)

Author: ThierryBerger

CHANGELOG.md

@@ -6,6 +6,10 @@
 
 - Added a serialization `serialization2` example for `bevy_rapier2d`.
 
+## Fixed
+
+- Fix position being incorrect when a rigidbody bevy entity has a scaled parent. [#646](https://github.com/dimforge/bevy_rapier/pull/646)
+
 ## v0.29.0 (18 February 2025)
 
 ### Added

src/plugin/plugin.rs

@@ -640,4 +640,63 @@ mod test {
             }
         }
     }
+
+    #[test]
+    fn parent_child() {
+        return main();
+
+        use bevy::prelude::*;
+
+        fn main() {
+            let mut app = App::new();
+            app.add_plugins((
+                TransformPlugin,
+                TimePlugin,
+                RapierPhysicsPlugin::<NoUserData>::default().in_fixed_schedule(),
+            ));
+            run_test(&mut app);
+        }
+
+        fn run_test(app: &mut App) {
+            app.insert_resource(TimeUpdateStrategy::ManualDuration(
+                std::time::Duration::from_secs_f32(1f32 / 60f32),
+            ));
+            app.add_systems(Startup, init_rapier_configuration);
+            app.add_systems(Startup, setup_physics);
+
+            app.finish();
+            for _ in 0..100 {
+                app.update();
+            }
+            let context = app
+                .world_mut()
+                .query::<RapierContext>()
+                .get_single(&app.world())
+                .unwrap();
+
+            println!("{:#?}", &context.rigidbody_set.bodies);
+        }
+
+        pub fn init_rapier_configuration(
+            mut config: Query<&mut RapierConfiguration, With<DefaultRapierContext>>,
+        ) {
+            let mut config = config.single_mut();
+            *config = RapierConfiguration {
+                force_update_from_transform_changes: true,
+                ..RapierConfiguration::new(1f32)
+            };
+        }
+
+        pub fn setup_physics(mut commands: Commands) {
+            let parent = commands
+                .spawn(Transform::from_scale(Vec3::splat(5f32)))
+                .id();
+            let mut entity = commands.spawn((
+                Collider::ball(1f32),
+                Transform::from_translation(Vec3::new(200f32, 100f32, 3f32)),
+                RigidBody::Fixed,
+            ));
+            entity.set_parent(parent);
+        }
+    }
 }

src/plugin/systems/mod.rs

@@ -239,10 +239,23 @@ pub mod tests {
             let child_handle = rigidbody_set.entity2body[&child];
             let child_body = rigidbody_set.bodies.get(child_handle).unwrap();
             let body_transform = utils::iso_to_transform(child_body.position());
-            assert_eq!(
-                GlobalTransform::from(body_transform),
-                *child_transform,
-                "Collider transform should have have global rotation and translation"
+
+            fn transforms_approx_equal(
+                a: &GlobalTransform,
+                b: &GlobalTransform,
+                epsilon: f32,
+            ) -> bool {
+                a.translation().abs_diff_eq(b.translation(), epsilon)
+                    && a.scale().abs_diff_eq(b.scale(), epsilon)
+                    && a.rotation().abs_diff_eq(b.rotation(), epsilon)
+            }
+            assert!(
+                transforms_approx_equal(
+                    &GlobalTransform::from(body_transform),
+                    child_transform,
+                    1.0e-5,
+                ),
+                "Collider transforms should have have equal global rotation and translation"
             );
         }
     }

src/plugin/systems/rigid_body.rs

@@ -470,7 +470,7 @@ pub fn writeback_rigid_bodies(
                     // We need to compute the new local transform such that:
                     // curr_parent_global_transform * new_transform = interpolated_pos
                     // new_transform = curr_parent_global_transform.inverse() * interpolated_pos
-                    let (_, inverse_parent_rotation, inverse_parent_translation) =
+                    let (inverse_parent_scale, inverse_parent_rotation, inverse_parent_translation) =
                         parent_global_transform
                             .affine()
                             .inverse()
@@ -479,6 +479,7 @@ pub fn writeback_rigid_bodies(
 
                     #[allow(unused_mut)] // mut is needed in 2D but not in 3D.
                     let mut new_translation = inverse_parent_rotation
+                        * inverse_parent_scale
                         * interpolated_pos.translation
                         + inverse_parent_translation;