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
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143

use alga::general::Real;
use na;
use math::{Vector, Orientation};
use resolution::constraint::velocity_constraint::VelocityConstraint;

/// Structure holding the result of the projected gauss seidel solver.
#[derive(PartialEq, Debug, Clone)]
pub struct Velocities<N: Real> {
    /// Linear velocity.
    pub lv: Vector<N>,
    /// Angular velocity.
    pub av: Orientation<N>
}

impl<N: Real> Velocities<N> {
    /// Creates a new `Velocities`.
    pub fn new() -> Velocities<N> {
        Velocities {
            lv: na::zero(),
            av: na::zero()
        }
    }

    /// Reset this structure to zero.
    pub fn reset(&mut self) {
        self.lv = na::zero();
        self.av = na::zero();
    }
}

/// Solve a set of velocity constraints using the projected gauss seidel solver.
///
/// # Arguments:
/// * `restitution` - constraints to simulate the restitution.
/// * `friction`    - constraints to simulate friction.
/// * `result`      - vector which will contain the result afterward. Must have the size
/// `num_bodies`.
/// * `num_bodies`  - the size of `result`.
/// * `num_iterations` - the number of iterations to perform.
/// * `is_lambda_zero` - indicates whether or not the every element of `result` has been
/// reinitialized. Set this to `false` if the `result` comes from a previous execution of
/// `projected_gauss_seidel_solve`: this will perform warm-starting.
pub fn projected_gauss_seidel_solve<N: Real>(restitution:    &mut [VelocityConstraint<N>],
                                             friction:       &mut [VelocityConstraint<N>],
                                             result:         &mut [Velocities<N>],
                                             num_bodies:     usize,
                                             num_iterations: usize,
                                             is_lambda_zero: bool) {
    // initialize the solution with zeros...
    // mj_lambda is result
    assert!(result.len() == num_bodies);

    for v in result.iter_mut() {
        v.reset();
    }

    // ... and warm start if possible
    if !is_lambda_zero {
        for c in restitution.iter() {
            setup_warmstart_for_constraint(c, result);
        }

        for c in friction.iter() {
            setup_warmstart_for_constraint(c, result);
        }
    }

    /*
     * solve the system
     */
    for _ in 0 .. num_iterations {
        for c in restitution.iter_mut() {
            solve_velocity_constraint(c, result);
        }

        for c in friction.iter_mut() {
            let impulse = restitution[c.friction_limit_id].impulse.clone();

            if impulse > na::zero() {
                let bound = c.friction_coeff * impulse;
                c.lobound = -bound;
                c.hibound = bound;

                solve_velocity_constraint(c, result);
            }
        }
    }
}

#[inline(always)]
fn setup_warmstart_for_constraint<N: Real>(c: &VelocityConstraint<N>, mj_lambda: &mut [Velocities<N>]) {
    let id1 = c.id1;
    let id2 = c.id2;

    if id1 >= 0 {
        mj_lambda[id1 as usize].lv = mj_lambda[id1 as usize].lv - c.weighted_normal1 * c.impulse;
        mj_lambda[id1 as usize].av = mj_lambda[id1 as usize].av + c.weighted_rot_axis1 * c.impulse;
    }

    if id2 >= 0 {
        mj_lambda[id2 as usize].lv = mj_lambda[id2 as usize].lv + c.weighted_normal2 * c.impulse;
        mj_lambda[id2 as usize].av = mj_lambda[id2 as usize].av + c.weighted_rot_axis2 * c.impulse;
    }
}

#[inline(always)]
fn solve_velocity_constraint<N: Real>(c: &mut VelocityConstraint<N>, mj_lambda: &mut [Velocities<N>]) {
    let id1 = c.id1;
    let id2 = c.id2;

    let mut d_lambda_i = c.objective.clone();

    if id1 >= 0 {
        d_lambda_i = d_lambda_i + na::dot(&c.normal, &mj_lambda[id1 as usize].lv)
                                - na::dot(&c.rot_axis1, &mj_lambda[id1 as usize].av);
    }

    if id2 >= 0 {
        d_lambda_i = d_lambda_i - na::dot(&c.normal, &mj_lambda[id2 as usize].lv)
                                - na::dot(&c.rot_axis2, &mj_lambda[id2 as usize].av);
    }

    d_lambda_i = d_lambda_i * c.inv_projected_mass;

    // clamp the value such that: lambda- <= lambda <= lambda+
    // (this is the ``projected'' flavour of Gauss-Seidel
    let lambda_i_0 = c.impulse.clone();

    c.impulse = *na::clamp(&(lambda_i_0 + d_lambda_i), &c.lobound, &c.hibound);

    d_lambda_i = c.impulse - lambda_i_0;


    if id1 >= 0 {
        mj_lambda[id1 as usize].lv = mj_lambda[id1 as usize].lv - c.weighted_normal1 * d_lambda_i;
        mj_lambda[id1 as usize].av = mj_lambda[id1 as usize].av + c.weighted_rot_axis1 * d_lambda_i;
    }

    if id2 >= 0 {
        mj_lambda[id2 as usize].lv = mj_lambda[id2 as usize].lv + c.weighted_normal2 * d_lambda_i;
        mj_lambda[id2 as usize].av = mj_lambda[id2 as usize].av + c.weighted_rot_axis2 * d_lambda_i;
    }
}