something's weird
This commit is contained in:
parent
cc519efdb6
commit
60fbbf4637
1 changed files with 35 additions and 32 deletions
|
@ -18,9 +18,9 @@ pub struct CatControllerSettings {
|
||||||
impl Default for CatControllerSettings {
|
impl Default for CatControllerSettings {
|
||||||
fn default() -> Self {
|
fn default() -> Self {
|
||||||
Self {
|
Self {
|
||||||
kp: 30.0,
|
kp: 25.0,
|
||||||
kd: 7.0,
|
kd: 3.0,
|
||||||
ki: 1.0,
|
ki: 1.1,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -127,7 +127,7 @@ mod systems {
|
||||||
// show which is forward
|
// show which is forward
|
||||||
gizmos.arrow(
|
gizmos.arrow(
|
||||||
*xform.forward() + xform.translation,
|
*xform.forward() + xform.translation,
|
||||||
1.5 * *xform.forward() + xform.translation,
|
2.5 * *xform.forward() + xform.translation,
|
||||||
bevy::color::palettes::basic::PURPLE,
|
bevy::color::palettes::basic::PURPLE,
|
||||||
);
|
);
|
||||||
|
|
||||||
|
@ -239,7 +239,12 @@ mod systems {
|
||||||
|
|
||||||
pub(super) fn steering(
|
pub(super) fn steering(
|
||||||
mut bike_query: Query<
|
mut bike_query: Query<
|
||||||
(&Transform, &mut ExternalForce, RigidBodyQueryReadOnly),
|
(
|
||||||
|
&Transform,
|
||||||
|
&LinearVelocity,
|
||||||
|
&mut ExternalForce,
|
||||||
|
RigidBodyQueryReadOnly,
|
||||||
|
),
|
||||||
With<CyberBikeBody>,
|
With<CyberBikeBody>,
|
||||||
>,
|
>,
|
||||||
mut wheels: Query<(&mut WheelState, &WheelConfig, &CyberWheel)>,
|
mut wheels: Query<(&mut WheelState, &WheelConfig, &CyberWheel)>,
|
||||||
|
@ -247,16 +252,16 @@ mod systems {
|
||||||
input: Res<InputState>,
|
input: Res<InputState>,
|
||||||
mut gizmos: Gizmos,
|
mut gizmos: Gizmos,
|
||||||
) {
|
) {
|
||||||
let Ok((bike_xform, mut bike_force, bike_body)) = bike_query.get_single_mut() else {
|
let Ok((bike_xform, bike_vel, mut bike_force, bike_body)) = bike_query.get_single_mut()
|
||||||
|
else {
|
||||||
bevy::log::warn!("No entity for bike_query.");
|
bevy::log::warn!("No entity for bike_query.");
|
||||||
return;
|
return;
|
||||||
};
|
};
|
||||||
|
let bike_vel = bike_vel.0;
|
||||||
|
|
||||||
let dt = time.delta().as_secs_f32();
|
let dt = time.delta().as_secs_f32();
|
||||||
let max_thrust = 1000.0;
|
let max_thrust = 1000.0;
|
||||||
let thrust_force = input.throttle * max_thrust * dt;
|
let yaw_angle = -yaw_to_angle(input.yaw);
|
||||||
let max_yaw = 500.0;
|
|
||||||
let yaw_force = input.yaw * max_yaw * dt;
|
|
||||||
|
|
||||||
for (mut state, config, wheel) in wheels.iter_mut() {
|
for (mut state, config, wheel) in wheels.iter_mut() {
|
||||||
if let Some(contact_point) = state.contact_point {
|
if let Some(contact_point) = state.contact_point {
|
||||||
|
@ -264,39 +269,32 @@ mod systems {
|
||||||
let normal = state.contact_normal.unwrap().normalize();
|
let normal = state.contact_normal.unwrap().normalize();
|
||||||
let max_force_mag = state.force_normal * config.friction;
|
let max_force_mag = state.force_normal * config.friction;
|
||||||
|
|
||||||
let yaw_dir = match wheel {
|
let rot = Quat::from_axis_angle(normal, yaw_angle);
|
||||||
CyberWheel::Front => normal.cross(*bike_xform.back()).normalize_or_zero(),
|
let forward = normal.cross(*bike_xform.right());
|
||||||
CyberWheel::Rear => Vec3::ZERO,
|
let (thrust_dir, thrust_force) = match wheel {
|
||||||
|
CyberWheel::Rear => (forward, input.throttle * max_thrust * dt),
|
||||||
|
CyberWheel::Front => (rot * forward, 0.0),
|
||||||
};
|
};
|
||||||
|
|
||||||
let thrust_dir = normal.cross(*bike_xform.right());
|
|
||||||
|
|
||||||
let vel = bike_body.velocity_at_point(contact_point - bike_xform.translation);
|
|
||||||
|
|
||||||
let yaw = yaw_force * yaw_dir;
|
|
||||||
let thrust = thrust_force * thrust_dir;
|
let thrust = thrust_force * thrust_dir;
|
||||||
let yust = yaw + thrust;
|
|
||||||
|
|
||||||
bevy::log::debug!("yust: {yust:?}");
|
|
||||||
bevy::log::debug!("{:?}", bike_xform.left());
|
|
||||||
|
|
||||||
let left = bike_xform.forward().cross(normal);
|
|
||||||
|
|
||||||
let friction_dir = match wheel {
|
let friction_dir = match wheel {
|
||||||
CyberWheel::Front => {
|
CyberWheel::Front => normal.cross(thrust_dir),
|
||||||
if yaw_force + thrust_force > 0.1 {
|
CyberWheel::Rear => normal.cross(*bike_xform.forward()),
|
||||||
normal.cross(yust.normalize()).normalize()
|
|
||||||
} else {
|
|
||||||
left
|
|
||||||
}
|
|
||||||
}
|
|
||||||
CyberWheel::Rear => left,
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// bevy::log::debug!(
|
||||||
|
// "{wheel:?}, thrust_dir: {thrust_dir:?}, friction_dir: {friction_dir:?}, dot: {}",
|
||||||
|
// thrust_dir.dot(friction_dir)
|
||||||
|
// );
|
||||||
|
|
||||||
|
let vel = bike_body.velocity_at_point(contact_point - bike_xform.translation);
|
||||||
let friction_factor = -0.025 * vel.dot(friction_dir);
|
let friction_factor = -0.025 * vel.dot(friction_dir);
|
||||||
let friction = (friction_factor / dt) * friction_dir;
|
let friction = (friction_factor / dt) * friction_dir;
|
||||||
|
|
||||||
let mut force = yust + friction;
|
bevy::log::debug!("{wheel:?}: vel diff: {:?}", bike_vel - vel);
|
||||||
|
|
||||||
|
let mut force = thrust + friction;
|
||||||
force *= dt * 50.0;
|
force *= dt * 50.0;
|
||||||
let force_mag = force.length();
|
let force_mag = force.length();
|
||||||
if force_mag > max_force_mag {
|
if force_mag > max_force_mag {
|
||||||
|
@ -313,6 +311,11 @@ mod systems {
|
||||||
contact_point + friction,
|
contact_point + friction,
|
||||||
Color::linear_rgb(1., 1., 0.2),
|
Color::linear_rgb(1., 1., 0.2),
|
||||||
);
|
);
|
||||||
|
gizmos.arrow(
|
||||||
|
contact_point,
|
||||||
|
contact_point + thrust,
|
||||||
|
Color::linear_rgb(1., 0., 0.2),
|
||||||
|
);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in a new issue