use crate::SdfNode;
use glam::Vec3;
pub type NodeId = u32;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum HandSide {
Left,
Right,
Both,
}
#[derive(Debug, Clone, PartialEq)]
pub enum IntentNode {
Grasp {
target_id: NodeId,
hand: HandSide,
force: f32,
},
Release {
target_id: NodeId,
},
Walk {
destination: Vec3,
speed: f32,
},
Gaze {
target: Vec3,
duration_ms: u32,
},
Point {
target: Vec3,
hand: HandSide,
},
Throw {
target: Vec3,
force: f32,
hand: HandSide,
},
Catch {
object_id: NodeId,
},
Push {
target_id: NodeId,
direction: Vec3,
force: f32,
},
Pull {
target_id: NodeId,
direction: Vec3,
force: f32,
},
Rotate {
target_id: NodeId,
axis: Vec3,
angle_rad: f32,
},
Align {
target_id: NodeId,
reference: Vec3,
},
Follow {
target_id: NodeId,
distance: f32,
},
Avoid {
target_id: NodeId,
min_distance: f32,
},
Rest {
duration_ms: u32,
},
LatentIntent {
values: Box<[f32]>,
semantic_hint: Option<String>,
},
Sequence(Vec<Self>),
Parallel(Vec<Self>),
Music {
packet: [u8; 8],
},
}
#[derive(Debug, Clone)]
pub struct Program {
pub sdf: SdfNode,
pub sdf_registry: Vec<SdfNode>,
pub intent: Option<IntentNode>,
}
impl Program {
#[must_use]
pub const fn sdf_only(sdf: SdfNode) -> Self {
Self {
sdf,
sdf_registry: Vec::new(),
intent: None,
}
}
#[must_use]
pub const fn as_sdf(&self) -> &SdfNode {
&self.sdf
}
#[must_use]
pub const fn has_intent(&self) -> bool {
self.intent.is_some()
}
#[must_use]
pub const fn registry_len(&self) -> usize {
self.sdf_registry.len()
}
#[must_use]
pub fn get(&self, id: NodeId) -> Option<&SdfNode> {
self.sdf_registry.get(id as usize)
}
}
#[derive(Debug, Default, Clone)]
pub struct ProgramBuilder {
sdf: Option<SdfNode>,
registry: Vec<SdfNode>,
intent: Option<IntentNode>,
}
impl ProgramBuilder {
#[must_use]
pub fn new() -> Self {
Self::default()
}
#[must_use]
pub fn with_sdf(mut self, sdf: SdfNode) -> Self {
self.sdf = Some(sdf);
self
}
#[must_use]
pub fn register(&mut self, node: SdfNode) -> NodeId {
let id = self.registry.len();
assert!(
u32::try_from(id).is_ok(),
"sdf_registry index が u32::MAX を超えた"
);
self.registry.push(node);
#[allow(clippy::cast_possible_truncation)]
(id as NodeId)
}
#[must_use]
pub fn with_intent(mut self, intent: IntentNode) -> Self {
self.intent = Some(intent);
self
}
#[must_use]
pub fn build(self) -> Program {
Program {
sdf: self
.sdf
.expect("ProgramBuilder::build には with_sdf が必須"),
sdf_registry: self.registry,
intent: self.intent,
}
}
}
#[must_use]
pub const fn grasp(target_id: NodeId, hand: HandSide, force: f32) -> IntentNode {
IntentNode::Grasp {
target_id,
hand,
force,
}
}
#[must_use]
pub const fn release(target_id: NodeId) -> IntentNode {
IntentNode::Release { target_id }
}
#[must_use]
pub const fn walk(destination: Vec3, speed: f32) -> IntentNode {
IntentNode::Walk { destination, speed }
}
#[must_use]
pub const fn gaze(target: Vec3, duration_ms: u32) -> IntentNode {
IntentNode::Gaze {
target,
duration_ms,
}
}
#[must_use]
pub const fn point(target: Vec3, hand: HandSide) -> IntentNode {
IntentNode::Point { target, hand }
}
#[must_use]
pub const fn throw(target: Vec3, force: f32, hand: HandSide) -> IntentNode {
IntentNode::Throw {
target,
force,
hand,
}
}
#[must_use]
pub const fn catch(object_id: NodeId) -> IntentNode {
IntentNode::Catch { object_id }
}
#[must_use]
pub const fn push(target_id: NodeId, direction: Vec3, force: f32) -> IntentNode {
IntentNode::Push {
target_id,
direction,
force,
}
}
#[must_use]
pub const fn pull(target_id: NodeId, direction: Vec3, force: f32) -> IntentNode {
IntentNode::Pull {
target_id,
direction,
force,
}
}
#[must_use]
pub const fn rotate(target_id: NodeId, axis: Vec3, angle_rad: f32) -> IntentNode {
IntentNode::Rotate {
target_id,
axis,
angle_rad,
}
}
#[must_use]
pub const fn align(target_id: NodeId, reference: Vec3) -> IntentNode {
IntentNode::Align {
target_id,
reference,
}
}
#[must_use]
pub const fn follow(target_id: NodeId, distance: f32) -> IntentNode {
IntentNode::Follow {
target_id,
distance,
}
}
#[must_use]
pub const fn avoid(target_id: NodeId, min_distance: f32) -> IntentNode {
IntentNode::Avoid {
target_id,
min_distance,
}
}
#[must_use]
pub const fn rest(duration_ms: u32) -> IntentNode {
IntentNode::Rest { duration_ms }
}
#[must_use]
pub const fn sequence(intents: Vec<IntentNode>) -> IntentNode {
IntentNode::Sequence(intents)
}
#[must_use]
pub const fn parallel(intents: Vec<IntentNode>) -> IntentNode {
IntentNode::Parallel(intents)
}
#[must_use]
pub const fn music_intent(packet: [u8; 8]) -> IntentNode {
IntentNode::Music { packet }
}
#[must_use]
pub fn latent_intent(values: impl Into<Box<[f32]>>) -> IntentNode {
IntentNode::LatentIntent {
values: values.into(),
semantic_hint: None,
}
}
#[must_use]
pub fn latent_intent_hinted(values: impl Into<Box<[f32]>>, hint: impl Into<String>) -> IntentNode {
IntentNode::LatentIntent {
values: values.into(),
semantic_hint: Some(hint.into()),
}
}
impl HandSide {
#[must_use]
pub const fn as_lol(self) -> &'static str {
match self {
Self::Left => "left",
Self::Right => "right",
Self::Both => "both",
}
}
}
fn fmt_vec3(v: Vec3) -> String {
format!("{}, {}, {}", fmt_f32(v.x), fmt_f32(v.y), fmt_f32(v.z))
}
use crate::emit::fmt_f32;
impl IntentNode {
#[must_use]
pub fn to_lol(&self) -> String {
match self {
Self::Grasp {
target_id,
hand,
force,
} => format!("grasp({target_id}, {}, {})", hand.as_lol(), fmt_f32(*force)),
Self::Release { target_id } => format!("release({target_id})"),
Self::Catch { object_id } => format!("catch({object_id})"),
Self::Walk { destination, speed } => {
format!("walk({}, {})", fmt_vec3(*destination), fmt_f32(*speed))
}
Self::Gaze {
target,
duration_ms,
} => format!("gaze({}, {duration_ms})", fmt_vec3(*target)),
Self::Point { target, hand } => {
format!("point({}, {})", fmt_vec3(*target), hand.as_lol())
}
Self::Throw {
target,
force,
hand,
} => format!(
"throw({}, {}, {})",
fmt_vec3(*target),
fmt_f32(*force),
hand.as_lol()
),
Self::Push {
target_id,
direction,
force,
} => format!(
"push({target_id}, {}, {})",
fmt_vec3(*direction),
fmt_f32(*force)
),
Self::Pull {
target_id,
direction,
force,
} => format!(
"pull({target_id}, {}, {})",
fmt_vec3(*direction),
fmt_f32(*force)
),
Self::Rotate {
target_id,
axis,
angle_rad,
} => format!(
"turn({target_id}, {}, {})",
fmt_vec3(*axis),
fmt_f32(*angle_rad)
),
Self::Align {
target_id,
reference,
} => format!("align({target_id}, {})", fmt_vec3(*reference)),
Self::Follow {
target_id,
distance,
} => format!("follow({target_id}, {})", fmt_f32(*distance)),
Self::Avoid {
target_id,
min_distance,
} => format!("avoid({target_id}, {})", fmt_f32(*min_distance)),
Self::Rest { duration_ms } => format!("rest({duration_ms})"),
Self::LatentIntent { values, .. } => {
let inner: Vec<String> = values.iter().map(|v| fmt_f32(*v)).collect();
format!("latent({})", inner.join(", "))
}
Self::Sequence(items) => {
let inner: Vec<String> = items.iter().map(Self::to_lol).collect();
format!("seq({})", inner.join(", "))
}
Self::Parallel(items) => {
let inner: Vec<String> = items.iter().map(Self::to_lol).collect();
format!("par({})", inner.join(", "))
}
Self::Music { packet } => {
let inner: Vec<String> = packet.iter().map(u8::to_string).collect();
format!("music({})", inner.join(", "))
}
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn program_sdf_only() {
let sdf = SdfNode::sphere(1.0);
let prog = Program::sdf_only(sdf);
assert!(!prog.has_intent());
assert_eq!(prog.registry_len(), 0);
}
#[test]
fn program_builder_full() {
let main = SdfNode::sphere(1.0).union(SdfNode::sphere(0.5).translate(2.0, 0.0, 0.0));
let mut builder = ProgramBuilder::new().with_sdf(main);
let cup_id = builder.register(SdfNode::sphere(0.3));
let table_id = builder.register(SdfNode::box3d(1.0, 0.1, 1.0));
assert_eq!(cup_id, 0);
assert_eq!(table_id, 1);
let intent = sequence(vec![
walk(Vec3::new(1.0, 0.0, 0.0), 0.5),
grasp(cup_id, HandSide::Right, 5.0),
]);
let prog = builder.with_intent(intent).build();
assert!(prog.has_intent());
assert_eq!(prog.registry_len(), 2);
assert!(prog.get(cup_id).is_some());
assert!(prog.get(table_id).is_some());
assert!(prog.get(99).is_none());
}
#[test]
fn all_14_verb_constructors() {
let intents = vec![
grasp(0, HandSide::Left, 3.0),
release(0),
walk(Vec3::new(1.0, 0.0, 0.0), 0.5),
gaze(Vec3::new(0.0, 1.0, 0.0), 1000),
point(Vec3::new(2.0, 0.0, 0.0), HandSide::Right),
throw(Vec3::new(5.0, 3.0, 0.0), 10.0, HandSide::Right),
catch(1),
push(0, Vec3::new(1.0, 0.0, 0.0), 2.0),
pull(0, Vec3::new(-1.0, 0.0, 0.0), 2.0),
rotate(0, Vec3::new(0.0, 1.0, 0.0), std::f32::consts::FRAC_PI_2),
align(0, Vec3::new(0.0, 0.0, 1.0)),
follow(1, 2.0),
avoid(2, 1.5),
rest(500),
];
assert_eq!(intents.len(), 14);
}
#[test]
fn to_lol_preserves_a_tiny_force_value() {
let text = grasp(0, HandSide::Right, 5e-7).to_lol();
assert!(
text.contains("5e-7"),
"a tiny force must not be rounded away: {text:?}"
);
}
#[test]
fn sequence_composition() {
let seq = sequence(vec![
grasp(0, HandSide::Right, 3.0),
walk(Vec3::ZERO, 1.0),
release(0),
]);
match seq {
IntentNode::Sequence(items) => assert_eq!(items.len(), 3),
_ => panic!("Sequence を期待"),
}
}
#[test]
fn parallel_composition() {
let par = parallel(vec![
gaze(Vec3::new(1.0, 0.0, 0.0), 500),
walk(Vec3::new(2.0, 0.0, 0.0), 0.5),
]);
match par {
IntentNode::Parallel(items) => assert_eq!(items.len(), 2),
_ => panic!("Parallel を期待"),
}
}
#[test]
fn nested_composition() {
let complex = sequence(vec![
parallel(vec![
walk(Vec3::new(3.0, 0.0, 0.0), 0.5),
gaze(Vec3::new(3.0, 1.0, 0.0), 1500),
]),
grasp(0, HandSide::Right, 4.0),
]);
if let IntentNode::Sequence(items) = &complex {
assert_eq!(items.len(), 2);
assert!(matches!(items[0], IntentNode::Parallel(_)));
} else {
panic!("Sequence を期待");
}
}
#[test]
fn backend_safety_gpu_view_only_sdf() {
let sdf = SdfNode::sphere(1.0);
let prog = ProgramBuilder::new()
.with_sdf(sdf)
.with_intent(rest(100))
.build();
let sdf_ref: &SdfNode = prog.as_sdf();
assert!(matches!(sdf_ref, SdfNode::Sphere { .. }));
}
#[test]
fn hand_side_equality() {
assert_eq!(HandSide::Left, HandSide::Left);
assert_ne!(HandSide::Left, HandSide::Right);
assert_ne!(HandSide::Both, HandSide::Left);
}
#[test]
fn program_get_out_of_range() {
let prog = ProgramBuilder::new().with_sdf(SdfNode::sphere(1.0)).build();
assert!(prog.get(0).is_none());
}
#[test]
fn intent_clone_and_eq() {
let g1 = grasp(0, HandSide::Right, 3.0);
let g2 = g1.clone();
assert_eq!(g1, g2);
let g3 = grasp(0, HandSide::Left, 3.0); assert_ne!(g1, g3);
}
#[test]
fn latent_intent_construction_min_dim() {
let li = latent_intent(vec![0.5, 0.3, 0.2, 0.8]);
match &li {
IntentNode::LatentIntent {
values,
semantic_hint,
} => {
assert_eq!(values.len(), 4);
assert!(semantic_hint.is_none());
assert!((values[0] - 0.5).abs() < 1e-6);
assert!((values[3] - 0.8).abs() < 1e-6);
}
_ => panic!("LatentIntent を期待"),
}
}
#[test]
fn latent_intent_with_hint() {
let li = latent_intent_hinted(vec![0.1, 0.2, 0.3, 0.4], "grasp-like");
match li {
IntentNode::LatentIntent {
values,
semantic_hint,
} => {
assert_eq!(values.len(), 4);
assert_eq!(semantic_hint.as_deref(), Some("grasp-like"));
}
_ => panic!("LatentIntent を期待"),
}
}
#[test]
fn latent_intent_dims_variety_32_64_128() {
for dim in [32_usize, 64, 128] {
#[allow(clippy::cast_precision_loss)]
let vals: Vec<f32> = (0..dim).map(|i| (i as f32) * 0.01).collect();
let li = latent_intent(vals);
if let IntentNode::LatentIntent { values, .. } = li {
assert_eq!(values.len(), dim);
} else {
panic!("LatentIntent を期待 (dim = {dim})");
}
}
}
#[test]
fn latent_intent_composable_in_sequence() {
let seq = sequence(vec![
latent_intent(vec![1.0, 0.0, 0.0, 0.5]),
grasp(0, HandSide::Right, 3.0),
latent_intent_hinted(vec![0.5, 0.5, 0.0, 0.3], "release-approach"),
]);
if let IntentNode::Sequence(items) = &seq {
assert_eq!(items.len(), 3);
assert!(matches!(items[0], IntentNode::LatentIntent { .. }));
assert!(matches!(items[2], IntentNode::LatentIntent { .. }));
} else {
panic!("Sequence を期待");
}
}
#[test]
fn latent_intent_composable_in_parallel() {
let par = parallel(vec![
latent_intent(vec![0.1, 0.2, 0.3, 0.5]),
latent_intent(vec![0.4, 0.5, 0.6, 0.7]),
]);
if let IntentNode::Parallel(items) = par {
assert_eq!(items.len(), 2);
assert!(matches!(items[0], IntentNode::LatentIntent { .. }));
} else {
panic!("Parallel を期待");
}
}
#[test]
fn latent_intent_clone_and_eq() {
let a = latent_intent(vec![1.0, 2.0, 3.0, 4.0]);
let b = a.clone();
assert_eq!(a, b);
let c = latent_intent(vec![1.0, 2.0, 3.0, 5.0]); assert_ne!(a, c);
}
#[test]
fn music_intent_constructor_wraps_packet() {
let packet = [1u8, 2, 3, 4, 5, 6, 7, 8];
let node = music_intent(packet);
match node {
IntentNode::Music { packet: p } => assert_eq!(p, packet),
_ => panic!("Music variant を期待"),
}
}
#[test]
fn music_intent_roundtrip_via_variant() {
let packet = [0u8, 0, 4, 80, 0, 0, 0xDE, 0xC0]; let node = music_intent(packet);
if let IntentNode::Music { packet: extracted } = node {
assert_eq!(extracted[0], 0); assert_eq!(extracted[3], 80); assert_eq!(u16::from_le_bytes([extracted[6], extracted[7]]), 0xC0DE);
} else {
panic!("Music variant を期待");
}
}
#[test]
fn music_intent_composable_in_sequence() {
let packet_a = [0u8, 0, 4, 80, 0, 0, 0x11, 0x11];
let packet_b = [1u8, 4, 8, 60, 4, 5, 0x22, 0x22];
let seq = sequence(vec![music_intent(packet_a), music_intent(packet_b)]);
if let IntentNode::Sequence(items) = &seq {
assert_eq!(items.len(), 2);
assert!(matches!(items[0], IntentNode::Music { .. }));
assert!(matches!(items[1], IntentNode::Music { .. }));
} else {
panic!("Sequence を期待");
}
}
#[test]
fn music_intent_composable_with_physical_verbs() {
let par = parallel(vec![
music_intent([0u8, 0, 4, 80, 0, 0, 0xC0, 0xDE]),
grasp(0, HandSide::Right, 5.0),
]);
if let IntentNode::Parallel(items) = par {
assert_eq!(items.len(), 2);
assert!(matches!(items[0], IntentNode::Music { .. }));
assert!(matches!(items[1], IntentNode::Grasp { .. }));
} else {
panic!("Parallel を期待");
}
}
#[test]
fn music_intent_clone_and_eq() {
let a = music_intent([1u8, 2, 3, 4, 5, 6, 7, 8]);
let b = a.clone();
assert_eq!(a, b);
let c = music_intent([1u8, 2, 3, 4, 5, 6, 7, 9]); assert_ne!(a, c);
}
}