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
144
145
146
147
148
149
150
151
152
153
154
155
156

use bit_vec::BitVec;
use visioncortex::PointF64;

pub const EPSILON: f64 = std::f64::EPSILON;

pub fn f64_approximately(a: f64, b: f64) -> bool {
    (a - b).abs() <= EPSILON
}

pub fn normalize_point_f64(p: &PointF64) -> PointF64 {
    let norm = p.norm();
    PointF64::new(p.x / norm, p.y / norm)
}

pub fn normalize_vec_f64(v: &[f64]) -> Vec<f64> {
    let norm = v.iter().fold(0.0, |acc, element| acc + element * element).sqrt();
    v.iter().map(|element| element / norm).collect()
}

pub fn euclid_dist_f64(p1: &PointF64, p2: &PointF64) -> f64 {
    (*p1-*p2).norm()
}

pub fn euclid_dist_vec_f64(v1: &[f64], v2: &[f64]) -> f64 {
    if v1.len() != v2.len() {
        panic!("Lengths of vectors do not agree.");
    }
    v1.iter().enumerate()
        .fold(0.0, |acc, (i, element)| acc + (element - v2[i])*(element - v2[i]))
        .sqrt()
}

/// Returns true iff the traversal p1->p2->p3 is in clockwise order and not collinear.
///
/// Assumes origin in top-left corner.
pub fn clockwise_points_f64(p1: &PointF64, p2: &PointF64, p3: &PointF64) -> bool {
    let cross_product_z_component = |a: PointF64, b: PointF64| { a.x * b.y - a.y * b.x };

    let p1p2 = *p2 - *p1;
    let p1p3 = *p3 - *p1;

    // (p1p2 x p1p3).z > 0 iff the traversal p1->p2->p3 is in clockwise order
    let cross_z = cross_product_z_component(p1p2, p1p3);

    cross_z > EPSILON && cross_z.is_sign_positive()
}

/// Returns the minimum number of bits needed to store n elements
pub fn num_bits_to_store(n: usize) -> usize {
    // Special cases
    if n == 0 {
        return 0; // 0 bits are needed to store 0 elements
    } else if n == 1 {
        return 1; // 1 bit is needed to store 1 element
    }
    let n = n-1; // Given 32 elements, we can label them from 0-31
    num_significant_bits(n)
}

pub fn num_significant_bits(n: usize) -> usize {
    (0_usize.leading_zeros() - n.leading_zeros()) as usize
}

/// Converts a usize into BitVec using the specified number of bits
pub fn into_bitvec(mut n: usize, len: usize) -> BitVec {
    if len < num_significant_bits(n) {
        panic!(format!("Not enough bits to store {}", n));
    }
    let mut bitvec = BitVec::from_elem(len, false);
    for i in (0..len).rev() {
        bitvec.set(i, n % 2 == 1);
        n >>= 1;
    }
    bitvec 
}

#[cfg(test)]
mod tests {

    use super::*;

    #[test]
    fn math_euclid_dist_unit() {
        let p1 = PointF64::new(1.0, 0.0);
        let p2 = PointF64::new(0.0, 1.0);
        let dist = euclid_dist_f64(&p1, &p2);
        assert!(f64_approximately(dist, 2.0_f64.sqrt()))
    }

    #[test]
    fn math_euclid_dist_zero() {
        let p1 = PointF64::new(0.0, 0.0);
        let p2 = PointF64::new(0.0, 0.0);
        let dist = euclid_dist_f64(&p1, &p2);
        assert!(f64_approximately(dist, 0.0))
    }

    #[test]
    fn math_euclid_dist_regular() {
        let p1 = PointF64::new(3.0, 4.0);
        let p2 = PointF64::new(1.0, 5.0);
        let dist = euclid_dist_f64(&p1, &p2);
        assert!(f64_approximately(dist, 5.0_f64.sqrt()))
    }

    #[test]
    fn math_clockwise_points() {
        // Clockwise
        assert!(clockwise_points_f64(&PointF64::new(0.0, 0.0), &PointF64::new(1.0, 0.0), &PointF64::new(0.0, 1.0)));
        // Anti-Clockwise
        assert!(!clockwise_points_f64(&PointF64::new(0.0, 0.0), &PointF64::new(0.0, 1.0), &PointF64::new(1.0, 0.0)));
        // Collinear
        assert!(!clockwise_points_f64(&PointF64::new(0.0, 0.0), &PointF64::new(1.0, 1.0), &PointF64::new(2.0, 2.0)));
        // Same point
        assert!(!clockwise_points_f64(&PointF64::new(1.0, 1.0), &PointF64::new(1.0, 1.0), &PointF64::new(1.0, 1.0)));
        // Random clockwise
        assert!(clockwise_points_f64(&PointF64::new(7.3, 5.2), &PointF64::new(10.5, 2.5), &PointF64::new(15.6, 4.2)));
        // Random anti-clockwise
        assert!(!clockwise_points_f64(&PointF64::new(23.3, 6.8), &PointF64::new(30.1, 14.7), &PointF64::new(27.5, 11.4)));
    }

    #[test]
    fn math_num_bits_to_store() {
        assert_eq!(num_bits_to_store(0), 0); // 0 bits are needed to store 0 elements
        assert_eq!(num_bits_to_store(1), 1); // 1 bit is needed to store 1 element
        assert_eq!(num_bits_to_store(2), 1); // 0,1 are the two elements
        assert_eq!(num_bits_to_store(3), 2); // 00,01,10 are the three elements
        assert_eq!(num_bits_to_store(32), 5); // 00000 -> 11111 (31)
        assert_eq!(num_bits_to_store(33), 6); // 000000 -> 100000 (32)
    }

    #[test]
    fn math_euclid_dist_vec() {
        let v1 = vec![1.0, 1.0, 1.0];
        let v2 = vec![1.0, 1.0, 1.0];
        assert!(f64_approximately(euclid_dist_vec_f64(&v1, &v2), 0.0));
        let v1 = vec![1.0, 0.5, 1.0];
        let v2 = vec![2.0, 1.0, 3.0];
        println!("{}", euclid_dist_vec_f64(&v1, &v2));
        assert!(f64_approximately(euclid_dist_vec_f64(&v1, &v2), 2.29128784747792));
    }

    #[test]
    fn math_into_bitvec() {
        let n = 0;
        assert!(into_bitvec(n, 1).eq_vec(&[false]));
        let n = 1;
        assert!(into_bitvec(n, 1).eq_vec(&[true]));
        let n = 9;
        assert!(into_bitvec(n, 4).eq_vec(&[true, false, false, true]));
        let n = 14;
        assert!(into_bitvec(n, 4).eq_vec(&[true, true, true, false])); 
        let n = 20;
        assert!(into_bitvec(n, 5).eq_vec(&[true, false, true, false, false])); 
    }
}