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

use na::Real;
use crate::solver::{BilateralConstraint, BilateralGroundConstraint, NonlinearUnilateralConstraint,
             UnilateralConstraint, UnilateralGroundConstraint};

/// Set of velocity-based constraints.
pub struct Constraints<N: Real> {
    /// Unilateral velocity constraints involving a dynamic body and the ground (or a body without any degrees of freedoms).
    pub unilateral_ground: Vec<UnilateralGroundConstraint<N>>,
    /// Unilateral velocity constraints between dynamic bodies.
    pub unilateral: Vec<UnilateralConstraint<N>>,
    /// Bilateral velocity constraints involving a dynamic body and the ground (or a body without any degrees of freedoms).
    pub bilateral_ground: Vec<BilateralGroundConstraint<N>>,
    /// Bilateral velocity constraints between dynamic bodies.
    pub bilateral: Vec<BilateralConstraint<N>>,
}

impl<N: Real> Constraints<N> {
    /// Creates a new empty set of constraints.
    pub fn new() -> Self {
        Constraints {
            unilateral_ground: Vec::new(),
            unilateral: Vec::new(),
            bilateral_ground: Vec::new(),
            bilateral: Vec::new(),
        }
    }

    /// The total number of constraints on this set.
    pub fn len(&self) -> usize {
        self.unilateral_ground.len() + self.unilateral.len() + self.bilateral_ground.len()
            + self.bilateral.len()
    }

    /// Remove all constraints from this set.
    pub fn clear(&mut self) {
        self.unilateral_ground.clear();
        self.unilateral.clear();
        self.bilateral_ground.clear();
        self.bilateral.clear();
    }
}

/// Set of non-linear position-based constraints.
pub struct NonlinearConstraints<N: Real> {
    /// Unilateral position-based constraints between two bodies.
    pub unilateral: Vec<NonlinearUnilateralConstraint<N>>,
}

impl<N: Real> NonlinearConstraints<N> {
    /// Create an empty set of nonlinear position-based constraints.
    pub fn new() -> Self {
        NonlinearConstraints {
            unilateral: Vec::new(),
        }
    }

    /// The total number of constraints on this set.
    pub fn len(&self) -> usize {
        self.unilateral.len()
    }

    /// Remove all constraints from this set.
    pub fn clear(&mut self) {
        self.unilateral.clear();
    }
}

/// A set of all velocity constraints and non-linear position-based constraints.
pub struct ConstraintSet<N: Real> {
    /// The velocity constraints constructed.
    pub velocity: Constraints<N>,
    /// The position constraints constructed.
    pub position: NonlinearConstraints<N>,
}

impl<N: Real> ConstraintSet<N> {
    /// Create a new empty set of constraints.
    pub fn new() -> Self {
        ConstraintSet {
            velocity: Constraints::new(),
            position: NonlinearConstraints::new(),
        }
    }

    /// The total number of constraints on this set.
    pub fn len(&self) -> usize {
        self.velocity.len() + self.position.len()
    }

    /// Remove all constraints from this set.
    pub fn clear(&mut self) {
        self.velocity.clear();
        self.position.clear();
    }
}