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

//! Simulation constraint entities

use {cgmath};
use {collision, component, geometry, math, object};

/// Constraint interface trait
pub trait Constraint <STATE> {
  fn evaluate_position (&self, &STATE) -> f64;
  fn evaluate_velocity (&self, &STATE) -> f64;
}

/// A planar constraint
#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
#[derive(Clone,Debug,PartialEq)]
pub struct Planar {
  plane : geometry::primitive::Plane3 <f64>
}

/// An inter-penetration constraint
#[derive(Clone,Debug,PartialEq)]
pub struct Penetration <'a, A, B> where
  A : object::Bounded + 'static,
  B : object::Bounded + 'static
{
  pub object_a : &'a A,
  pub object_b : &'a B
}

impl Planar {
  #[inline]
  pub fn new (
    position : cgmath::Point3 <f64>, normal : math::Unit3 <f64>
  ) -> Self {
    let base  = position;
    let plane = geometry::primitive::Plane3 { base, normal };
    Planar { plane }
  }

  pub fn position (&self) -> cgmath::Point3 <f64> {
    self.plane.base
  }
  pub fn normal (&self) -> math::Unit3 <f64> {
    self.plane.normal
  }
}

impl <O : object::Temporal> Constraint <O> for Planar {
  fn evaluate_position (&self, object : &O) -> f64 {
    use cgmath::EuclideanSpace;
    let component::Position (ref position) = &object.position();
    cgmath::dot (position.to_vec() - self.position().to_vec(), *self.normal())
  }
  fn evaluate_velocity (&self, object : &O) -> f64 {
    use cgmath::EuclideanSpace;
    cgmath::dot (object.derivatives().velocity - self.position().to_vec(),
      *self.normal())
  }
}

impl <'a, A, B> Penetration <'a, A, B> where
  A : object::Bounded, B : object::Bounded
{

  /// Apply a linear translation based on the result of a distance query on the
  /// objects.
  ///
  /// If distance query indicates the objects are not intersecting, 'None' is
  /// returned.
  pub fn resolve_position (&self) -> Option <(A, B)> {
    let distance = collision::Proximity::query (self.object_a, self.object_b);
    match distance.relation() {
      collision::proximity::Relation::Intersect => {
        let mut out_a = self.object_a.clone();
        let mut out_b = self.object_b.clone();
        let component::Position (ref position_a) = self.object_a.position();
        let component::Position (ref position_b) = self.object_b.position();
        *out_a.position_mut() = component::Position (*position_a + distance.half_axis);
        *out_b.position_mut() = component::Position (*position_b - distance.half_axis);
        Some ((out_a, out_b))
      }
      _ => None
    }
  }

}

// TODO: penetration constraint impl