phyz 0.2.0

Multi-physics differentiable simulation engine
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94

//! Contact dynamics and soft contact resolution for phyz physics engine.
//!
//! Implements MuJoCo-style soft contacts using penalty forces.

pub mod material;
pub mod solver;

pub use material::ContactMaterial;
pub use solver::{contact_forces, find_contacts, find_ground_contacts};

use crate::collision::Collision;
use crate::math::{SpatialVec, Vec3};

/// Compute contact force for a single collision.
pub fn compute_contact_force(
    collision: &Collision,
    material: &ContactMaterial,
    velocity_i: &Vec3,
    velocity_j: &Vec3,
) -> SpatialVec {
    let depth = collision.penetration_depth;
    if depth <= 0.0 {
        return SpatialVec::zero();
    }

    let normal = collision.contact_normal;
    let rel_vel = velocity_j - velocity_i;
    let normal_vel = rel_vel.dot(&normal);

    // Penalty force: F = k * depth^p - c * v_n
    let k = material.stiffness;
    let c = material.damping;
    let p = 1.0; // Linear spring for now

    let force_magnitude = k * depth.powf(p) - c * normal_vel;
    let force_magnitude = force_magnitude.max(0.0); // No pulling

    let force = normal * force_magnitude;

    // Friction
    let tangent_vel = rel_vel - normal * normal_vel;
    let tangent_speed = tangent_vel.norm();
    let friction_force = if tangent_speed > 1e-10 {
        let tangent_dir = tangent_vel / tangent_speed;
        let friction_magnitude = (material.friction * force_magnitude).min(c * tangent_speed);
        -tangent_dir * friction_magnitude
    } else {
        Vec3::zeros()
    };

    let total_force = force + friction_force;

    // Convert to spatial force (wrench) at contact point
    // τ = r × F, where r is from body COM to contact point
    // For now, assume force applied at body origin (simplified)
    SpatialVec::new(Vec3::zeros(), total_force)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_contact_force_zero_depth() {
        let collision = Collision {
            body_i: 0,
            body_j: 1,
            contact_point: Vec3::zeros(),
            contact_normal: Vec3::z(),
            penetration_depth: 0.0,
        };
        let material = ContactMaterial::default();
        let force = compute_contact_force(&collision, &material, &Vec3::zeros(), &Vec3::zeros());
        assert!(force.linear.norm() < 1e-10);
    }

    #[test]
    fn test_contact_force_penetration() {
        let collision = Collision {
            body_i: 0,
            body_j: 1,
            contact_point: Vec3::zeros(),
            contact_normal: Vec3::z(),
            penetration_depth: 0.1,
        };
        let material = ContactMaterial {
            stiffness: 1000.0,
            ..Default::default()
        };
        let force = compute_contact_force(&collision, &material, &Vec3::zeros(), &Vec3::zeros());
        assert!(force.linear.norm() > 0.0);
        assert!(force.linear.dot(&Vec3::z()) > 0.0);
    }
}