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

#![doc(hidden)]

use std::iter;
use std::mem;
use std::hash::{Hash, Hasher};
use std::collections::HashMap;

use alga::general::Real;
use na;
use math::Point;
use ncollide::utils::AsBytes;
use utils::DeterministicState;

#[derive(PartialEq)]
/// The identifier of a contact stored in the impulse cache.
pub struct ContactIdentifier<N: Real> {
    obj1:    usize,
    obj2:    usize,
    ccenter: Point<N>
}

impl<N: Real> Eq for ContactIdentifier<N> { } // NOTE: this is  wrong because of floats, but we dont care

impl<N: Real> Hash for ContactIdentifier<N> {
    #[inline]
    fn hash<H: Hasher>(&self, state: &mut H) {
        state.write(self.ccenter.as_bytes())
    }
}

impl<N: Real> ContactIdentifier<N> {
    pub fn new(obj1: usize, obj2: usize, center: Point<N>, step: &N) -> ContactIdentifier<N> {
        let mut cell = center / *step;

        for x in cell.coords.iter_mut() {
            *x = x.trunc()
        }

        ContactIdentifier {
            obj1:    obj1,
            obj2:    obj2,
            ccenter: cell
        }
    }
}

pub struct ImpulseCache<N: Real> {
    // XXX: simulations won't be reproductible because of the randomized HashMap.
    hash_prev:           HashMap<ContactIdentifier<N>, (usize, usize), DeterministicState>,
    cache_prev:          Vec<N>,
    hash_next:           HashMap<ContactIdentifier<N>, (usize, usize), DeterministicState>,
    cache_next:          Vec<N>,
    step:                N,
    impulse_per_contact: usize
}

impl<N: Real> ImpulseCache<N> {
    pub fn new(step: N, impulse_per_contact: usize) -> ImpulseCache<N> {

        ImpulseCache {
            hash_prev:           HashMap::with_capacity_and_hasher(32, DeterministicState::new()),
            hash_next:           HashMap::with_capacity_and_hasher(32, DeterministicState::new()),
            cache_prev:          iter::repeat(na::zero()).take(impulse_per_contact).collect(),
            cache_next:          iter::repeat(na::zero()).take(impulse_per_contact).collect(),
            step:                step,
            impulse_per_contact: impulse_per_contact
        }
    }

    pub fn insert(&mut self, cid: usize, obj1: usize, obj2: usize, center: Point<N>) {
        let id = ContactIdentifier::new(obj1, obj2, center, &self.step);
        let imp =
            match self.hash_prev.get(&id).cloned() {
                Some((_, i)) => i,
                None         => 0
            };

        let _ = self.hash_next.insert(id, (cid, imp));
    }

    pub fn hash(&self) -> &HashMap<ContactIdentifier<N>, (usize, usize), DeterministicState> {
        &self.hash_next
    }

    pub fn hash_mut(&mut self) -> &mut HashMap<ContactIdentifier<N>, (usize, usize), DeterministicState> {
        &mut self.hash_next
    }

    pub fn push_impulsions(&mut self) -> &mut [N] {
        let begin = self.cache_next.len();

        for _ in 0 .. self.impulse_per_contact {
            self.cache_next.push(na::zero());
        }

        let end = self.cache_next.len();

        &mut self.cache_next[begin .. end]
    }

    pub fn reserved_impulse_offset(&self) -> usize {
        self.impulse_per_contact
    }

    pub fn impulsions_at(&self, at: usize) -> &[N] {
        &self.cache_prev[at .. at + self.impulse_per_contact]
    }

    pub fn len(&self) -> usize {
        self.hash_next.len()
    }

    pub fn clear(&mut self) {
        self.cache_prev.clear();
        self.hash_prev.clear();
        self.cache_next.clear();
        self.hash_next.clear();

        self.cache_prev.extend(iter::repeat(na::zero::<N>()).take(self.impulse_per_contact));
        self.cache_next.extend(iter::repeat(na::zero::<N>()).take(self.impulse_per_contact));
    }

    pub fn swap(&mut self) {
        mem::swap(&mut self.hash_prev, &mut self.hash_next);
        mem::swap(&mut self.cache_prev,&mut self.cache_next);
        self.hash_next.clear();
        self.cache_next.truncate(self.impulse_per_contact);
    }
}