haply 0.8.2

Haply Robotics Client Library for the Inverse Service
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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109

//! Physics computation module for haptic interactions.
//! Provides utilities for calculating forces and interactions with virtual objects.

use serde::{Deserialize, Serialize};

/// A trait representing a physical object that can compute a force given a position.
pub trait PhysicalObject {
    /// Compute the force vector (as [x, y, z]) for a given position.
    fn compute_force(&self, position: [f32; 3]) -> [f32; 3];
}

/// A sphere model with physical properties.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Sphere {
    pub center: [f32; 3],
    pub radius: f32,
    pub stiffness: f32, // restoring force constant (if inside)
    pub magnetism: f32, // attractive force magnitude (if outside)
    pub damping: f32,   // damping factor (not used in this simple example)
}

impl PhysicalObject for Sphere {
    fn compute_force(&self, position: [f32; 3]) -> [f32; 3] {
        let dx = position[0] - self.center[0];
        let dy = position[1] - self.center[1];
        let dz = position[2] - self.center[2];
        let distance = (dx * dx + dy * dy + dz * dz).sqrt();
        if distance < self.radius {
            // Inside the sphere: spring restoring force based on the penetration depth.
            let norm_dx = dx / distance;
            let norm_dy = dy / distance;
            let norm_dz = dz / distance;
            let penetration = self.radius - distance;
            [
                self.stiffness * penetration * norm_dx,
                self.stiffness * penetration * norm_dy,
                self.stiffness * penetration * norm_dz,
            ]
        } else if distance > 0.0 {
            // Outside the sphere: attractive force toward the center.
            let norm_dx = (self.center[0] - position[0]) / distance;
            let norm_dy = (self.center[1] - position[1]) / distance;
            let norm_dz = (self.center[2] - position[2]) / distance;
            [
                self.magnetism * norm_dx,
                self.magnetism * norm_dy,
                self.magnetism * norm_dz,
            ]
        } else {
            [0.0, 0.0, 0.0]
        }
    }
}

/// A cube model with physical properties.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Cube {
    pub center: [f32; 3],
    pub size: f32,      // edge length of the cube
    pub stiffness: f32, // restoring force constant
    pub magnetism: f32, // attraction force magnitude
    pub damping: f32,   // damping factor (not used in this example)
}

impl PhysicalObject for Cube {
    fn compute_force(&self, position: [f32; 3]) -> [f32; 3] {
        // For simplicity, assume that if a coordinate is within half the size,
        // apply a restoring force for that axis; otherwise, apply an attractive force.
        let half = self.size / 2.0;
        let mut force = [0.0_f32, 0.0, 0.0];
        for i in 0..3 {
            let diff = position[i] - self.center[i];
            if diff.abs() < half {
                force[i] = -self.stiffness * diff;
            } else {
                // Apply a constant attractive force toward the center on that axis.
                force[i] = if diff > 0.0 {
                    self.magnetism
                } else {
                    -self.magnetism
                };
            }
        }
        force
    }
}


/// Computes the total force vector acting on a point in 3D space.
/// 
/// Takes a position and a list of physical objects, then calculates and combines
/// the forces from each object to determine the total force at that position.
/// 
/// # Arguments
/// * `position` - The 3D position at which to compute forces
/// * `objects` - A slice of physical objects that can exert forces
/// 
/// # Returns
/// A 3D force vector as [f32; 3]
pub fn compute_total_force(position: [f32; 3], objects: &[Box<dyn PhysicalObject>]) -> [f32; 3] {
    let mut total = [0.0_f32, 0.0, 0.0];
    for obj in objects {
        let f = obj.compute_force(position);
        total[0] += f[0];
        total[1] += f[1];
        total[2] += f[2];
    }
    total
}