use lgwks_deps::libm;
use lgwks_std::id::Uuid;
use super::clock::Clock;
use super::graph::MotionGraph;
use super::node::accumulator::{AccumulatorKind, AccumulatorStore};
use super::node::spring::{SpringConfig, integrate_spring};
use super::node::{MotionNode, NodeKind};
use super::scrub::ScrubSourceMap;
pub type TargetHandle = Uuid;
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub struct BindingWrite {
node_id: TargetHandle,
value: BindingValue,
}
impl BindingWrite {
#[must_use]
pub const fn new(node_id: TargetHandle, value: BindingValue) -> Self {
Self { node_id, value }
}
#[must_use]
pub const fn node_id(&self) -> TargetHandle {
self.node_id
}
#[must_use]
pub const fn value(&self) -> &BindingValue {
&self.value
}
}
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub enum BindingValue {
Scalar(f32),
}
impl BindingValue {
#[must_use]
pub const fn as_scalar(&self) -> Option<f32> {
match *self {
Self::Scalar(value) => Some(value),
}
}
}
#[derive(Clone, Debug)]
#[non_exhaustive]
pub struct Frame {
t_wall_ms: f64,
bindings: Vec<BindingWrite>,
}
impl Frame {
#[must_use]
pub const fn t_wall_ms(&self) -> f64 {
self.t_wall_ms
}
#[must_use]
pub fn bindings(&self) -> &[BindingWrite] {
&self.bindings
}
}
#[must_use]
pub fn frame(
graph: &MotionGraph,
clock: &Clock,
accumulators: &mut AccumulatorStore,
scrubs: &dyn ScrubSourceMap,
) -> Frame {
let t_wall_ms = clock.wall_clock_ms();
let mut bindings = Vec::with_capacity(graph.node_count());
for node in graph.nodes() {
bindings.push(evaluate_node(node, t_wall_ms, accumulators, scrubs));
}
Frame {
t_wall_ms,
bindings,
}
}
fn evaluate_node(
node: &MotionNode,
t_wall_ms: f64,
accumulators: &mut AccumulatorStore,
scrubs: &dyn ScrubSourceMap,
) -> BindingWrite {
let value = match *node.kind() {
NodeKind::Tween(ref tween) => {
BindingValue::Scalar(tween.value_at_wall_ms(narrow_f64_to_f32(t_wall_ms)))
}
NodeKind::Accumulator(ref accumulator) => {
evaluate_accumulator(node.node_id(), accumulator, t_wall_ms, accumulators, scrubs)
}
};
BindingWrite::new(node.node_id(), value)
}
fn evaluate_accumulator(
node_id: Uuid,
accumulator: &super::node::accumulator::AccumulatorComponent,
t_wall_ms: f64,
accumulators: &mut AccumulatorStore,
scrubs: &dyn ScrubSourceMap,
) -> BindingValue {
match accumulator.kind() {
AccumulatorKind::Spring => evaluate_spring(node_id, accumulator, t_wall_ms, accumulators),
AccumulatorKind::Scrub => evaluate_scrub(node_id, accumulator, accumulators, scrubs),
AccumulatorKind::Drag => evaluate_drag(node_id, accumulators),
}
}
const FIRST_FRAME_DT_SECONDS: f64 = 0.0166;
const SPRING_EXPLODE_DT_SECONDS: f64 = 0.250;
const MAX_INTEGRATION_DT_SECONDS: f64 = 0.064;
const SPRING_TARGET: f32 = 1.0;
const MAX_SPRING_SUBSTEPS: u16 = 64;
const MAX_STEP_RATE_PRODUCT: f32 = 1.0;
fn substep_count(config: &SpringConfig, dt_seconds: f32) -> u16 {
let stiffness = libm::sqrtf(config.tension() / config.mass());
let damping = config.friction() / config.mass();
let demand = dt_seconds * stiffness.max(damping) / MAX_STEP_RATE_PRODUCT;
let mut steps: u16 = 1;
while steps < MAX_SPRING_SUBSTEPS && f32::from(steps) < demand {
steps = steps.saturating_add(1);
}
steps
}
fn frame_dt_seconds(last_t_wall_ms: Option<f64>, t_wall_ms: f64) -> f64 {
match last_t_wall_ms {
Some(last) => (t_wall_ms - last) / 1000.0,
None => FIRST_FRAME_DT_SECONDS,
}
}
fn evaluate_spring(
node_id: Uuid,
accumulator: &super::node::accumulator::AccumulatorComponent,
t_wall_ms: f64,
accumulators: &mut AccumulatorStore,
) -> BindingValue {
let state = accumulators.get_or_default(node_id);
let Some(config) = accumulator.spring_config() else {
return BindingValue::Scalar(state.position());
};
if !t_wall_ms.is_finite() {
return BindingValue::Scalar(state.position());
}
let raw_dt = frame_dt_seconds(state.last_t_wall_ms(), t_wall_ms);
if raw_dt > SPRING_EXPLODE_DT_SECONDS {
state.set_position(SPRING_TARGET);
state.set_velocity(0.0);
state.set_last_t_wall_ms(t_wall_ms);
return BindingValue::Scalar(SPRING_TARGET);
}
let dt_seconds = narrow_f64_to_f32(raw_dt.clamp(0.0, MAX_INTEGRATION_DT_SECONDS));
let steps = substep_count(config, dt_seconds);
let step_seconds = dt_seconds / f32::from(steps);
let (mut new_position, mut new_velocity) = (state.position(), state.velocity());
for _ in 0..steps {
(new_position, new_velocity) = integrate_spring(
new_position,
new_velocity,
SPRING_TARGET,
config,
step_seconds,
);
}
if !new_position.is_finite() || !new_velocity.is_finite() {
state.set_position(SPRING_TARGET);
state.set_velocity(0.0);
state.set_last_t_wall_ms(t_wall_ms);
return BindingValue::Scalar(SPRING_TARGET);
}
state.set_position(new_position);
state.set_velocity(new_velocity);
state.set_last_t_wall_ms(t_wall_ms);
BindingValue::Scalar(new_position)
}
fn evaluate_scrub(
node_id: Uuid,
accumulator: &super::node::accumulator::AccumulatorComponent,
accumulators: &mut AccumulatorStore,
scrubs: &dyn ScrubSourceMap,
) -> BindingValue {
let scrub_value = match accumulator.scrub_source_id().and_then(|id| scrubs.get(id)) {
Some(source) if source.value().is_nan() => 0.0,
Some(source) => source.value(),
None => 0.0,
};
let state = accumulators.get_or_default(node_id);
state.set_last_scrub_value(scrub_value);
state.set_position(scrub_value.clamp(0.0, 1.0));
BindingValue::Scalar(state.position())
}
fn evaluate_drag(node_id: Uuid, accumulators: &mut AccumulatorStore) -> BindingValue {
let state = accumulators.get_or_default(node_id);
BindingValue::Scalar(state.position())
}
fn narrow_f64_to_f32(value: f64) -> f32 {
let bits = value.to_bits();
let sign = bits & 0x8000_0000_0000_0000;
let sign32 = if sign == 0 { 0 } else { 0x8000_0000 };
let biased = (bits >> 52) & 0x7ff;
if biased == 0x7ff {
if (bits & 0x000f_ffff_ffff_ffff) == 0 {
return f32::from_bits(sign32 | 0x7f80_0000);
}
return f32::NAN;
}
if (bits & 0x7fff_ffff_ffff_ffff) == 0 {
return f32::from_bits(sign32);
}
if !(897..=1150).contains(&biased) {
if biased > 1150 {
return f32::from_bits(sign32 | 0x7f80_0000);
}
return f32::from_bits(sign32);
}
let exponent32 = biased.wrapping_sub(1023).wrapping_add(127);
let mantissa = bits & 0x000f_ffff_ffff_ffff;
let full = (1_u64 << 52) | mantissa;
let kept = full >> 29;
let round_bit = (full >> 28) & 1;
let sticky = full & 0x0fff_ffff;
let round_up = round_bit == 1 && (sticky != 0 || (kept & 1) == 1);
let mut rounded = if round_up { kept.wrapping_add(1) } else { kept };
let mut exponent = exponent32;
if rounded >= 1_u64 << 24 {
rounded >>= 1;
exponent = exponent.wrapping_add(1);
}
let rounded_bytes = rounded.to_le_bytes();
let rounded32 = u32::from_le_bytes([
rounded_bytes[0],
rounded_bytes[1],
rounded_bytes[2],
rounded_bytes[3],
]);
let shifted_bytes = exponent.wrapping_shl(23).to_le_bytes();
let exponent32 = u32::from_le_bytes([
shifted_bytes[0],
shifted_bytes[1],
shifted_bytes[2],
shifted_bytes[3],
]);
f32::from_bits(sign32 | exponent32 | (rounded32 & 0x007f_ffff))
}
#[cfg(test)]
mod tests {
use super::narrow_f64_to_f32;
fn assert_narrowed(input: f64, expected: f32) -> Result<(), String> {
let got = narrow_f64_to_f32(input);
if got.to_bits() != expected.to_bits() {
return Err(format!(
"narrow({input}) = {got} ({:08x}), want {expected} ({:08x})",
got.to_bits(),
expected.to_bits()
));
}
Ok(())
}
#[test]
fn narrow_round_trips_every_normal_f32() -> Result<(), String> {
for value in [
0.0_f32,
-0.0_f32,
1.0,
-1.0,
0.0166,
300_000.0,
123.456,
-0.5,
f32::MIN_POSITIVE,
f32::MAX,
1.0e-30,
2.0,
100.0,
] {
assert_narrowed(f64::from(value), value)?;
}
Ok(())
}
#[test]
fn narrow_flushes_subnormal_results_to_signed_zero() -> Result<(), String> {
assert_narrowed(f64::from(1.0e-40_f32), 0.0)?;
assert_narrowed(f64::from(f32::MIN_POSITIVE) / 2.0, 0.0)?;
let negative_tiny = -f64::from(1.0e-40_f32);
let got = narrow_f64_to_f32(negative_tiny);
if got.to_bits() != (-0.0_f32).to_bits() {
return Err(String::from("negative tiny input must flush to -0.0"));
}
Ok(())
}
#[test]
fn narrow_handles_non_finite_and_overflow() -> Result<(), String> {
if !narrow_f64_to_f32(f64::NAN).is_nan() {
return Err(String::from("NaN must stay NaN"));
}
assert_narrowed(f64::INFINITY, f32::INFINITY)?;
assert_narrowed(f64::NEG_INFINITY, f32::NEG_INFINITY)?;
assert_narrowed(1.0e300, f32::INFINITY)?;
assert_narrowed(-1.0e300, f32::NEG_INFINITY)?;
Ok(())
}
#[test]
fn narrow_rounds_ties_to_even() -> Result<(), String> {
let halfway_up = 1.0_f64 + 5.960464477539063e-8_f64;
assert_narrowed(halfway_up, 1.0)?;
let one_ulp = f64::from(f32::from_bits(0x3f80_0001));
let two_ulp = f64::from(f32::from_bits(0x3f80_0002));
assert_narrowed((one_ulp + two_ulp) / 2.0, f32::from_bits(0x3f80_0002))?;
Ok(())
}
}