normalize rear offset

src/bike.rs

@@ -13,7 +13,9 @@ fn spawn_bike(
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
 ) {
-    let xform = Transform::from_xyz(4.0, 4.0, 0.0);
+    let pos = Vec3::new(0.0, 4.0, 0.0);
+    let xform = Transform::from_translation(pos).with_rotation(Quat::from_rotation_z(0.0));
+
     let body_collider =
         Collider::capsule_endpoints(0.5, Vec3::new(0.0, 0.0, -0.65), Vec3::new(0.0, 0.0, 0.8));
 
@@ -66,6 +68,73 @@ pub struct Rearing;
 #[derive(Component)]
 pub struct RearHub;
 
+fn helper(
+    commands: &mut Commands,
+    meshes: &mut ResMut<Assets<Mesh>>,
+    materials: &mut ResMut<Assets<StandardMaterial>>,
+    position: Vec3,
+) -> Entity {
+    let mut tire_mesh: Mesh = Torus::new(0.30, 0.40).into();
+    let tire_verts = tire_mesh
+        .attribute(Mesh::ATTRIBUTE_POSITION)
+        .unwrap()
+        .as_float3()
+        .unwrap()
+        .iter()
+        .map(|v| {
+            //
+            let v = Vec3::from_array(*v);
+            let m = Mat3::from_rotation_z(90.0f32.to_radians());
+            let p = m.mul_vec3(v);
+            p.to_array()
+        })
+        .collect::<Vec<[f32; 3]>>();
+    tire_mesh.remove_attribute(Mesh::ATTRIBUTE_POSITION);
+    tire_mesh.insert_attribute(Mesh::ATTRIBUTE_POSITION, tire_verts);
+    let wheel_material = StandardMaterial {
+        base_color: Color::srgb(0.01, 0.01, 0.01),
+        alpha_mode: AlphaMode::Opaque,
+        perceptual_roughness: 0.5,
+        ..Default::default()
+    };
+    let collider = Collider::convex_decomposition_from_mesh(&tire_mesh).unwrap();
+    let mut tire = None;
+    let tref = &mut tire;
+
+    let xform = Transform::from_translation(position);
+
+    let hub = commands
+        .spawn((
+            SpatialBundle::from_transform(xform),
+            RigidBody::Dynamic,
+            MassPropertiesBundle::new_computed(&Collider::sphere(0.1), 0.01),
+        ))
+        .with_children(move |parent| {
+            let mesh: Mesh = Sphere::new(0.1).into();
+            parent.spawn(PbrBundle {
+                mesh: meshes.add(mesh),
+                material: materials.add(wheel_material.clone()),
+                ..Default::default()
+            });
+            let tire_id = parent
+                .spawn((
+                    CyberWheel,
+                    RigidBody::Dynamic,
+                    collider.clone(),
+                    ColliderDensity(0.05),
+                    CollisionLayers::from_bits(2, 2),
+                    ExternalTorque::ZERO.with_persistence(false),
+                ))
+                .id();
+            let _ = tref.insert(tire_id);
+        })
+        .id();
+
+    // connect hubs and tires to make wheels
+    commands.spawn(RevoluteJoint::new(hub, tire.unwrap()).with_aligned_axis(Vec3::X));
+    hub
+}
+
 fn spawn_wheels(
     mut commands: Commands,
     mut meshes: ResMut<Assets<Mesh>>,
@@ -73,97 +142,23 @@ fn spawn_wheels(
     xform: Transform,
     body: Entity,
 ) {
-    let rake_vec = Vec3::new(0.0, -1.0, 0.57).normalize(); // about 30 degrees
-    let front_pos = xform.translation + *xform.forward() + rake_vec;
+    let rake_vec = Vec3::new(0.0, -1.0, -0.57).normalize(); // about 30 degrees
+    let front_pos = xform.translation + rake_vec;
 
-    let wheel_mesh: Mesh = Torus::new(0.30, 0.40).into();
-    let wheel_material = StandardMaterial {
-        base_color: Color::srgb(0.01, 0.01, 0.01),
-        alpha_mode: AlphaMode::Opaque,
-        perceptual_roughness: 0.5,
-        ..Default::default()
-    };
-    let collider = Collider::convex_decomposition_from_mesh(&wheel_mesh).unwrap();
-
-    let front_hub = commands
-        .spawn((
-            FrontHub,
-            SpatialBundle::from_transform(Transform::from_translation(front_pos)),
-            RigidBody::Dynamic,
-            MassPropertiesBundle::new_computed(&Collider::sphere(0.01), 1.0),
-        ))
-        .id();
-
-    let mut tire_rot = Transform::default();
-    tire_rot.rotate_z(FRAC_PI_2);
-
-    let front_tire = commands
-        .spawn((
-            SpatialBundle::from_transform(tire_rot),
-            CyberWheel,
-            RigidBody::Dynamic,
-            collider.clone(),
-            ColliderDensity(0.05),
-            CollisionLayers::from_bits(2, 2),
-            ExternalTorque::ZERO.with_persistence(false),
-        ))
-        .with_children(|b| {
-            b.spawn(PbrBundle {
-                mesh: meshes.add(wheel_mesh.clone()),
-                material: materials.add(wheel_material.clone()),
-                // transform: Transform::from_translation(front_pos)
-                //     .with_rotation(Quat::from_rotation_z(FRAC_PI_2)),
-                ..Default::default()
-            });
-        })
-        .id();
-
-    // connect hubs and tires to make wheels
-    commands.spawn(RevoluteJoint::new(front_hub, front_tire).with_aligned_axis(Vec3::X));
-
-    // suspension joints connect the hubs and the body
+    let front_hub = helper(&mut commands, &mut meshes, &mut materials, front_pos);
+    commands.entity(front_hub).insert(FrontHub);
     commands.spawn((
         Steering,
         FixedJoint::new(front_hub, body).with_local_anchor_2(*xform.forward() + rake_vec),
     ));
 
-    /*
-    let rear_pos = *xform.back() + xform.translation + rake_vec.y;
-    let rear_hub = commands
-        .spawn((
-            RearHub,
-            SpatialBundle::from_transform(Transform::from_translation(front_pos)),
-            RigidBody::Dynamic,
-            MassPropertiesBundle::new_computed(&Collider::sphere(0.01), 1.0),
-        ))
-        .id();
-
-    let rear_tire = commands
-        .spawn((
-            CyberWheel,
-            RigidBody::Dynamic,
-            collider,
-            ColliderDensity(0.05),
-            CollisionLayers::from_bits(2, 2),
-            PbrBundle {
-                mesh: meshes.add(wheel_mesh),
-                material: materials.add(wheel_material),
-                transform: Transform::from_translation(rear_pos)
-                    .with_rotation(Quat::from_rotation_z(FRAC_PI_2)),
-                ..Default::default()
-            },
-            ExternalTorque::ZERO.with_persistence(false),
-        ))
-        .id();
-
-    commands.spawn(RevoluteJoint::new(rear_hub,
-    rear_tire).with_aligned_axis(Vec3::X));
-
+    let rear_pos = xform.translation + Vec3::new(0.0, -1.0, 0.57).normalize();
+    let rear_hub = helper(&mut commands, &mut meshes, &mut materials, rear_pos);
+    commands.entity(rear_hub).insert(RearHub);
     commands.spawn((
         Rearing,
-        FixedJoint::new(rear_hub, body).with_local_anchor_2(*xform.back() + xform.translation),
+        FixedJoint::new(rear_hub, body).with_local_anchor_2(*xform.back() + rake_vec.y),
     ));
-    */
 }
 
 pub struct CyberBikePlugin;

src/physics.rs

@@ -106,8 +106,8 @@ mod systems {
         let v2_r = v_squared / radius;
         let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
 
-        if tan_theta.is_finite() && !tan_theta.is_subnormal() {
-            lean.lean = tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
+        if tan_theta.is_normal() {
+            lean.lean = -tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
         } else {
             lean.lean = 0.0;
         }
@@ -121,10 +121,24 @@ mod systems {
         mut gizmos: Gizmos,
     ) {
         let (xform, mut torque, mut control_vars) = bike_query.single_mut();
-        let world_up = xform.translation.normalize();
+        let world_up = Vec3::Y; //xform.translation.normalize();
         let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
         let target_up = rotate_point(&world_up, &rot).normalize();
 
+        // show which is up
+        gizmos.arrow(
+            xform.translation,
+            xform.translation + *xform.up(),
+            bevy::color::palettes::basic::YELLOW,
+        );
+
+        // show which is forward
+        gizmos.arrow(
+            *xform.forward() + xform.translation,
+            1.5 * *xform.forward() + xform.translation,
+            bevy::color::palettes::basic::PURPLE,
+        );
+
         let bike_right = xform.right();
 
         let roll_error = bike_right.dot(target_up);