phyz_contact/
lib.rs

1//! Contact dynamics and soft contact resolution for phyz physics engine.
2//!
3//! Implements MuJoCo-style soft contacts using penalty forces.
4
5pub mod material;
6pub mod solver;
7
8pub use material::ContactMaterial;
9pub use solver::{contact_forces, find_contacts, find_ground_contacts};
10
11use phyz_collision::Collision;
12use phyz_math::{SpatialVec, Vec3};
13
14/// Compute contact force for a single collision.
15pub fn compute_contact_force(
16    collision: &Collision,
17    material: &ContactMaterial,
18    velocity_i: &Vec3,
19    velocity_j: &Vec3,
20) -> SpatialVec {
21    let depth = collision.penetration_depth;
22    if depth <= 0.0 {
23        return SpatialVec::zero();
24    }
25
26    let normal = collision.contact_normal;
27    let rel_vel = velocity_j - velocity_i;
28    let normal_vel = rel_vel.dot(&normal);
29
30    // Penalty force: F = k * depth^p - c * v_n
31    let k = material.stiffness;
32    let c = material.damping;
33    let p = 1.0; // Linear spring for now
34
35    let force_magnitude = k * depth.powf(p) - c * normal_vel;
36    let force_magnitude = force_magnitude.max(0.0); // No pulling
37
38    let force = normal * force_magnitude;
39
40    // Friction
41    let tangent_vel = rel_vel - normal * normal_vel;
42    let tangent_speed = tangent_vel.norm();
43    let friction_force = if tangent_speed > 1e-10 {
44        let tangent_dir = tangent_vel / tangent_speed;
45        let friction_magnitude = (material.friction * force_magnitude).min(c * tangent_speed);
46        -tangent_dir * friction_magnitude
47    } else {
48        Vec3::zeros()
49    };
50
51    let total_force = force + friction_force;
52
53    // Convert to spatial force (wrench) at contact point
54    // τ = r × F, where r is from body COM to contact point
55    // For now, assume force applied at body origin (simplified)
56    SpatialVec::from_linear_angular(total_force, Vec3::zeros())
57}
58
59#[cfg(test)]
60mod tests {
61    use super::*;
62
63    #[test]
64    fn test_contact_force_zero_depth() {
65        let collision = Collision {
66            body_i: 0,
67            body_j: 1,
68            contact_point: Vec3::zeros(),
69            contact_normal: Vec3::z(),
70            penetration_depth: 0.0,
71        };
72        let material = ContactMaterial::default();
73        let force = compute_contact_force(&collision, &material, &Vec3::zeros(), &Vec3::zeros());
74        assert!(force.linear().norm() < 1e-10);
75    }
76
77    #[test]
78    fn test_contact_force_penetration() {
79        let collision = Collision {
80            body_i: 0,
81            body_j: 1,
82            contact_point: Vec3::zeros(),
83            contact_normal: Vec3::z(),
84            penetration_depth: 0.1,
85        };
86        let material = ContactMaterial {
87            stiffness: 1000.0,
88            ..Default::default()
89        };
90        let force = compute_contact_force(&collision, &material, &Vec3::zeros(), &Vec3::zeros());
91        assert!(force.linear().norm() > 0.0);
92        assert!(force.linear().dot(&Vec3::z()) > 0.0);
93    }
94}
phyz_contact/lib.rs

phyz_contact/
lib.rs
