From ff9b1671b15ef0a94fff057d9841864891f41129 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sat, 15 Jun 2024 15:43:48 -0700 Subject: [PATCH] checkpoint; wheel_forces skeleton fn --- src/action/systems.rs | 129 +++++++++++++++++------------------------- src/bike/wheels.rs | 11 +--- 2 files changed, 55 insertions(+), 85 deletions(-) diff --git a/src/action/systems.rs b/src/action/systems.rs index c2baef3..5c5827f 100644 --- a/src/action/systems.rs +++ b/src/action/systems.rs @@ -129,52 +129,16 @@ pub(super) fn falling_cat( } } -/// Apply forces to the cyberbike as a result of input. -pub(super) fn input_forces( - settings: Res, - input: Res, - mut braking_query: Query<&mut MultibodyJoint, (Without, With)>, - mut body_query: Query<(&Transform, &mut ExternalForce), With>, - mut steering_query: Query<&mut MultibodyJoint, With>, -) { - let (xform, mut forces) = body_query.single_mut(); - - // thrust - let thrust = xform.forward() * input.throttle * settings.accel; - let point = xform.translation + *xform.back(); - let force = ExternalForce::at_point(thrust, point, xform.translation); - *forces += force; - - // brake + thrust - // for mut motor in braking_query.iter_mut() { - // let factor = if input.brake { - // 500.00 - // } else { - // input.throttle * settings.accel - // }; - // let speed = if input.brake { 0.0 } else { -70.0 }; - // motor.data = (*motor - // .data - // .as_revolute_mut() - // .unwrap() - // .set_motor_max_force(factor) - // .set_motor_velocity(speed, factor)) - // .into(); - // } - - // steering - let angle = yaw_to_angle(input.yaw); - let mut steering = steering_query.single_mut(); - steering.data = (*steering - .data - .as_revolute_mut() - .unwrap() - .set_motor_position(-angle, 100.0, 0.5)) - .into(); -} - pub(super) fn wheel_forces( - mut wheels_query: Query<(&Transform, &Velocity, &mut PreviousVelocity), With>, + mut wheels_query: Query< + ( + &Transform, + &Velocity, + &mut PreviousVelocity, + Option<&CyberSteering>, + ), + With, + >, mut body_query: Query< ( &Transform, @@ -184,47 +148,58 @@ pub(super) fn wheel_forces( ), With, >, - config: Res, + wheel_config: Res, + rapier_config: Res, context: Res, + settings: Res, + input: Res, ) { let (body_xform, body_vel, mut body_pvel, body_mass) = body_query.single_mut(); - for (xform, vel, mut pvel) in wheels_query.iter_mut() { - // + let gvec = rapier_config.gravity; + + // body kinematics + let body_lin_accel = body_vel.linvel - body_pvel.0.linvel; + let body_ang_accel = body_vel.angvel - body_pvel.0.angvel; + let mut body_forward = body_vel + .linvel + .try_normalize() + .unwrap_or_else(|| body_xform.forward().normalize()); + // we're just projecting onto the ground, so nuke the Y. + body_forward.y = 0.0; + let body_half_mass = body_mass.mass * 0.5; + let g = gvec * settings.gravity * body_half_mass; + + // inputs + let steering_angle = yaw_to_angle(input.yaw); + let throttle = input.throttle * settings.accel; + + for (xform, vel, mut pvel, steering) in wheels_query.iter_mut() { + if let Some((_, projected)) = + context.project_point(xform.translation, true, QueryFilter::only_fixed()) + { + let normal = -(projected.point - xform.translation); + let len = normal.length(); + let normal = normal.normalize(); + if len < wheel_config.radius { + // we're in contact + + // do gravity's share + + // do linear acceleration's share + + // do friction's share + + // do thrust's share + + // do steering's share + } + } } body_pvel.0 = *body_vel; } -/// Don't let the wheels get stuck underneat the planet -pub(super) fn surface_fix( - mut wheel_query: Query<(Entity, &Transform, &mut CollisionGroups), With>, - span_query: Query<&Transform, With>, - config: Res, - context: Res, -) { - let mut wheels = Vec::new(); - for xform in span_query.iter() { - wheels.push(xform); - } - let span = (wheels[1].translation - wheels[0].translation).normalize(); - - for (entity, xform, mut cgroups) in wheel_query.iter_mut() { - //let ray_dir = xform.translation.normalize(); - let ray_dir = Vec3::NEG_Y; //xform.right().cross(span).normalize(); - if let Some(hit) = context.cast_ray_and_get_normal( - xform.translation, - ray_dir, - config.radius, - false, - QueryFilter::only_fixed(), - ) { - //cgroups.memberships = Group::NONE; - //cgroups.filters = Group::NONE; - } - } -} - /// General velocity-based drag-force calculation; does not take orientation /// into account. pub(super) fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With>) { diff --git a/src/bike/wheels.rs b/src/bike/wheels.rs index d39758e..4b41b57 100644 --- a/src/bike/wheels.rs +++ b/src/bike/wheels.rs @@ -87,8 +87,7 @@ pub(crate) fn spawn_wheels( .id(); let axel_parent_entity = if let Some(steering) = steering { - let neck_builder = - RevoluteJointBuilder::new(rake_vec).local_anchor1(Vec3::new(0.0, 0.0, 0.1)); // this adds another 0.1m of trail + let neck_builder = FixedJointBuilder::new().local_anchor1(Vec3::new(0.0, 0.0, 0.1)); // this adds another 0.1m of trail let neck_joint = MultibodyJoint::new(fork_rb_entity, neck_builder); let neck = commands .spawn(RigidBody::Dynamic) @@ -101,17 +100,13 @@ pub(crate) fn spawn_wheels( fork_rb_entity }; - let axel_builder = FixedJointBuilder::new(); //RevoluteJointBuilder::new(Vec3::X); + let axel_builder = FixedJointBuilder::new(); let axel_joint = MultibodyJoint::new(axel_parent_entity, axel_builder); - let wheel_damping = Damping { - //linear_damping: 0.8, - ..Default::default() - }; commands.spawn(pbr_bundle).insert(( collider, mass_props, - wheel_damping, + //wheel_damping, ccd, not_sleeping, axel_joint,