use serde_json::Value;
use crate::glb::Glb;
#[derive(Clone, Debug, PartialEq)]
pub struct ClipInfo {
pub index: usize,
pub name: Option<String>,
pub duration: f32,
pub channels: usize,
}
pub fn clips(glb: &Glb) -> Vec<ClipInfo> {
let Some(anims) = glb.json["animations"].as_array() else {
return Vec::new();
};
anims
.iter()
.enumerate()
.map(|(index, a)| ClipInfo {
index,
name: a["name"].as_str().map(str::to_owned),
duration: duration(glb, a),
channels: a["channels"].as_array().map_or(0, Vec::len),
})
.collect()
}
fn duration(glb: &Glb, anim: &Value) -> f32 {
let Some(samplers) = anim["samplers"].as_array() else {
return 0.0;
};
let mut max = 0.0f32;
for s in samplers {
let Some(input) = s["input"].as_u64() else {
continue;
};
if let Some(m) = glb.json["accessors"][input as usize]["max"][0].as_f64() {
max = max.max(m as f32);
}
}
max
}
pub fn node_index(glb: &Glb, name: &str) -> Option<usize> {
glb.json["nodes"]
.as_array()?
.iter()
.position(|n| n["name"].as_str() == Some(name))
}
pub fn root_motion(glb: &Glb, clip: usize, node: usize) -> [f32; 3] {
let Some((_, values)) = track(glb, clip, node, "translation") else {
return [0.0; 3];
};
let mut lo = [f32::INFINITY; 3];
let mut hi = [f32::NEG_INFINITY; 3];
for v in &values {
for a in 0..3 {
lo[a] = lo[a].min(v[a]);
hi[a] = hi[a].max(v[a]);
}
}
if values.is_empty() {
return [0.0; 3];
}
[hi[0] - lo[0], hi[1] - lo[1], hi[2] - lo[2]]
}
fn quat_angle_deg(a: &[f32], b: &[f32]) -> f32 {
let dot: f32 = a.iter().zip(b).map(|(x, y)| x * y).sum();
2.0 * dot.abs().min(1.0).acos().to_degrees()
}
fn node_name(glb: &Glb, index: usize) -> String {
glb.json["nodes"]
.as_array()
.and_then(|nodes| nodes.get(index))
.and_then(|n| n["name"].as_str())
.map(str::to_owned)
.unwrap_or_else(|| format!("node {index}"))
}
#[derive(Clone, Debug, PartialEq)]
pub struct LoopClosure {
pub joints: usize,
pub worst_joint: String,
pub max_angle_deg: f32,
pub max_translation: f32,
pub worst_translation_joint: Option<String>,
}
pub fn loop_closure(glb: &Glb, clip: usize) -> Option<LoopClosure> {
let anim = glb.json["animations"].as_array()?.get(clip)?;
let channels = anim["channels"].as_array()?;
let samplers = anim["samplers"].as_array()?;
let mut joints = 0usize;
let mut worst_joint = String::new();
let mut max_angle_deg = 0.0f32;
let mut max_translation = 0.0f32;
let mut worst_translation_joint = None;
for ch in channels {
let path = ch["target"]["path"].as_str().unwrap_or("");
let node = ch["target"]["node"].as_u64().map(|n| n as usize);
let (Some(node), true) = (node, path == "rotation" || path == "translation") else {
continue;
};
let Some(s) = samplers.get(ch["sampler"].as_u64().unwrap_or(u64::MAX) as usize) else {
continue;
};
let width = if path == "rotation" { 4 } else { 3 };
let Some(values) = s["output"]
.as_u64()
.and_then(|ix| floats(glb, ix as usize, width))
else {
continue;
};
let (Some(first), Some(last)) = (values.first(), values.last()) else {
continue;
};
if path == "rotation" {
joints += 1;
let angle = quat_angle_deg(first, last);
if angle > max_angle_deg {
max_angle_deg = angle;
worst_joint = node_name(glb, node);
}
} else {
let d = first
.iter()
.zip(last)
.map(|(a, b)| (a - b) * (a - b))
.sum::<f32>()
.sqrt();
if d > max_translation {
max_translation = d;
worst_translation_joint = Some(node_name(glb, node));
}
}
}
(joints > 0).then_some(LoopClosure {
joints,
worst_joint,
max_angle_deg,
max_translation,
worst_translation_joint,
})
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct SourceRate {
pub keys: usize,
pub fps: f32,
}
pub fn source_rate(glb: &Glb, clip: usize) -> Option<SourceRate> {
let anim = glb.json["animations"].as_array()?.get(clip)?;
let channels = anim["channels"].as_array()?;
let samplers = anim["samplers"].as_array()?;
let mut best: Option<Vec<f32>> = None;
for ch in channels {
let Some(s) = samplers.get(ch["sampler"].as_u64().unwrap_or(u64::MAX) as usize) else {
continue;
};
let Some(times) = s["input"].as_u64().and_then(|ix| scalars(glb, ix as usize)) else {
continue;
};
if times.len() > best.as_ref().map_or(0, Vec::len) {
best = Some(times);
}
}
let times = best.filter(|t| t.len() >= 2)?;
let mut gaps: Vec<f32> = times.windows(2).map(|w| w[1] - w[0]).collect();
gaps.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
let median = gaps[gaps.len() / 2];
(median > 0.0).then_some(SourceRate {
keys: times.len(),
fps: 1.0 / median,
})
}
#[derive(Clone, Debug, PartialEq)]
pub struct PoseDelta {
pub joints: usize,
pub worst_joint: String,
pub max_angle_deg: f32,
}
pub fn end_pose_delta(glb: &Glb, clip: usize, reference_clip: usize) -> Option<PoseDelta> {
let anims = glb.json["animations"].as_array()?;
let anim = anims.get(clip)?;
anims.get(reference_clip)?;
let channels = anim["channels"].as_array()?;
let samplers = anim["samplers"].as_array()?;
let mut joints = 0usize;
let mut worst_joint = String::new();
let mut max_angle_deg = 0.0f32;
for ch in channels {
if ch["target"]["path"].as_str() != Some("rotation") {
continue;
}
let Some(node) = ch["target"]["node"].as_u64().map(|n| n as usize) else {
continue;
};
let Some(s) = samplers.get(ch["sampler"].as_u64().unwrap_or(u64::MAX) as usize) else {
continue;
};
let Some(values) = s["output"]
.as_u64()
.and_then(|ix| floats(glb, ix as usize, 4))
else {
continue;
};
let Some(end) = values.last() else { continue };
let reference = track_raw(glb, reference_clip, node, "rotation")
.and_then(|(_, vals)| vals.into_iter().next())
.unwrap_or_else(|| {
let (_, r, _) = rest(glb, node);
r.to_vec()
});
joints += 1;
let angle = quat_angle_deg(end, &reference);
if angle > max_angle_deg {
max_angle_deg = angle;
worst_joint = node_name(glb, node);
}
}
(joints > 0).then_some(PoseDelta {
joints,
worst_joint,
max_angle_deg,
})
}
fn track_raw(glb: &Glb, clip: usize, node: usize, path: &str) -> Option<(Vec<f32>, Vec<Vec<f32>>)> {
let anim = glb.json["animations"].as_array()?.get(clip)?;
let channels = anim["channels"].as_array()?;
let samplers = anim["samplers"].as_array()?;
let ch = channels.iter().find(|c| {
c["target"]["node"].as_u64() == Some(node as u64)
&& c["target"]["path"].as_str() == Some(path)
})?;
let s = samplers.get(ch["sampler"].as_u64()? as usize)?;
let times = scalars(glb, s["input"].as_u64()? as usize)?;
let width = if path == "rotation" { 4 } else { 3 };
let values = floats(glb, s["output"].as_u64()? as usize, width)?;
Some((times, values))
}
fn track(glb: &Glb, clip: usize, node: usize, path: &str) -> Option<(Vec<f32>, Vec<[f32; 3]>)> {
let (times, raw) = track_raw(glb, clip, node, path)?;
let values = raw
.into_iter()
.map(|v| [v[0], v[1], v[2]])
.collect::<Vec<_>>();
Some((times, values))
}
fn scalars(glb: &Glb, index: usize) -> Option<Vec<f32>> {
floats(glb, index, 1).map(|rows| rows.into_iter().map(|r| r[0]).collect())
}
fn floats(glb: &Glb, index: usize, width: usize) -> Option<Vec<Vec<f32>>> {
let acc = &glb.json["accessors"][index];
if acc["componentType"].as_u64()? != 5126 {
return None;
}
let count = acc["count"].as_u64()? as usize;
let view = &glb.json["bufferViews"][acc["bufferView"].as_u64()? as usize];
let base = view["byteOffset"].as_u64().unwrap_or(0) as usize
+ acc["byteOffset"].as_u64().unwrap_or(0) as usize;
let stride = view["byteStride"].as_u64().unwrap_or(0) as usize;
let stride = if stride == 0 { width * 4 } else { stride };
let mut out = Vec::with_capacity(count);
for i in 0..count {
let mut row = Vec::with_capacity(width);
for c in 0..width {
let at = base + i * stride + c * 4;
let bytes = glb.bin.get(at..at + 4)?;
row.push(f32::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]));
}
out.push(row);
}
Some(out)
}
type Mat4 = [[f32; 4]; 4];
fn identity() -> Mat4 {
let mut m = [[0.0; 4]; 4];
for (i, row) in m.iter_mut().enumerate() {
row[i] = 1.0;
}
m
}
fn mul(a: &Mat4, b: &Mat4) -> Mat4 {
let mut m = [[0.0; 4]; 4];
for (i, row) in m.iter_mut().enumerate() {
for (j, cell) in row.iter_mut().enumerate() {
*cell = (0..4).map(|k| a[i][k] * b[k][j]).sum();
}
}
m
}
fn trs(t: [f32; 3], r: [f32; 4], s: [f32; 3]) -> Mat4 {
let [x, y, z, w] = r;
let (xx, yy, zz) = (x * x, y * y, z * z);
let (xy, xz, yz) = (x * y, x * z, y * z);
let (wx, wy, wz) = (w * x, w * y, w * z);
[
[
(1.0 - 2.0 * (yy + zz)) * s[0],
(2.0 * (xy - wz)) * s[1],
(2.0 * (xz + wy)) * s[2],
t[0],
],
[
(2.0 * (xy + wz)) * s[0],
(1.0 - 2.0 * (xx + zz)) * s[1],
(2.0 * (yz - wx)) * s[2],
t[1],
],
[
(2.0 * (xz - wy)) * s[0],
(2.0 * (yz + wx)) * s[1],
(1.0 - 2.0 * (xx + yy)) * s[2],
t[2],
],
[0.0, 0.0, 0.0, 1.0],
]
}
fn sample(times: &[f32], values: &[Vec<f32>], t: f32) -> Option<Vec<f32>> {
if times.is_empty() || values.len() < times.len() {
return None;
}
if t <= times[0] {
return values.first().cloned();
}
if t >= times[times.len() - 1] {
return values.get(times.len() - 1).cloned();
}
let hi = times.iter().position(|&k| k >= t)?;
let lo = hi.saturating_sub(1);
let span = times[hi] - times[lo];
let f = if span > 0.0 { (t - times[lo]) / span } else { 0.0 };
let mut out: Vec<f32> = values[lo]
.iter()
.zip(&values[hi])
.map(|(a, b)| a + (b - a) * f)
.collect();
if out.len() == 4 {
let n = out.iter().map(|c| c * c).sum::<f32>().sqrt();
if n > 0.0 {
for c in &mut out {
*c /= n;
}
}
}
Some(out)
}
fn parents(glb: &Glb) -> Vec<Option<usize>> {
let nodes = glb.json["nodes"].as_array().map_or(&[][..], |v| v.as_slice());
let mut out = vec![None; nodes.len()];
for (i, n) in nodes.iter().enumerate() {
if let Some(kids) = n["children"].as_array() {
for k in kids.iter().filter_map(Value::as_u64) {
if let Some(slot) = out.get_mut(k as usize) {
*slot = Some(i);
}
}
}
}
out
}
fn rest(glb: &Glb, node: usize) -> ([f32; 3], [f32; 4], [f32; 3]) {
let n = &glb.json["nodes"][node];
let vec3 = |v: &Value, d: [f32; 3]| -> [f32; 3] {
v.as_array()
.filter(|a| a.len() == 3)
.map(|a| {
let g = |i: usize| a[i].as_f64().unwrap_or(d[i] as f64) as f32;
[g(0), g(1), g(2)]
})
.unwrap_or(d)
};
let t = vec3(&n["translation"], [0.0; 3]);
let s = vec3(&n["scale"], [1.0; 3]);
let r = n["rotation"]
.as_array()
.filter(|a| a.len() == 4)
.map(|a| {
let g = |i: usize| a[i].as_f64().unwrap_or(0.0) as f32;
[g(0), g(1), g(2), g(3)]
})
.unwrap_or([0.0, 0.0, 0.0, 1.0]);
(t, r, s)
}
pub fn world_track(glb: &Glb, clip: usize, node: usize) -> Option<(Vec<f32>, Vec<[f32; 3]>)> {
let parent = parents(glb);
let mut chain = vec![node];
let mut at = node;
while let Some(Some(p)) = parent.get(at).copied() {
if chain.len() > parent.len() {
return None;
}
chain.push(p);
at = p;
}
chain.reverse();
let mut times: Vec<f32> = Vec::new();
for &n in &chain {
for path in ["translation", "rotation", "scale"] {
if let Some((ts, _)) = track_raw(glb, clip, n, path) {
times.extend(ts);
}
}
}
if times.is_empty() {
return None;
}
times.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
times.dedup_by(|a, b| (*a - *b).abs() < 1.0e-6);
let tracks: Vec<_> = chain
.iter()
.map(|&n| {
(
n,
track_raw(glb, clip, n, "translation"),
track_raw(glb, clip, n, "rotation"),
track_raw(glb, clip, n, "scale"),
)
})
.collect();
let mut out = Vec::with_capacity(times.len());
for &t in × {
let mut m = identity();
for (n, tt, rr, ss) in &tracks {
let (rt, rr_rest, rs) = rest(glb, *n);
let tv = tt
.as_ref()
.and_then(|(ts, vs)| sample(ts, vs, t))
.map_or(rt, |v| [v[0], v[1], v[2]]);
let rv = rr
.as_ref()
.and_then(|(ts, vs)| sample(ts, vs, t))
.map_or(rr_rest, |v| [v[0], v[1], v[2], v[3]]);
let sv = ss
.as_ref()
.and_then(|(ts, vs)| sample(ts, vs, t))
.map_or(rs, |v| [v[0], v[1], v[2]]);
m = mul(&m, &trs(tv, rv, sv));
}
out.push([m[0][3], m[1][3], m[2][3]]);
}
Some((times, out))
}
pub const PHASE_BINS: usize = 128;
pub const CONTACT_SEPARABILITY_MIN: f32 = 0.5;
pub const MIN_STANCE_FRACTION: f32 = 0.2;
struct Resampled {
pos: Vec<[f32; 3]>,
dt: f32,
duration: f32,
}
fn resampled(glb: &Glb, clip: usize, node: usize) -> Option<Resampled> {
let (times, world) = world_track(glb, clip, node)?;
if world.len() < 4 {
return None;
}
let first = *times.first()?;
let span = *times.last()? - first;
if span <= 0.0 {
return None;
}
let raw: Vec<Vec<f32>> = world.iter().map(|p| p.to_vec()).collect();
let mut pos = Vec::with_capacity(PHASE_BINS);
for i in 0..PHASE_BINS {
let t = first + span * (i as f32 / PHASE_BINS as f32);
let v = sample(×, &raw, t)?;
pos.push([v[0], v[1], v[2]]);
}
let anim = glb.json["animations"].as_array()?.get(clip)?;
Some(Resampled {
pos,
dt: span / PHASE_BINS as f32,
duration: duration(glb, anim),
})
}
fn median(mut v: Vec<f32>) -> Option<f32> {
if v.is_empty() {
return None;
}
v.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
Some(v[v.len() / 2])
}
#[derive(Clone, Debug, PartialEq)]
pub struct ContactTrack {
pub bins: usize,
pub contact: Vec<bool>,
pub speed: Vec<f32>,
pub body_velocity: [f32; 2],
pub stance_fraction: f32,
pub duration: f32,
pub threshold: f32,
}
impl ContactTrack {
pub fn stance_speed(&self) -> f32 {
let planted: Vec<f32> = self
.contact
.iter()
.zip(&self.speed)
.filter(|(c, _)| **c)
.map(|(_, s)| *s)
.collect();
median(planted).unwrap_or(0.0)
}
pub fn cycle_distance(&self) -> f32 {
self.stance_speed() * self.duration
}
}
fn otsu_threshold(values: &[f32]) -> Option<(f32, f32)> {
let mut sorted: Vec<f32> = values
.iter()
.filter(|v| **v > 0.0)
.map(|v| v.ln())
.collect();
sorted.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
let n = sorted.len();
if n < 4 {
return None;
}
let total_mean = sorted.iter().sum::<f32>() / n as f32;
let total_var = sorted.iter().map(|v| (v - total_mean).powi(2)).sum::<f32>() / n as f32;
if total_var <= 1.0e-12 {
return None;
}
let mut best_split = 0usize;
let mut best_between = 0.0f32;
let mut lead_sum = 0.0f32;
let total_sum: f32 = sorted.iter().sum();
for k in 1..n {
lead_sum += sorted[k - 1];
let (w0, w1) = (k as f32 / n as f32, (n - k) as f32 / n as f32);
let mu0 = lead_sum / k as f32;
let mu1 = (total_sum - lead_sum) / (n - k) as f32;
let between = w0 * w1 * (mu0 - mu1) * (mu0 - mu1);
if between > best_between {
best_between = between;
best_split = k;
}
}
if best_split == 0 {
return None;
}
let separability = best_between / total_var;
if separability < CONTACT_SEPARABILITY_MIN {
return None;
}
let threshold = (0.5 * (sorted[best_split - 1] + sorted[best_split])).exp();
Some((threshold, separability))
}
fn contact_core(r: &Resampled, eps: Option<f32>) -> Option<ContactTrack> {
let bins = PHASE_BINS;
let vel: Vec<[f32; 2]> = (0..bins)
.map(|i| {
let p = r.pos[i];
let q = r.pos[(i + 1) % bins];
[(q[0] - p[0]) / r.dt, (q[2] - p[2]) / r.dt]
})
.collect();
let heights: Vec<f32> = r.pos.iter().map(|p| p[1]).collect();
let cut = median(heights.clone())?;
let seed: Vec<usize> = (0..bins).filter(|&i| heights[i] <= cut).collect();
if seed.len() < 4 {
return None;
}
let vx = median(seed.iter().map(|&i| vel[i][0]).collect())?;
let vz = median(seed.iter().map(|&i| vel[i][1]).collect())?;
let stance = (vx * vx + vz * vz).sqrt();
if stance <= 1.0e-6 {
return None;
}
let dist: Vec<f32> = vel
.iter()
.map(|v| {
let (dx, dz) = (v[0] - vx, v[1] - vz);
(dx * dx + dz * dz).sqrt() / stance
})
.collect();
let threshold = match eps {
Some(e) => e,
None => otsu_threshold(&dist)?.0,
};
let contact: Vec<bool> = dist.iter().map(|d| *d < threshold).collect();
let planted = contact.iter().filter(|&&c| c).count();
if planted == 0 || planted == bins {
return None;
}
let speed: Vec<f32> = vel.iter().map(|v| (v[0] * v[0] + v[1] * v[1]).sqrt()).collect();
Some(ContactTrack {
bins,
contact,
speed,
body_velocity: [-vx, -vz],
stance_fraction: planted as f32 / bins as f32,
duration: r.duration,
threshold,
})
}
pub fn contact_track(glb: &Glb, clip: usize, foot: usize, eps: Option<f32>) -> Option<ContactTrack> {
contact_core(&resampled(glb, clip, foot)?, eps)
}
pub fn stance_speed(glb: &Glb, clip: usize, foot: usize, eps: Option<f32>) -> Option<f32> {
Some(contact_track(glb, clip, foot, eps)?.stance_speed())
}
pub fn cycle_distance(glb: &Glb, clip: usize, foot: usize, eps: Option<f32>) -> Option<f32> {
Some(contact_track(glb, clip, foot, eps)?.cycle_distance())
}
pub fn stance_onsets(track: &ContactTrack) -> Vec<usize> {
let n = track.contact.len();
(0..n)
.filter(|&i| track.contact[i] && !track.contact[(i + n - 1) % n])
.collect()
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PhaseMatch {
pub offset: f32,
pub ambiguous: bool,
}
pub fn phase_match(glb: &Glb, a: usize, b: usize, foot: usize) -> Option<PhaseMatch> {
phase_match_joints(glb, a, b, &[foot])
}
pub fn phase_match_joints(glb: &Glb, a: usize, b: usize, joints: &[usize]) -> Option<PhaseMatch> {
let bins = PHASE_BINS;
let curve = |clip: usize, joint: usize| -> Option<Vec<f32>> {
let r = resampled(glb, clip, joint)?;
let mut out: Vec<f32> = r.pos.iter().map(|p| p[1]).collect();
let mean = out.iter().sum::<f32>() / out.len() as f32;
for v in &mut out {
*v -= mean;
}
Some(out)
};
let mut pairs: Vec<(Vec<f32>, Vec<f32>)> = Vec::new();
for &j in joints {
pairs.push((curve(a, j)?, curve(b, j)?));
}
if pairs.is_empty() {
return None;
}
let offset_of = |lag: usize| -> f32 {
let f = lag as f32 / bins as f32;
if f == 0.0 { 0.0 } else { 1.0 - f }
};
let scores: Vec<f32> = (0..bins)
.map(|lag| {
pairs
.iter()
.map(|(ca, cb)| (0..bins).map(|i| ca[i] * cb[(i + lag) % bins]).sum::<f32>())
.sum()
})
.collect();
let mut best = 0usize;
for lag in 1..bins {
let tighter = (signed_offset(offset_of(lag)).abs(), lag)
< (signed_offset(offset_of(best)).abs(), best);
if scores[lag] > scores[best] || (scores[lag] == scores[best] && tighter) {
best = lag;
}
}
let far = bins / 8;
let ambiguous = scores[best] > 0.0
&& (0..bins).any(|lag| {
let d = lag.abs_diff(best).min(bins - lag.abs_diff(best));
d > far && scores[lag] >= 0.9 * scores[best]
});
Some(PhaseMatch {
offset: offset_of(best),
ambiguous,
})
}
pub fn phase_offset(glb: &Glb, a: usize, b: usize, foot: usize) -> Option<f32> {
Some(phase_match(glb, a, b, foot)?.offset)
}
pub fn signed_offset(frac: f32) -> f32 {
if frac > 0.5 { frac - 1.0 } else { frac }
}
pub fn contact_candidates(glb: &Glb, clip: usize) -> Vec<(String, f32)> {
let Some(nodes) = glb.json["nodes"].as_array() else {
return Vec::new();
};
let is_leaf =
|n: &Value| n["children"].as_array().is_none_or(|k| k.is_empty());
let mut lows: Vec<f32> = Vec::new();
let mut named: Vec<(String, f32, f32)> = Vec::new();
for (i, n) in nodes.iter().enumerate() {
if !is_leaf(n) {
continue;
}
let Some(r) = resampled(glb, clip, i) else {
continue;
};
let low = r.pos.iter().map(|p| p[1]).fold(f32::INFINITY, f32::min);
lows.push(low);
let Some(name) = n["name"].as_str() else {
continue;
};
if let Some(t) = contact_core(&r, None)
&& t.stance_fraction >= MIN_STANCE_FRACTION
{
named.push((name.to_owned(), t.stance_fraction, low));
}
}
if lows.is_empty() {
return Vec::new();
}
lows.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
let quartile = lows[lows.len() / 4];
let mut out: Vec<(String, f32)> = named
.into_iter()
.filter(|(_, _, low)| *low <= quartile)
.map(|(name, fraction, _)| (name, fraction))
.collect();
out.sort_by(|a, b| {
b.1.partial_cmp(&a.1)
.unwrap_or(std::cmp::Ordering::Equal)
.then_with(|| a.0.cmp(&b.0))
});
out
}
#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
pub struct GaitCurves {
pub bins: usize,
pub duration: f32,
pub foot_height: Vec<f32>,
pub ground_speed: Vec<f32>,
pub root_drift: Vec<f32>,
pub trace: Vec<[f32; 2]>,
pub contact: Vec<bool>,
pub body_velocity: [f32; 2],
}
pub fn gait_curves(
glb: &Glb,
clip: usize,
foot: usize,
root: Option<usize>,
eps: Option<f32>,
) -> Option<GaitCurves> {
let r = resampled(glb, clip, foot)?;
let c = contact_core(&r, eps)?;
let root_drift = match root.and_then(|n| resampled(glb, clip, n)) {
Some(rr) => {
let o = rr.pos[0];
rr.pos
.iter()
.map(|p| {
let (dx, dz) = (p[0] - o[0], p[2] - o[2]);
(dx * dx + dz * dz).sqrt()
})
.collect()
}
None => vec![0.0; PHASE_BINS],
};
Some(GaitCurves {
bins: PHASE_BINS,
duration: r.duration,
foot_height: r.pos.iter().map(|p| p[1]).collect(),
ground_speed: c.speed.clone(),
root_drift,
trace: r.pos.iter().map(|p| [p[0], p[2]]).collect(),
contact: c.contact.clone(),
body_velocity: c.body_velocity,
})
}
pub fn joint_curves(glb: &Glb, clip: usize, node: usize) -> Option<GaitCurves> {
let r = resampled(glb, clip, node)?;
Some(GaitCurves {
bins: PHASE_BINS,
duration: r.duration,
foot_height: r.pos.iter().map(|p| p[1]).collect(),
ground_speed: (0..PHASE_BINS)
.map(|i| {
let p = r.pos[i];
let q = r.pos[(i + 1) % PHASE_BINS];
let (dx, dz) = ((q[0] - p[0]) / r.dt, (q[2] - p[2]) / r.dt);
(dx * dx + dz * dz).sqrt()
})
.collect(),
root_drift: vec![0.0; PHASE_BINS],
trace: r.pos.iter().map(|p| [p[0], p[2]]).collect(),
contact: vec![false; PHASE_BINS],
body_velocity: [0.0, 0.0],
})
}
#[cfg(test)]
mod tests {
use super::*;
const VALKYRIE: &str = "../../assets/characters/valkyrie.glb";
fn valkyrie() -> Glb {
Glb::open(std::path::Path::new(VALKYRIE))
.unwrap_or_else(|e| panic!("{VALKYRIE}: {e}"))
}
fn figurine_scale() -> f32 {
let text = std::fs::read_to_string("../../assets/grimoire/rigs.ron")
.unwrap_or_else(|e| panic!("rigs.ron: {e}"));
let rigs = crate::rigs::Rigs::parse(&text).unwrap_or_else(|e| panic!("{e}"));
rigs.get("valkyrie")
.unwrap_or_else(|| panic!("no valkyrie in the manifest"))
.scale
}
const TABLE: [(usize, &str, f32, f32); 6] = [
(5, "walk", 1.417, 1.388),
(11, "run", 0.750, 2.135),
(8, "walk_back", 1.458, 1.538),
(12, "run_back", 0.583, 1.185),
(13, "strafe_l", 0.708, 1.937),
(14, "strafe_r", 0.583, 1.259),
];
#[test]
fn the_clip_table_reads_back_off_the_asset() {
let glb = valkyrie();
let all = clips(&glb);
assert_eq!(all.len(), 20, "the Valkyrie ships 20 clips");
for (ix, name, dur, _) in TABLE {
let c = &all[ix];
assert_eq!(c.index, ix);
assert!(
(c.duration - dur).abs() < 1.0 / 24.0,
"clip {ix} ({name}) is {:.3}s, the guide says {dur:.3}s",
c.duration
);
assert!(c.channels > 0, "clip {ix} ({name}) drives nothing");
}
}
#[test]
fn the_gait_clips_are_authored_in_place() {
let glb = valkyrie();
let root = node_index(&glb, "Root").unwrap_or_else(|| panic!("no Root node"));
for (ix, name, _, _) in TABLE {
let m = root_motion(&glb, ix, root);
for (axis, v) in m.iter().enumerate() {
assert!(
*v < 1.0e-4,
"clip {ix} ({name}) moves Root on axis {axis} by {v} — gait clips must be in place"
);
}
}
}
#[test]
fn the_reference_gaits_measure_what_the_guide_recorded() {
let glb = valkyrie();
let foot = node_index(&glb, "foot_l").unwrap_or_else(|| panic!("no foot_l node"));
for (ix, name, _, want) in TABLE.iter().take(2) {
let raw = cycle_distance(&glb, *ix, foot, None)
.unwrap_or_else(|| panic!("clip {ix} ({name}): no measurable stance"));
let got = raw * figurine_scale();
let err = (got - want).abs() / want;
assert!(
err < 0.03,
"clip {ix} ({name}) measures {got:.3} u/cycle, the guide says {want:.3} ({:.1}% out)",
err * 100.0
);
}
}
#[test]
fn phase_offsets_line_up_with_the_guide() {
let glb = valkyrie();
let foot = node_index(&glb, "foot_l").unwrap_or_else(|| panic!("no foot_l node"));
assert_eq!(
phase_offset(&glb, 5, 5, foot),
Some(0.0),
"a clip must be in phase with itself"
);
let got = phase_offset(&glb, 5, 8, foot).unwrap_or_else(|| panic!("no offset"));
assert!(
(got - 0.859).abs() < 0.02,
"walk -> walk_back measured {got:.3} of a cycle, the guide says 0.859 (-0.141)"
);
}
#[test]
fn a_static_mesh_reports_no_clips() {
let glb = Glb::parse(&[]).err();
assert!(glb.is_some(), "an empty buffer is not a GLB");
}
#[test]
fn contact_fractions_stay_plausible() {
let glb = valkyrie();
let foot = node_index(&glb, "foot_l").unwrap_or_else(|| panic!("no foot_l node"));
let derived = [
(5, "walk", 0.30, 0.60),
(11, "run", 0.30, 0.60),
(8, "walk_back", 0.30, 0.60),
(12, "run_back", 0.30, 0.60),
(13, "strafe_l", 0.30, 0.60),
(14, "strafe_r", 0.30, 0.60),
];
for (ix, name, lo, hi) in derived {
let t = contact_track(&glb, ix, foot, None)
.unwrap_or_else(|| panic!("clip {ix} ({name}): no contact track"));
assert!(
(lo..=hi).contains(&t.stance_fraction),
"clip {ix} ({name}) labels {:.3} of the cycle as stance (derived), expected \
{lo}..{hi}",
t.stance_fraction
);
}
let declared = [
(5, "walk", 0.30, 0.60),
(11, "run", 0.30, 0.60),
(8, "walk_back", 0.30, 0.60),
(12, "run_back", 0.08, 0.40),
(13, "strafe_l", 0.30, 0.60),
(14, "strafe_r", 0.05, 0.35),
];
for (ix, name, lo, hi) in declared {
let t = contact_track(&glb, ix, foot, Some(0.35))
.unwrap_or_else(|| panic!("clip {ix} ({name}): no contact track"));
assert!(
(lo..=hi).contains(&t.stance_fraction),
"clip {ix} ({name}) labels {:.3} of the cycle as stance (at 0.35), expected \
{lo}..{hi}",
t.stance_fraction
);
}
}
#[test]
fn the_strafe_clips_travel_the_directions_the_guide_records() {
let glb = valkyrie();
let foot = node_index(&glb, "foot_l").unwrap_or_else(|| panic!("no foot_l node"));
let vx = |clip: usize| {
contact_track(&glb, clip, foot, None)
.unwrap_or_else(|| panic!("clip {clip}: no contact track"))
.body_velocity[0]
};
assert!(vx(13) < -0.5, "clip 13 measures body vx {}, the guide says −X", vx(13));
assert!(vx(14) > 0.5, "clip 14 measures body vx {}, the guide says +X", vx(14));
}
#[test]
fn signed_offsets_prefer_the_small_magnitude() {
assert_eq!(signed_offset(0.0), 0.0);
assert_eq!(signed_offset(0.5), 0.5);
assert!((signed_offset(0.859) - -0.141).abs() < 1.0e-6);
assert!((signed_offset(0.25) - 0.25).abs() < 1.0e-6);
}
fn push_accessor(
accessors: &mut Vec<serde_json::Value>,
views: &mut Vec<serde_json::Value>,
bin: &mut Vec<u8>,
rows: &[Vec<f32>],
) -> usize {
use serde_json::json;
let offset = bin.len();
for row in rows {
for v in row {
bin.extend_from_slice(&v.to_le_bytes());
}
}
let width = rows.first().map_or(1, Vec::len);
views.push(json!({ "byteOffset": offset, "byteLength": rows.len() * width * 4 }));
let max: Vec<f32> = (0..width)
.map(|c| rows.iter().map(|r| r[c]).fold(f32::MIN, f32::max))
.collect();
accessors.push(json!({
"bufferView": views.len() - 1,
"componentType": 5126,
"count": rows.len(),
"max": max,
}));
accessors.len() - 1
}
fn qy(deg: f32) -> Vec<f32> {
let h = deg.to_radians() / 2.0;
vec![0.0, h.sin(), 0.0, h.cos()]
}
fn pose_fixture() -> Glb {
use serde_json::json;
let mut bin = Vec::new();
let mut accessors = Vec::new();
let mut views = Vec::new();
let times = vec![
vec![0.0f32],
vec![1.0 / 24.0],
vec![2.0 / 24.0],
vec![3.0 / 24.0],
vec![1.0],
];
let t = push_accessor(&mut accessors, &mut views, &mut bin, ×);
let closed = vec![qy(0.0), qy(20.0), qy(-15.0), qy(10.0), qy(0.0)];
let closed_out = push_accessor(&mut accessors, &mut views, &mut bin, &closed);
let open = vec![qy(0.0), qy(15.0), qy(30.0), qy(35.0), qy(40.0)];
let open_out = push_accessor(&mut accessors, &mut views, &mut bin, &open);
Glb {
json: json!({
"nodes": [{ "name": "hips" }],
"animations": [
{
"name": "loopy",
"channels": [{ "sampler": 0, "target": { "node": 0, "path": "rotation" } }],
"samplers": [{ "input": t, "output": closed_out }],
},
{
"name": "open",
"channels": [{ "sampler": 0, "target": { "node": 0, "path": "rotation" } }],
"samplers": [{ "input": t, "output": open_out }],
},
],
"accessors": accessors,
"bufferViews": views,
}),
bin,
}
}
#[test]
fn a_closed_loop_passes_and_an_open_one_names_the_worst_joint() {
let glb = pose_fixture();
let closed = loop_closure(&glb, 0).unwrap_or_else(|| panic!("no closure for loopy"));
assert_eq!(closed.joints, 1);
assert!(closed.max_angle_deg < 0.01, "{}", closed.max_angle_deg);
let open = loop_closure(&glb, 1).unwrap_or_else(|| panic!("no closure for open"));
assert_eq!(open.worst_joint, "hips");
assert!(
(open.max_angle_deg - 40.0).abs() < 0.1,
"{}",
open.max_angle_deg
);
assert!(loop_closure(&glb, 9).is_none());
}
#[test]
fn a_one_shot_ending_off_the_reference_idle_is_measured() {
let glb = pose_fixture();
let pd = end_pose_delta(&glb, 1, 0).unwrap_or_else(|| panic!("no delta"));
assert_eq!(pd.joints, 1);
assert_eq!(pd.worst_joint, "hips");
assert!((pd.max_angle_deg - 40.0).abs() < 0.1, "{}", pd.max_angle_deg);
let pd = end_pose_delta(&glb, 0, 0).unwrap_or_else(|| panic!("no delta"));
assert!(pd.max_angle_deg < 0.01, "{}", pd.max_angle_deg);
}
#[test]
fn source_rate_uses_the_median_of_irregular_keys() {
let glb = pose_fixture();
let sr = source_rate(&glb, 0).unwrap_or_else(|| panic!("no rate"));
assert_eq!(sr.keys, 5);
assert!((sr.fps - 24.0).abs() < 0.01, "{}", sr.fps);
}
#[test]
fn joint_curves_carry_shape_without_contact_claims() {
let glb = valkyrie();
let foot = node_index(&glb, "foot_l").unwrap_or_else(|| panic!("no foot_l"));
let c = joint_curves(&glb, 0, foot).unwrap_or_else(|| panic!("no curves"));
assert_eq!(c.bins, PHASE_BINS);
assert_eq!(c.foot_height.len(), PHASE_BINS);
assert!(c.duration > 0.0);
assert!(c.contact.iter().all(|planted| !planted), "no contact was measured");
assert!(c.root_drift.iter().all(|d| *d == 0.0));
assert_eq!(c.body_velocity, [0.0, 0.0]);
let lo = c.foot_height.iter().fold(f32::MAX, |a, &b| a.min(b));
let hi = c.foot_height.iter().fold(f32::MIN, |a, &b| a.max(b));
assert!(hi > lo, "a flat line would mean the resample read nothing");
}
}