diff --git a/Cargo.toml b/Cargo.toml index 28462fb..0f176d6 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,6 +10,7 @@ categories = ["graphics", "rendering", "rendering::data-formats"] keywords = ["usd", "usda", "usdc", "usdz", "pxr"] [features] +physics = [] serde = ["dep:serde", "half/serde"] [dependencies] @@ -45,3 +46,7 @@ required-features = ["serde"] [[test]] name = "cross_format_roundtrip" required-features = ["serde"] + +[[test]] +name = "physics_reader" +required-features = ["physics"] diff --git a/fixtures/usdPhysics_scene.usda b/fixtures/usdPhysics_scene.usda new file mode 100644 index 0000000..3110796 --- /dev/null +++ b/fixtures/usdPhysics_scene.usda @@ -0,0 +1,136 @@ +#usda 1.0 +( + defaultPrim = "World" + upAxis = "Y" + metersPerUnit = 1.0 + kilogramsPerUnit = 1.0 + doc = "UsdPhysics reader integration test fixture: covers PhysicsScene, RigidBody+Mass+Collision, every joint kind, multi-apply LimitAPI + DriveAPI, PhysicsMaterial binding, CollisionGroup, FilteredPairsAPI, ArticulationRoot." +) + +def Xform "World" +{ + def PhysicsScene "PhysicsScene" + { + vector3f physics:gravityDirection = (0, -1, 0) + float physics:gravityMagnitude = 9.81 + } + + def Material "Rubber" ( + prepend apiSchemas = ["PhysicsMaterialAPI"] + ) + { + float physics:dynamicFriction = 0.8 + float physics:staticFriction = 0.9 + float physics:restitution = 0.6 + float physics:density = 1100 + } + + def Cube "Base" ( + prepend apiSchemas = [ + "PhysicsRigidBodyAPI", + "PhysicsMassAPI", + "PhysicsCollisionAPI", + "PhysicsArticulationRootAPI", + ] + ) + { + double size = 1.0 + float physics:mass = 2.5 + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.1, 0.1, 0.1) + quatf physics:principalAxes = (1, 0, 0, 0) + bool physics:kinematicEnabled = 1 + vector3f physics:velocity = (0, 0, 0) + } + + def Cube "Arm" ( + prepend apiSchemas = [ + "PhysicsRigidBodyAPI", + "PhysicsCollisionAPI", + "MaterialBindingAPI", + "PhysicsFilteredPairsAPI", + ] + ) + { + double size = 0.5 + rel material:binding:physics = + rel physics:filteredPairs = + double3 xformOp:translate = (1, 0.5, 0) + uniform token[] xformOpOrder = ["xformOp:translate"] + } + + def PhysicsRevoluteJoint "Hinge" + { + rel physics:body0 = + rel physics:body1 = + point3f physics:localPos0 = (0.5, 0.5, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + point3f physics:localPos1 = (-0.25, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + uniform token physics:axis = "Z" + float physics:lowerLimit = -45 + float physics:upperLimit = 45 + float physics:breakForce = 1000 + float physics:breakTorque = 500 + } + + def PhysicsPrismaticJoint "Slider" + { + rel physics:body0 = + rel physics:body1 = + uniform token physics:axis = "X" + float physics:lowerLimit = -1 + float physics:upperLimit = 1 + } + + def PhysicsSphericalJoint "Ball" + { + rel physics:body0 = + rel physics:body1 = + uniform token physics:axis = "Y" + float physics:coneAngle0Limit = 30 + float physics:coneAngle1Limit = 45 + } + + def PhysicsDistanceJoint "Tether" + { + rel physics:body0 = + rel physics:body1 = + float physics:minDistance = 0.5 + float physics:maxDistance = 2.0 + } + + def PhysicsFixedJoint "Lock" + { + rel physics:body0 = + rel physics:body1 = + bool physics:jointEnabled = 0 + } + + def PhysicsJoint "Generic" ( + prepend apiSchemas = [ + "PhysicsLimitAPI:transX", + "PhysicsLimitAPI:rotZ", + "PhysicsDriveAPI:rotZ", + ] + ) + { + rel physics:body0 = + rel physics:body1 = + float limit:transX:physics:low = 1 + float limit:transX:physics:high = 0 + float limit:rotZ:physics:low = -30 + float limit:rotZ:physics:high = 30 + uniform token drive:rotZ:physics:type = "force" + float drive:rotZ:physics:targetVelocity = 90 + float drive:rotZ:physics:stiffness = 100 + float drive:rotZ:physics:damping = 10 + float drive:rotZ:physics:maxForce = 50 + } + + def PhysicsCollisionGroup "Group" + { + rel collection:colliders:includes = [, ] + string physics:mergeGroup = "default" + } +} diff --git a/src/lib.rs b/src/lib.rs index 50f19ac..f65f05c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -33,6 +33,8 @@ pub mod ar; pub mod expr; pub mod layer; pub mod pcp; +#[cfg(feature = "physics")] +pub mod physics; pub mod sdf; pub mod stage; pub mod usda; diff --git a/src/physics/mod.rs b/src/physics/mod.rs new file mode 100644 index 0000000..e7b7529 --- /dev/null +++ b/src/physics/mod.rs @@ -0,0 +1,69 @@ +//! UsdPhysics schema reader. +//! +//! Decodes Pixar's `UsdPhysics` schema family from a composed +//! [`crate::Stage`]. Mirrors the C++ surface in +//! `pxr/usd/usdPhysics/`: +//! +//! Concrete prim types: +//! - [`tokens::T_PHYSICS_SCENE`] — simulation-wide settings (gravity). +//! - [`tokens::T_PHYSICS_JOINT`] — generic 6-DOF joint base. +//! - [`tokens::T_PHYSICS_FIXED_JOINT`] — locks all DOFs. +//! - [`tokens::T_PHYSICS_REVOLUTE_JOINT`] — single-axis rotation. +//! - [`tokens::T_PHYSICS_PRISMATIC_JOINT`] — single-axis translation. +//! - [`tokens::T_PHYSICS_SPHERICAL_JOINT`] — ball joint with cone limits. +//! - [`tokens::T_PHYSICS_DISTANCE_JOINT`] — min/max distance constraint. +//! - [`tokens::T_PHYSICS_COLLISION_GROUP`] — coarse collision filtering. +//! +//! Single-apply API schemas: +//! - [`tokens::API_RIGID_BODY`] — mark prim as physics-driven. +//! - [`tokens::API_MASS`] — explicit mass / inertia / centre-of-mass. +//! - [`tokens::API_COLLISION`] — enable collision on a prim. +//! - [`tokens::API_MESH_COLLISION`] — mesh shape approximation token. +//! - [`tokens::API_PHYSICS_MATERIAL`] — friction / restitution / density. +//! - [`tokens::API_ARTICULATION_ROOT`] — mark a reduced-coordinate articulation. +//! - [`tokens::API_FILTERED_PAIRS`] — fine-grained pair filtering. +//! +//! Multi-apply API schemas (one instance per DOF): +//! - [`tokens::API_LIMIT`] — per-DOF lock / range. +//! - [`tokens::API_DRIVE`] — per-DOF spring-damper actuator. +//! +//! ## Conventions +//! +//! Reader functions return values in the scene's authored units: +//! - Linear values stay in scene units (caller applies `metersPerUnit`). +//! - Mass values stay in scene mass units (caller applies `kilogramsPerUnit`). +//! - Rotational values stay in degrees (USD's authoring convention). +//! - Quaternions stay in USD's textual `(w, x, y, z)` order. +//! - `lower > upper` on any limit encodes a locked DOF. +//! +//! ## Example +//! +//! ```ignore +//! use openusd::{physics, Stage}; +//! +//! let stage = Stage::open("scene.usd")?; +//! let physics = physics::find_physics_prims(&stage)?; +//! for joint_path in &physics.joints { +//! let path = openusd::sdf::path(joint_path)?; +//! if let Some(joint) = physics::read_joint(&stage, &path)? { +//! println!("{}: {:?} body0={:?} body1={:?}", +//! joint.path, joint.kind, joint.body0, joint.body1); +//! } +//! } +//! # anyhow::Ok(()) +//! ``` + +pub mod tokens; + +mod read; +mod types; + +pub use read::{ + find_physics_prims, read_collision_group, read_collision_shape, read_filtered_pairs, read_has_articulation_root, + read_has_collision, read_has_rigid_body, read_is_physics_scene, read_joint, read_joint_drives, read_joint_limits, + read_mass, read_physics_material, read_physics_scene, read_rigid_body, +}; +pub use types::{ + CollisionApprox, Dof, DriveType, JointKind, PhysicsPrims, ReadCollisionGroup, ReadCollisionShape, ReadDrive, + ReadFilteredPairs, ReadJoint, ReadLimit, ReadMass, ReadPhysicsMaterial, ReadPhysicsScene, ReadRigidBody, +}; diff --git a/src/physics/read.rs b/src/physics/read.rs new file mode 100644 index 0000000..1fe7943 --- /dev/null +++ b/src/physics/read.rs @@ -0,0 +1,413 @@ +//! Reader functions for the [UsdPhysics](super) schema family. +//! +//! Each `read_*` helper takes a composed [`Stage`] plus the prim +//! [`Path`] to inspect, returns the decoded struct (or [`None`] when +//! the schema isn't applied / the prim isn't of the expected type), +//! or surfaces a `Result` for IO / decode errors. +//! +//! All numeric values stay in the scene's authored units. Quaternions +//! stay in USD's textual `(w, x, y, z)` order. Angles stay in degrees +//! (as USD authors them). Callers apply `metersPerUnit` / +//! `kilogramsPerUnit` and degrees-to-radians at the import boundary. + +use anyhow::Result; + +use crate::sdf::{Path, Value}; +use crate::Stage; + +use super::tokens::*; +use super::types::*; + +/// Returns `true` when `PhysicsRigidBodyAPI` is applied to the prim. +pub fn read_has_rigid_body(stage: &Stage, prim: &Path) -> Result { + stage.has_api_schema(prim, API_RIGID_BODY) +} + +/// Returns `true` when `PhysicsCollisionAPI` is applied to the prim. +pub fn read_has_collision(stage: &Stage, prim: &Path) -> Result { + stage.has_api_schema(prim, API_COLLISION) +} + +/// Returns `true` when `PhysicsArticulationRootAPI` is applied to the prim. +pub fn read_has_articulation_root(stage: &Stage, prim: &Path) -> Result { + stage.has_api_schema(prim, API_ARTICULATION_ROOT) +} + +/// Returns `true` when the prim is typed `PhysicsScene`. Predicate kept +/// for callers that don't need the gravity attributes; otherwise prefer +/// [`read_physics_scene`]. +pub fn read_is_physics_scene(stage: &Stage, prim: &Path) -> Result { + Ok(stage.type_name(prim)?.as_deref() == Some(T_PHYSICS_SCENE)) +} + +/// Read `PhysicsRigidBodyAPI` state from a prim. Returns `None` when +/// the API isn't applied. +pub fn read_rigid_body(stage: &Stage, prim: &Path) -> Result> { + if !stage.has_api_schema(prim, API_RIGID_BODY)? { + return Ok(None); + } + Ok(Some(ReadRigidBody { + rigid_body_enabled: read_bool(stage, prim, A_RIGID_BODY_ENABLED)?.unwrap_or(true), + kinematic_enabled: read_bool(stage, prim, A_KINEMATIC_ENABLED)?.unwrap_or(false), + starts_asleep: read_bool(stage, prim, A_STARTS_ASLEEP)?.unwrap_or(false), + velocity: read_vec3f(stage, prim, A_VELOCITY)?, + angular_velocity: read_vec3f(stage, prim, A_ANGULAR_VELOCITY)?, + simulation_owner: read_rel_first_target(stage, prim, A_SIMULATION_OWNER)?, + })) +} + +/// Read `PhysicsMassAPI` attributes. Returns `None` when the prim +/// hasn't applied `MassAPI` (so callers can distinguish "unauthored" +/// from "zero mass"). +pub fn read_mass(stage: &Stage, prim: &Path) -> Result> { + if !stage.has_api_schema(prim, API_MASS)? { + return Ok(None); + } + Ok(Some(ReadMass { + mass: read_scalar_f32(stage, prim, A_MASS)?, + center_of_mass: read_vec3f(stage, prim, A_CENTER_OF_MASS)?, + diagonal_inertia: read_vec3f(stage, prim, A_DIAGONAL_INERTIA)?, + principal_axes: read_quatf(stage, prim, A_PRINCIPAL_AXES)?, + density: read_scalar_f32(stage, prim, A_DENSITY)?, + })) +} + +/// Read a `PhysicsScene` prim. Returns `None` when the prim is not +/// typed `PhysicsScene`. +pub fn read_physics_scene(stage: &Stage, prim: &Path) -> Result> { + if !read_is_physics_scene(stage, prim)? { + return Ok(None); + } + Ok(Some(ReadPhysicsScene { + path: prim.as_str().to_string(), + gravity_direction: read_vec3f(stage, prim, A_GRAVITY_DIRECTION)?, + gravity_magnitude: read_scalar_f32(stage, prim, A_GRAVITY_MAGNITUDE)?, + })) +} + +/// Read `PhysicsCollisionAPI` (plus optional `PhysicsMeshCollisionAPI`) +/// state on a prim. Returns `None` when CollisionAPI isn't applied. +/// +/// `approximation` is `Some` only when MeshCollisionAPI is applied; +/// otherwise the prim is a primitive shape and the engine uses its +/// native collider type. +/// +/// `physics_material_path` is resolved through `material:binding:physics` +/// first, then plain `material:binding`. +pub fn read_collision_shape(stage: &Stage, prim: &Path) -> Result> { + let api = stage.api_schemas(prim)?; + if !api.iter().any(|s| s == API_COLLISION) { + return Ok(None); + } + let has_mesh_collision = api.iter().any(|s| s == API_MESH_COLLISION); + let collision_enabled = match read_attr_value(stage, prim, A_COLLISION_ENABLED)? { + Some(Value::Bool(b)) => b, + _ => true, + }; + let approximation = if has_mesh_collision { + read_token(stage, prim, A_APPROXIMATION)?.and_then(|s| CollisionApprox::from_token(&s)) + } else { + None + }; + let simulation_owner = read_rel_first_target(stage, prim, A_SIMULATION_OWNER)?; + let physics_material_path = read_rel_first_target(stage, prim, REL_MATERIAL_BINDING_PHYSICS)? + .or(read_rel_first_target(stage, prim, REL_MATERIAL_BINDING)?); + Ok(Some(ReadCollisionShape { + has_collision_api: true, + has_mesh_collision_api: has_mesh_collision, + collision_enabled, + approximation, + simulation_owner, + physics_material_path, + })) +} + +/// Read `PhysicsMaterialAPI` from a prim. Returns `None` unless the +/// API is applied (regardless of the prim's typeName, so non-Material +/// prims that carry it are also accepted). +pub fn read_physics_material(stage: &Stage, prim: &Path) -> Result> { + if !stage.has_api_schema(prim, API_PHYSICS_MATERIAL)? { + return Ok(None); + } + Ok(Some(ReadPhysicsMaterial { + path: prim.as_str().to_string(), + static_friction: read_scalar_f32(stage, prim, A_STATIC_FRICTION)?, + dynamic_friction: read_scalar_f32(stage, prim, A_DYNAMIC_FRICTION)?, + restitution: read_scalar_f32(stage, prim, A_RESTITUTION)?, + density: read_scalar_f32(stage, prim, A_DENSITY)?, + })) +} + +/// Decode every multi-apply `PhysicsLimitAPI:` instance on a joint. +pub fn read_joint_limits(stage: &Stage, prim: &Path) -> Result> { + let mut out = Vec::new(); + for name in stage.api_schemas(prim)? { + let Some(rest) = name.strip_prefix(API_LIMIT) else { + continue; + }; + let Some(dof_token) = rest.strip_prefix(':') else { + continue; + }; + let Some(dof) = Dof::from_token(dof_token) else { + continue; + }; + let low = read_scalar_f32(stage, prim, &format!("limit:{dof_token}:physics:{LIMIT_SUB_LOW}"))?.unwrap_or(0.0); + let high = read_scalar_f32(stage, prim, &format!("limit:{dof_token}:physics:{LIMIT_SUB_HIGH}"))?.unwrap_or(0.0); + out.push(ReadLimit { dof, low, high }); + } + Ok(out) +} + +/// Decode every multi-apply `PhysicsDriveAPI:` instance on a joint. +pub fn read_joint_drives(stage: &Stage, prim: &Path) -> Result> { + let mut out = Vec::new(); + for name in stage.api_schemas(prim)? { + let Some(rest) = name.strip_prefix(API_DRIVE) else { + continue; + }; + let Some(dof_token) = rest.strip_prefix(':') else { + continue; + }; + let Some(dof) = Dof::from_token(dof_token) else { + continue; + }; + let drive_type = read_token(stage, prim, &format!("drive:{dof_token}:physics:{DRIVE_SUB_TYPE}"))? + .and_then(|s| DriveType::from_token(&s)) + .unwrap_or(DriveType::Force); + let target_position = read_scalar_f32( + stage, + prim, + &format!("drive:{dof_token}:physics:{DRIVE_SUB_TARGET_POSITION}"), + )?; + let target_velocity = read_scalar_f32( + stage, + prim, + &format!("drive:{dof_token}:physics:{DRIVE_SUB_TARGET_VELOCITY}"), + )?; + let damping = + read_scalar_f32(stage, prim, &format!("drive:{dof_token}:physics:{DRIVE_SUB_DAMPING}"))?.unwrap_or(0.0); + let stiffness = + read_scalar_f32(stage, prim, &format!("drive:{dof_token}:physics:{DRIVE_SUB_STIFFNESS}"))?.unwrap_or(0.0); + let max_force = read_scalar_f32(stage, prim, &format!("drive:{dof_token}:physics:{DRIVE_SUB_MAX_FORCE}"))?; + out.push(ReadDrive { + dof, + drive_type, + target_position, + target_velocity, + damping, + stiffness, + max_force, + }); + } + Ok(out) +} + +/// Read any `Physics*Joint` prim. Returns `None` when the prim's +/// typeName isn't a known joint type. +pub fn read_joint(stage: &Stage, prim: &Path) -> Result> { + let kind = match stage.type_name(prim)?.as_deref().unwrap_or_default() { + T_PHYSICS_FIXED_JOINT => JointKind::Fixed, + T_PHYSICS_REVOLUTE_JOINT => JointKind::Revolute, + T_PHYSICS_PRISMATIC_JOINT => JointKind::Prismatic, + T_PHYSICS_SPHERICAL_JOINT => JointKind::Spherical, + T_PHYSICS_DISTANCE_JOINT => JointKind::Distance, + T_PHYSICS_JOINT => JointKind::Generic, + _ => return Ok(None), + }; + let body0 = read_rel_first_target(stage, prim, A_BODY0)?; + let body1 = read_rel_first_target(stage, prim, A_BODY1)?; + let local_pos0 = read_vec3f(stage, prim, A_LOCAL_POS_0)?.unwrap_or([0.0; 3]); + let local_pos1 = read_vec3f(stage, prim, A_LOCAL_POS_1)?.unwrap_or([0.0; 3]); + let local_rot0 = read_quatf(stage, prim, A_LOCAL_ROT_0)?.unwrap_or([1.0, 0.0, 0.0, 0.0]); + let local_rot1 = read_quatf(stage, prim, A_LOCAL_ROT_1)?.unwrap_or([1.0, 0.0, 0.0, 0.0]); + let axis = read_token(stage, prim, A_AXIS)?; + let lower_limit = read_scalar_f32(stage, prim, A_LOWER_LIMIT)?; + let upper_limit = read_scalar_f32(stage, prim, A_UPPER_LIMIT)?; + let collision_enabled = matches!( + read_attr_value(stage, prim, A_JOINT_COLLISION_ENABLED)?, + Some(Value::Bool(true)) + ); + let joint_enabled = match read_attr_value(stage, prim, A_JOINT_ENABLED)? { + Some(Value::Bool(b)) => b, + _ => true, + }; + let exclude_from_articulation = matches!( + read_attr_value(stage, prim, A_EXCLUDE_FROM_ARTICULATION)?, + Some(Value::Bool(true)) + ); + let break_force = read_scalar_f32(stage, prim, A_BREAK_FORCE)?; + let break_torque = read_scalar_f32(stage, prim, A_BREAK_TORQUE)?; + let min_distance = read_scalar_f32(stage, prim, A_MIN_DISTANCE)?; + let max_distance = read_scalar_f32(stage, prim, A_MAX_DISTANCE)?; + let cone_angle_0 = read_scalar_f32(stage, prim, A_CONE_ANGLE_0_LIMIT)?; + let cone_angle_1 = read_scalar_f32(stage, prim, A_CONE_ANGLE_1_LIMIT)?; + let limits = read_joint_limits(stage, prim)?; + let drives = read_joint_drives(stage, prim)?; + + Ok(Some(ReadJoint { + path: prim.as_str().to_string(), + kind, + body0, + body1, + local_pos0, + local_rot0, + local_pos1, + local_rot1, + axis, + lower_limit, + upper_limit, + collision_enabled, + joint_enabled, + exclude_from_articulation, + break_force, + break_torque, + min_distance, + max_distance, + cone_angle_0, + cone_angle_1, + limits, + drives, + })) +} + +/// Read a `PhysicsCollisionGroup` prim. Returns `None` when the +/// typeName doesn't match. +/// +/// Note: full UsdCollectionAPI rule evaluation (includes / excludes / +/// expansion-rule semantics) is left to a follow-up; for now the +/// reader returns the explicit `collection:colliders:includes` target +/// list, adequate for the common authoring pattern. +pub fn read_collision_group(stage: &Stage, prim: &Path) -> Result> { + if stage.type_name(prim)?.as_deref() != Some(T_PHYSICS_COLLISION_GROUP) { + return Ok(None); + } + let members = read_rel_all_targets(stage, prim, "collection:colliders:includes")?; + let filtered_groups = read_rel_all_targets(stage, prim, A_FILTERED_GROUPS)?; + let merge_group = read_token(stage, prim, A_MERGE_GROUP)?; + let invert_filtered_groups = matches!( + read_attr_value(stage, prim, A_INVERT_FILTERED_GROUPS)?, + Some(Value::Bool(true)) + ); + Ok(Some(ReadCollisionGroup { + path: prim.as_str().to_string(), + members, + filtered_groups, + merge_group, + invert_filtered_groups, + })) +} + +/// Read `PhysicsFilteredPairsAPI` from a body prim. Returns `None` +/// unless the API is applied. +pub fn read_filtered_pairs(stage: &Stage, prim: &Path) -> Result> { + if !stage.has_api_schema(prim, API_FILTERED_PAIRS)? { + return Ok(None); + } + let filtered = read_rel_all_targets(stage, prim, A_FILTERED_PAIRS)?; + Ok(Some(ReadFilteredPairs { + path: prim.as_str().to_string(), + filtered, + })) +} + +/// Walk the entire stage once and return categorised path lists. +/// Saves callers from re-walking for every schema family. +pub fn find_physics_prims(stage: &Stage) -> Result { + let mut out = PhysicsPrims::default(); + stage.traverse(|path| { + if let Ok(Some(type_name)) = stage.type_name(path) { + match type_name.as_str() { + T_PHYSICS_SCENE => out.scenes.push(path.as_str().to_string()), + T_PHYSICS_JOINT + | T_PHYSICS_FIXED_JOINT + | T_PHYSICS_REVOLUTE_JOINT + | T_PHYSICS_PRISMATIC_JOINT + | T_PHYSICS_SPHERICAL_JOINT + | T_PHYSICS_DISTANCE_JOINT => out.joints.push(path.as_str().to_string()), + T_PHYSICS_COLLISION_GROUP => out.collision_groups.push(path.as_str().to_string()), + _ => {} + } + } + if let Ok(api) = stage.api_schemas(path) { + let p = path.as_str().to_string(); + if api.iter().any(|s| s == API_RIGID_BODY) { + out.rigid_bodies.push(p.clone()); + } + if api.iter().any(|s| s == API_ARTICULATION_ROOT) { + out.articulation_roots.push(p.clone()); + } + if api.iter().any(|s| s == API_COLLISION) { + out.colliders.push(p.clone()); + } + if api.iter().any(|s| s == API_PHYSICS_MATERIAL) { + out.materials.push(p.clone()); + } + if api.iter().any(|s| s == API_FILTERED_PAIRS) { + out.filtered_pairs.push(p); + } + } + })?; + Ok(out) +} + +// ──────────────────────────────────────────────────────────────────────── +// internal value helpers +// ──────────────────────────────────────────────────────────────────────── + +fn read_attr_value(stage: &Stage, prim: &Path, name: &str) -> Result> { + let attr_path = prim.append_property(name)?; + stage.field::(attr_path, "default") +} + +fn read_bool(stage: &Stage, prim: &Path, name: &str) -> Result> { + Ok(match read_attr_value(stage, prim, name)? { + Some(Value::Bool(b)) => Some(b), + _ => None, + }) +} + +fn read_scalar_f32(stage: &Stage, prim: &Path, name: &str) -> Result> { + Ok(match read_attr_value(stage, prim, name)? { + Some(Value::Float(f)) => Some(f), + Some(Value::Double(d)) => Some(d as f32), + _ => None, + }) +} + +fn read_vec3f(stage: &Stage, prim: &Path, name: &str) -> Result> { + Ok(match read_attr_value(stage, prim, name)? { + Some(Value::Vec3f(a)) => Some(a), + Some(Value::Vec3d(a)) => Some([a[0] as f32, a[1] as f32, a[2] as f32]), + _ => None, + }) +} + +fn read_quatf(stage: &Stage, prim: &Path, name: &str) -> Result> { + Ok(match read_attr_value(stage, prim, name)? { + Some(Value::Quatf(q)) => Some(q), + Some(Value::Quatd(q)) => Some([q[0] as f32, q[1] as f32, q[2] as f32, q[3] as f32]), + _ => None, + }) +} + +fn read_token(stage: &Stage, prim: &Path, name: &str) -> Result> { + Ok(match read_attr_value(stage, prim, name)? { + Some(Value::Token(s)) | Some(Value::String(s)) => Some(s), + _ => None, + }) +} + +fn read_rel_first_target(stage: &Stage, prim: &Path, rel_name: &str) -> Result> { + Ok(read_rel_all_targets(stage, prim, rel_name)?.into_iter().next()) +} + +fn read_rel_all_targets(stage: &Stage, prim: &Path, rel_name: &str) -> Result> { + let rel_path = prim.append_property(rel_name)?; + let raw = stage.field::(rel_path, "targetPaths")?; + let paths = match raw { + Some(Value::PathListOp(op)) => op.flatten(), + Some(Value::PathVec(v)) => v, + _ => Vec::new(), + }; + Ok(paths.into_iter().map(|p| p.as_str().to_string()).collect()) +} diff --git a/src/physics/tokens.rs b/src/physics/tokens.rs new file mode 100644 index 0000000..753b29c --- /dev/null +++ b/src/physics/tokens.rs @@ -0,0 +1,142 @@ +//! Token constants for the [UsdPhysics](super) schema family. +//! +//! Centralised so consumers can match against canonical strings instead of +//! retyping literals. Mirrors the grouping in Pixar's +//! `pxr/usd/usdPhysics/tokens.h`. + +// Concrete prim type names. +pub const T_PHYSICS_SCENE: &str = "PhysicsScene"; +pub const T_PHYSICS_JOINT: &str = "PhysicsJoint"; +pub const T_PHYSICS_FIXED_JOINT: &str = "PhysicsFixedJoint"; +pub const T_PHYSICS_REVOLUTE_JOINT: &str = "PhysicsRevoluteJoint"; +pub const T_PHYSICS_PRISMATIC_JOINT: &str = "PhysicsPrismaticJoint"; +pub const T_PHYSICS_SPHERICAL_JOINT: &str = "PhysicsSphericalJoint"; +pub const T_PHYSICS_DISTANCE_JOINT: &str = "PhysicsDistanceJoint"; +pub const T_PHYSICS_COLLISION_GROUP: &str = "PhysicsCollisionGroup"; + +// API schemas (single-apply unless noted). +pub const API_RIGID_BODY: &str = "PhysicsRigidBodyAPI"; +pub const API_MASS: &str = "PhysicsMassAPI"; +pub const API_COLLISION: &str = "PhysicsCollisionAPI"; +pub const API_MESH_COLLISION: &str = "PhysicsMeshCollisionAPI"; +pub const API_ARTICULATION_ROOT: &str = "PhysicsArticulationRootAPI"; +pub const API_PHYSICS_MATERIAL: &str = "PhysicsMaterialAPI"; +pub const API_FILTERED_PAIRS: &str = "PhysicsFilteredPairsAPI"; +/// Multi-apply: applied as `PhysicsLimitAPI:` per DOF. +pub const API_LIMIT: &str = "PhysicsLimitAPI"; +/// Multi-apply: applied as `PhysicsDriveAPI:` per DOF. +pub const API_DRIVE: &str = "PhysicsDriveAPI"; + +// Stage metadata. +pub const STAGE_KILOGRAMS_PER_UNIT: &str = "kilogramsPerUnit"; + +// PhysicsScene attribute names. +pub const A_GRAVITY_DIRECTION: &str = "physics:gravityDirection"; +pub const A_GRAVITY_MAGNITUDE: &str = "physics:gravityMagnitude"; + +// PhysicsRigidBodyAPI attribute names. +pub const A_RIGID_BODY_ENABLED: &str = "physics:rigidBodyEnabled"; +pub const A_KINEMATIC_ENABLED: &str = "physics:kinematicEnabled"; +pub const A_STARTS_ASLEEP: &str = "physics:startsAsleep"; +pub const A_VELOCITY: &str = "physics:velocity"; +pub const A_ANGULAR_VELOCITY: &str = "physics:angularVelocity"; +pub const A_SIMULATION_OWNER: &str = "physics:simulationOwner"; + +// PhysicsMassAPI attribute names. +pub const A_MASS: &str = "physics:mass"; +pub const A_DENSITY: &str = "physics:density"; +pub const A_CENTER_OF_MASS: &str = "physics:centerOfMass"; +pub const A_DIAGONAL_INERTIA: &str = "physics:diagonalInertia"; +pub const A_PRINCIPAL_AXES: &str = "physics:principalAxes"; + +// PhysicsCollisionAPI / PhysicsMeshCollisionAPI attribute names. +pub const A_COLLISION_ENABLED: &str = "physics:collisionEnabled"; +pub const A_APPROXIMATION: &str = "physics:approximation"; + +// PhysicsMaterialAPI attribute names. +pub const A_DYNAMIC_FRICTION: &str = "physics:dynamicFriction"; +pub const A_STATIC_FRICTION: &str = "physics:staticFriction"; +pub const A_RESTITUTION: &str = "physics:restitution"; + +// Joint base attribute names. +pub const A_BODY0: &str = "physics:body0"; +pub const A_BODY1: &str = "physics:body1"; +pub const A_LOCAL_POS_0: &str = "physics:localPos0"; +pub const A_LOCAL_ROT_0: &str = "physics:localRot0"; +pub const A_LOCAL_POS_1: &str = "physics:localPos1"; +pub const A_LOCAL_ROT_1: &str = "physics:localRot1"; +pub const A_JOINT_ENABLED: &str = "physics:jointEnabled"; +/// On a joint, allow collision between the two attached bodies (default false). +pub const A_JOINT_COLLISION_ENABLED: &str = "physics:collisionEnabled"; +pub const A_EXCLUDE_FROM_ARTICULATION: &str = "physics:excludeFromArticulation"; +pub const A_BREAK_FORCE: &str = "physics:breakForce"; +pub const A_BREAK_TORQUE: &str = "physics:breakTorque"; + +// Single-axis joints (revolute / prismatic). +pub const A_AXIS: &str = "physics:axis"; +pub const A_LOWER_LIMIT: &str = "physics:lowerLimit"; +pub const A_UPPER_LIMIT: &str = "physics:upperLimit"; + +// PhysicsSphericalJoint cone limit attribute names. +pub const A_CONE_ANGLE_0_LIMIT: &str = "physics:coneAngle0Limit"; +pub const A_CONE_ANGLE_1_LIMIT: &str = "physics:coneAngle1Limit"; + +// PhysicsDistanceJoint attribute names. +pub const A_MIN_DISTANCE: &str = "physics:minDistance"; +pub const A_MAX_DISTANCE: &str = "physics:maxDistance"; + +// PhysicsCollisionGroup attribute names. +pub const A_FILTERED_GROUPS: &str = "physics:filteredGroups"; +pub const A_MERGE_GROUP: &str = "physics:mergeGroup"; +pub const A_INVERT_FILTERED_GROUPS: &str = "physics:invertFilteredGroups"; + +// PhysicsFilteredPairsAPI attribute names. +pub const A_FILTERED_PAIRS: &str = "physics:filteredPairs"; + +// Material binding relationships. UsdPhysics looks up +// `material:binding:physics` first and falls back to `material:binding`. +pub const REL_MATERIAL_BINDING: &str = "material:binding"; +pub const REL_MATERIAL_BINDING_PHYSICS: &str = "material:binding:physics"; + +// DOF tokens — used as multi-apply instance names on +// `PhysicsLimitAPI:` and `PhysicsDriveAPI:`, and as the +// segment in `limit::physics:*` / `drive::physics:*`. +pub const DOF_TRANS_X: &str = "transX"; +pub const DOF_TRANS_Y: &str = "transY"; +pub const DOF_TRANS_Z: &str = "transZ"; +pub const DOF_ROT_X: &str = "rotX"; +pub const DOF_ROT_Y: &str = "rotY"; +pub const DOF_ROT_Z: &str = "rotZ"; +pub const DOF_LINEAR: &str = "linear"; +pub const DOF_ANGULAR: &str = "angular"; +pub const DOF_DISTANCE: &str = "distance"; + +// Collision-approximation tokens authored on `PhysicsMeshCollisionAPI`. +pub const APPROX_NONE: &str = "none"; +pub const APPROX_CONVEX_HULL: &str = "convexHull"; +pub const APPROX_CONVEX_DECOMPOSITION: &str = "convexDecomposition"; +pub const APPROX_BOUNDING_SPHERE: &str = "boundingSphere"; +pub const APPROX_BOUNDING_CUBE: &str = "boundingCube"; +pub const APPROX_MESH_SIMPLIFICATION: &str = "meshSimplification"; + +// Joint axis tokens (single-axis joints). +pub const AXIS_X: &str = "X"; +pub const AXIS_Y: &str = "Y"; +pub const AXIS_Z: &str = "Z"; + +// Drive type tokens (`PhysicsDriveAPI::physics:type`). +pub const DRIVE_TYPE_FORCE: &str = "force"; +pub const DRIVE_TYPE_ACCELERATION: &str = "acceleration"; + +// Drive sub-attribute names. Drives are authored as +// `drive::physics:` per the Pixar spec. +pub const DRIVE_SUB_TYPE: &str = "type"; +pub const DRIVE_SUB_TARGET_POSITION: &str = "targetPosition"; +pub const DRIVE_SUB_TARGET_VELOCITY: &str = "targetVelocity"; +pub const DRIVE_SUB_DAMPING: &str = "damping"; +pub const DRIVE_SUB_STIFFNESS: &str = "stiffness"; +pub const DRIVE_SUB_MAX_FORCE: &str = "maxForce"; + +// Limit sub-attribute names (`limit::physics:`). +pub const LIMIT_SUB_LOW: &str = "low"; +pub const LIMIT_SUB_HIGH: &str = "high"; diff --git a/src/physics/types.rs b/src/physics/types.rs new file mode 100644 index 0000000..98c5f7f --- /dev/null +++ b/src/physics/types.rs @@ -0,0 +1,283 @@ +//! Decoded structs and enums returned by [`super::read`] functions. + +use super::tokens::*; + +/// Joint prim types recognised by [`super::read::read_joint`]. +/// `Generic` is `PhysicsJoint` (no built-in axis), typically paired +/// with multi-apply [`super::read::read_joint_limits`] entries. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum JointKind { + Fixed, + Revolute, + Prismatic, + Spherical, + Distance, + Generic, +} + +/// Joint DOF tokens used by multi-apply `PhysicsLimitAPI` and +/// `PhysicsDriveAPI`. `Linear` / `Angular` / `Distance` are the +/// shorthand instance names some authoring tools emit on single-axis +/// joints; `TransX..RotZ` are the canonical six DOFs on generic joints. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub enum Dof { + TransX, + TransY, + TransZ, + RotX, + RotY, + RotZ, + Linear, + Angular, + Distance, +} + +impl Dof { + pub fn as_token(self) -> &'static str { + match self { + Dof::TransX => DOF_TRANS_X, + Dof::TransY => DOF_TRANS_Y, + Dof::TransZ => DOF_TRANS_Z, + Dof::RotX => DOF_ROT_X, + Dof::RotY => DOF_ROT_Y, + Dof::RotZ => DOF_ROT_Z, + Dof::Linear => DOF_LINEAR, + Dof::Angular => DOF_ANGULAR, + Dof::Distance => DOF_DISTANCE, + } + } + + pub fn from_token(s: &str) -> Option { + Some(match s { + DOF_TRANS_X => Dof::TransX, + DOF_TRANS_Y => Dof::TransY, + DOF_TRANS_Z => Dof::TransZ, + DOF_ROT_X => Dof::RotX, + DOF_ROT_Y => Dof::RotY, + DOF_ROT_Z => Dof::RotZ, + DOF_LINEAR => Dof::Linear, + DOF_ANGULAR => Dof::Angular, + DOF_DISTANCE => Dof::Distance, + _ => return None, + }) + } +} + +/// `PhysicsDriveAPI::physics:type` token values. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum DriveType { + Force, + Acceleration, +} + +impl DriveType { + pub fn as_token(self) -> &'static str { + match self { + DriveType::Force => DRIVE_TYPE_FORCE, + DriveType::Acceleration => DRIVE_TYPE_ACCELERATION, + } + } + + pub fn from_token(s: &str) -> Option { + Some(match s { + DRIVE_TYPE_FORCE => DriveType::Force, + DRIVE_TYPE_ACCELERATION => DriveType::Acceleration, + _ => return None, + }) + } +} + +/// `physics:approximation` token values on `PhysicsMeshCollisionAPI`. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum CollisionApprox { + /// Default — engine-specific fallback. For dynamic bodies an + /// importer typically substitutes [`CollisionApprox::ConvexHull`]; + /// for static bodies, a trimesh. + None, + ConvexHull, + ConvexDecomposition, + BoundingSphere, + BoundingCube, + MeshSimplification, +} + +impl CollisionApprox { + pub fn as_token(self) -> &'static str { + match self { + CollisionApprox::None => APPROX_NONE, + CollisionApprox::ConvexHull => APPROX_CONVEX_HULL, + CollisionApprox::ConvexDecomposition => APPROX_CONVEX_DECOMPOSITION, + CollisionApprox::BoundingSphere => APPROX_BOUNDING_SPHERE, + CollisionApprox::BoundingCube => APPROX_BOUNDING_CUBE, + CollisionApprox::MeshSimplification => APPROX_MESH_SIMPLIFICATION, + } + } + + pub fn from_token(s: &str) -> Option { + Some(match s { + APPROX_NONE => CollisionApprox::None, + APPROX_CONVEX_HULL => CollisionApprox::ConvexHull, + APPROX_CONVEX_DECOMPOSITION => CollisionApprox::ConvexDecomposition, + APPROX_BOUNDING_SPHERE => CollisionApprox::BoundingSphere, + APPROX_BOUNDING_CUBE => CollisionApprox::BoundingCube, + APPROX_MESH_SIMPLIFICATION => CollisionApprox::MeshSimplification, + _ => return None, + }) + } +} + +/// Decoded `PhysicsRigidBodyAPI` state. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct ReadRigidBody { + /// `physics:rigidBodyEnabled`, defaults to `true` when unauthored. + pub rigid_body_enabled: bool, + pub kinematic_enabled: bool, + pub starts_asleep: bool, + pub velocity: Option<[f32; 3]>, + /// USD authors `physics:angularVelocity` in degrees per second. + pub angular_velocity: Option<[f32; 3]>, + /// `physics:simulationOwner` rel target — composed prim path of + /// the `PhysicsScene` this body belongs to. + pub simulation_owner: Option, +} + +/// Decoded inertial properties from a prim with `PhysicsMassAPI` applied. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct ReadMass { + pub mass: Option, + pub center_of_mass: Option<[f32; 3]>, + pub diagonal_inertia: Option<[f32; 3]>, + /// Quaternion `(w, x, y, z)` of the principal-axes frame. + pub principal_axes: Option<[f32; 4]>, + /// `physics:density`, used as a fallback when `mass` is absent. + pub density: Option, +} + +/// Decoded `UsdPhysicsScene`. `gravity_direction` is a free vector +/// (typically a unit vector pointing in the direction of gravity); +/// `gravity_magnitude` scales it. Both default to `None` when the +/// scene prim authored only the type but no attributes. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct ReadPhysicsScene { + pub path: String, + pub gravity_direction: Option<[f32; 3]>, + pub gravity_magnitude: Option, +} + +/// Decoded `PhysicsCollisionAPI` (plus optional `PhysicsMeshCollisionAPI`) +/// state on a prim. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct ReadCollisionShape { + pub has_collision_api: bool, + pub has_mesh_collision_api: bool, + /// `physics:collisionEnabled`, defaults to `true` when unauthored + /// (Pixar spec: CollisionAPI applied implies enabled). + pub collision_enabled: bool, + pub approximation: Option, + pub simulation_owner: Option, + /// Resolved through `material:binding:physics` first, then + /// `material:binding`. Empty when no material is bound. + pub physics_material_path: Option, +} + +/// Decoded `PhysicsMaterialAPI` on a `Material` prim. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct ReadPhysicsMaterial { + pub path: String, + pub static_friction: Option, + pub dynamic_friction: Option, + pub restitution: Option, + pub density: Option, +} + +/// One entry from a multi-apply `PhysicsLimitAPI:` schema. +/// `lower > upper` encodes a locked DOF (canonical USD convention). +#[derive(Debug, Clone, PartialEq)] +pub struct ReadLimit { + pub dof: Dof, + pub low: f32, + pub high: f32, +} + +/// One entry from a multi-apply `PhysicsDriveAPI:` schema. +#[derive(Debug, Clone, PartialEq)] +pub struct ReadDrive { + pub dof: Dof, + pub drive_type: DriveType, + pub target_position: Option, + pub target_velocity: Option, + pub damping: f32, + pub stiffness: f32, + pub max_force: Option, +} + +/// Decoded `Physics*Joint` prim. `axis` is `"X" | "Y" | "Z"` when +/// set; `lower_limit` / `upper_limit` are the built-in single-axis +/// limits authored on `PhysicsRevoluteJoint` / `PhysicsPrismaticJoint` +/// (revolute in degrees, prismatic in scene distance units). +/// +/// `limits` and `drives` carry multi-apply `PhysicsLimitAPI:` / +/// `PhysicsDriveAPI:` opinions, used on generic joints to lock / +/// limit / drive specific DOFs. +#[derive(Debug, Clone)] +pub struct ReadJoint { + pub path: String, + pub kind: JointKind, + pub body0: Option, + pub body1: Option, + pub local_pos0: [f32; 3], + pub local_rot0: [f32; 4], + pub local_pos1: [f32; 3], + pub local_rot1: [f32; 4], + pub axis: Option, + pub lower_limit: Option, + pub upper_limit: Option, + pub collision_enabled: bool, + pub joint_enabled: bool, + pub exclude_from_articulation: bool, + pub break_force: Option, + pub break_torque: Option, + /// `physics:minDistance` / `maxDistance` on `PhysicsDistanceJoint`. + pub min_distance: Option, + pub max_distance: Option, + /// `physics:coneAngle0Limit` / `coneAngle1Limit` on + /// `PhysicsSphericalJoint` (degrees, `-1.0` = unlimited). + pub cone_angle_0: Option, + pub cone_angle_1: Option, + pub limits: Vec, + pub drives: Vec, +} + +/// Decoded `PhysicsCollisionGroup`. `members` is the raw list of +/// `collection:colliders:includes` targets — full collection-rule +/// flattening is left to a follow-up. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct ReadCollisionGroup { + pub path: String, + pub members: Vec, + pub filtered_groups: Vec, + pub merge_group: Option, + pub invert_filtered_groups: bool, +} + +/// Decoded `PhysicsFilteredPairsAPI` on a body prim. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct ReadFilteredPairs { + pub path: String, + pub filtered: Vec, +} + +/// Result of [`super::read::find_physics_prims`] — a single-pass +/// stage walk that returns categorised path lists. Saves the caller +/// from re-walking the stage for each schema family. +#[derive(Debug, Clone, Default)] +pub struct PhysicsPrims { + pub scenes: Vec, + pub rigid_bodies: Vec, + pub articulation_roots: Vec, + pub colliders: Vec, + pub joints: Vec, + pub materials: Vec, + pub collision_groups: Vec, + pub filtered_pairs: Vec, +} diff --git a/tests/physics_reader.rs b/tests/physics_reader.rs new file mode 100644 index 0000000..5870cf6 --- /dev/null +++ b/tests/physics_reader.rs @@ -0,0 +1,234 @@ +//! Integration tests for `physics::read::*` against a fixture +//! that exercises every schema family the reader covers. + +use anyhow::Result; +use openusd::physics::{self, CollisionApprox, Dof, DriveType, JointKind}; +use openusd::sdf; +use openusd::Stage; + +const FIXTURE: &str = "fixtures/usdPhysics_scene.usda"; + +fn open() -> Result { + Stage::open(FIXTURE) +} + +#[test] +fn finds_every_physics_prim_family() -> Result<()> { + let stage = open()?; + let prims = physics::find_physics_prims(&stage)?; + + assert_eq!(prims.scenes, vec!["/World/PhysicsScene".to_string()]); + assert_eq!(prims.articulation_roots, vec!["/World/Base".to_string()]); + assert_eq!(prims.materials, vec!["/World/Rubber".to_string()]); + assert_eq!(prims.collision_groups, vec!["/World/Group".to_string()]); + assert_eq!(prims.filtered_pairs, vec!["/World/Arm".to_string()]); + + assert!(prims.rigid_bodies.contains(&"/World/Base".to_string())); + assert!(prims.rigid_bodies.contains(&"/World/Arm".to_string())); + assert_eq!(prims.rigid_bodies.len(), 2); + + assert!(prims.colliders.contains(&"/World/Base".to_string())); + assert!(prims.colliders.contains(&"/World/Arm".to_string())); + assert_eq!(prims.colliders.len(), 2); + + // 6 joints: Hinge, Slider, Ball, Tether, Lock, Generic. + assert_eq!(prims.joints.len(), 6); + + Ok(()) +} + +#[test] +fn reads_physics_scene_gravity() -> Result<()> { + let stage = open()?; + let scene = physics::read_physics_scene(&stage, &sdf::path("/World/PhysicsScene")?)?.expect("PhysicsScene present"); + assert_eq!(scene.path, "/World/PhysicsScene"); + assert_eq!(scene.gravity_direction, Some([0.0, -1.0, 0.0])); + assert_eq!(scene.gravity_magnitude, Some(9.81)); + Ok(()) +} + +#[test] +fn reads_rigid_body_state() -> Result<()> { + let stage = open()?; + let rb = physics::read_rigid_body(&stage, &sdf::path("/World/Base")?)?.expect("RigidBodyAPI on Base"); + assert!(rb.kinematic_enabled); + assert!(rb.rigid_body_enabled); + assert_eq!(rb.velocity, Some([0.0, 0.0, 0.0])); + + // Arm authors RigidBodyAPI but no kinematic/velocity attrs. + let arm = physics::read_rigid_body(&stage, &sdf::path("/World/Arm")?)?.expect("RigidBodyAPI on Arm"); + assert!(!arm.kinematic_enabled); + assert!(arm.velocity.is_none()); + Ok(()) +} + +#[test] +fn reads_mass_inertia_density() -> Result<()> { + let stage = open()?; + let mass = physics::read_mass(&stage, &sdf::path("/World/Base")?)?.expect("MassAPI on Base"); + assert_eq!(mass.mass, Some(2.5)); + assert_eq!(mass.center_of_mass, Some([0.0, 0.0, 0.0])); + assert_eq!(mass.diagonal_inertia, Some([0.1, 0.1, 0.1])); + assert_eq!(mass.principal_axes, Some([1.0, 0.0, 0.0, 0.0])); + + // Arm has no MassAPI applied. + assert!(physics::read_mass(&stage, &sdf::path("/World/Arm")?)?.is_none()); + Ok(()) +} + +#[test] +fn reads_collision_shape_with_material_binding() -> Result<()> { + let stage = open()?; + let arm = physics::read_collision_shape(&stage, &sdf::path("/World/Arm")?)?.expect("CollisionAPI on Arm"); + assert!(arm.has_collision_api); + assert!(!arm.has_mesh_collision_api); + assert!(arm.collision_enabled); + assert!(arm.approximation.is_none()); // no MeshCollisionAPI applied + assert_eq!( + arm.physics_material_path, + Some("/World/Rubber".to_string()), + "material:binding:physics rel should resolve" + ); + Ok(()) +} + +#[test] +fn reads_physics_material_friction_and_restitution() -> Result<()> { + let stage = open()?; + let m = + physics::read_physics_material(&stage, &sdf::path("/World/Rubber")?)?.expect("PhysicsMaterialAPI on Rubber"); + assert_eq!(m.dynamic_friction, Some(0.8)); + assert_eq!(m.static_friction, Some(0.9)); + assert_eq!(m.restitution, Some(0.6)); + assert_eq!(m.density, Some(1100.0)); + Ok(()) +} + +#[test] +fn reads_revolute_joint_with_built_in_limits_and_break() -> Result<()> { + let stage = open()?; + let hinge = physics::read_joint(&stage, &sdf::path("/World/Hinge")?)?.expect("Hinge is a known joint"); + assert_eq!(hinge.kind, JointKind::Revolute); + assert_eq!(hinge.body0.as_deref(), Some("/World/Base")); + assert_eq!(hinge.body1.as_deref(), Some("/World/Arm")); + assert_eq!(hinge.axis.as_deref(), Some("Z")); + assert_eq!(hinge.lower_limit, Some(-45.0)); + assert_eq!(hinge.upper_limit, Some(45.0)); + assert_eq!(hinge.break_force, Some(1000.0)); + assert_eq!(hinge.break_torque, Some(500.0)); + assert!((hinge.local_pos0[0] - 0.5).abs() < 1e-5); + assert!(hinge.joint_enabled); + Ok(()) +} + +#[test] +fn reads_every_specialised_joint_kind() -> Result<()> { + let stage = open()?; + for (path, kind) in [ + ("/World/Hinge", JointKind::Revolute), + ("/World/Slider", JointKind::Prismatic), + ("/World/Ball", JointKind::Spherical), + ("/World/Tether", JointKind::Distance), + ("/World/Lock", JointKind::Fixed), + ("/World/Generic", JointKind::Generic), + ] { + let j = physics::read_joint(&stage, &sdf::path(path)?)?.unwrap_or_else(|| panic!("joint {path} missing")); + assert_eq!(j.kind, kind, "{path}"); + } + Ok(()) +} + +#[test] +fn reads_spherical_cone_and_distance_min_max() -> Result<()> { + let stage = open()?; + let ball = physics::read_joint(&stage, &sdf::path("/World/Ball")?)?.unwrap(); + assert_eq!(ball.cone_angle_0, Some(30.0)); + assert_eq!(ball.cone_angle_1, Some(45.0)); + + let tether = physics::read_joint(&stage, &sdf::path("/World/Tether")?)?.unwrap(); + assert_eq!(tether.min_distance, Some(0.5)); + assert_eq!(tether.max_distance, Some(2.0)); + Ok(()) +} + +#[test] +fn reads_disabled_joint_flag() -> Result<()> { + let stage = open()?; + let lock = physics::read_joint(&stage, &sdf::path("/World/Lock")?)?.unwrap(); + assert!(!lock.joint_enabled, "Lock authors physics:jointEnabled = 0"); + Ok(()) +} + +#[test] +fn reads_multi_apply_limit_and_drive_on_generic_joint() -> Result<()> { + let stage = open()?; + let g = physics::read_joint(&stage, &sdf::path("/World/Generic")?)?.unwrap(); + assert_eq!(g.kind, JointKind::Generic); + + assert_eq!(g.limits.len(), 2); + let trans_x = g.limits.iter().find(|l| l.dof == Dof::TransX).unwrap(); + assert_eq!(trans_x.low, 1.0); + assert_eq!(trans_x.high, 0.0); // low > high → locked + let rot_z = g.limits.iter().find(|l| l.dof == Dof::RotZ).unwrap(); + assert_eq!(rot_z.low, -30.0); + assert_eq!(rot_z.high, 30.0); + + assert_eq!(g.drives.len(), 1); + let d = &g.drives[0]; + assert_eq!(d.dof, Dof::RotZ); + assert_eq!(d.drive_type, DriveType::Force); + assert_eq!(d.target_velocity, Some(90.0)); + assert_eq!(d.stiffness, 100.0); + assert_eq!(d.damping, 10.0); + assert_eq!(d.max_force, Some(50.0)); + Ok(()) +} + +#[test] +fn reads_collision_group_members_and_merge_name() -> Result<()> { + let stage = open()?; + let g = physics::read_collision_group(&stage, &sdf::path("/World/Group")?)?.expect("CollisionGroup present"); + assert!(g.members.contains(&"/World/Base".to_string())); + assert!(g.members.contains(&"/World/Arm".to_string())); + assert_eq!(g.merge_group.as_deref(), Some("default")); + Ok(()) +} + +#[test] +fn reads_filtered_pairs_targets() -> Result<()> { + let stage = open()?; + let f = physics::read_filtered_pairs(&stage, &sdf::path("/World/Arm")?)?.expect("FilteredPairsAPI on Arm"); + assert_eq!(f.filtered, vec!["/World/Base".to_string()]); + Ok(()) +} + +#[test] +fn approx_token_round_trip() { + for tok in [ + CollisionApprox::None, + CollisionApprox::ConvexHull, + CollisionApprox::ConvexDecomposition, + CollisionApprox::BoundingSphere, + CollisionApprox::BoundingCube, + CollisionApprox::MeshSimplification, + ] { + assert_eq!(CollisionApprox::from_token(tok.as_token()), Some(tok)); + } +} + +#[test] +fn dof_token_round_trip() { + for tok in [ + Dof::TransX, + Dof::TransY, + Dof::TransZ, + Dof::RotX, + Dof::RotY, + Dof::RotZ, + Dof::Linear, + Dof::Angular, + Dof::Distance, + ] { + assert_eq!(Dof::from_token(tok.as_token()), Some(tok)); + } +}