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
157
158
159
160
161
162
163
164
165

use nalgebra::{distance_squared, Point3, Vector3};
use alga::general::Real;
use alga::general::AbstractModule;
use num_traits::identities::Zero;
use IsEnabled;

use std::cell::RefCell;
use std::rc::Rc;

/// Represents result of a steering behaviour computation. User can aggregate
/// more than one behaviour result into single acceleration struct.
#[derive(Debug, PartialEq)]
pub struct SteeringAcceleration<T: Real> {
    /// linear acceleration component
    pub linear: Vector3<T>,
    /// angular acceleration component
    pub angular: T,
}


impl<T: Real> SteeringAcceleration<T> {
    pub fn default() -> SteeringAcceleration<T> {
        SteeringAcceleration {
            linear: Vector3::zero(),
            angular: T::zero(),
        }
    }
    /// Creates a steering acceleration struct using given linear and angular components
    pub fn new(
        linear_acceleration: Vector3<T>,
        angular_acceleration: T,
    ) -> SteeringAcceleration<T> {
        SteeringAcceleration {
            linear: linear_acceleration,
            angular: angular_acceleration,
        }
    }

    /// Tests whether both linear and angular acceleration compenents are zero
    pub fn is_zero(&self) -> bool {
        self.angular.is_zero() && self.linear.is_zero()
    }

    /// Sets both compononents to zero
    pub fn set_zero(&mut self) -> &mut Self {
        self.angular = T::zero();
        self.linear = Vector3::zero();
        self
    }

    ///
    pub fn add(&mut self, other: SteeringAcceleration<T>) -> &mut Self {
        self.angular += other.angular;
        self.linear += other.linear;
        self
    }

    ///
    pub fn scl(&mut self, scale: T) -> &mut Self {
        self.angular *= scale;
        self.linear = self.linear.multiply_by(scale);
        self
    }

    ///
    pub fn mul_add(&mut self, other: SteeringAcceleration<T>, scale: T) -> &mut Self {
        self.angular += other.angular * scale;
        self.linear += other.linear.multiply_by(scale);
        self
    }

    ///
    pub fn calculate_square_magnitude(&self) -> T {
        distance_squared(&Point3::from_coordinates(self.linear), &Point3::origin()) +
            self.angular * self.angular
    }

    ///
    pub fn calculate_magnitude(&self) -> T {
        self.calculate_square_magnitude().sqrt()
    }
}

pub trait SteeringAccelerationCalculator<T: Real>: IsEnabled<T> {
    fn calculate_steering(
        &mut self,
        steering_acceleration: Rc<RefCell<SteeringAcceleration<T>>>,
    ) -> Rc<RefCell<SteeringAcceleration<T>>> {
        if self.is_enabled() {
            self.calculate_real_steering(steering_acceleration.clone());
            steering_acceleration
        } else {
            steering_acceleration.borrow_mut().set_zero();
            steering_acceleration
        }
    }

    fn calculate_real_steering(
        &self,
        steering_acceleration: Rc<RefCell<SteeringAcceleration<T>>>,
    ) -> Rc<RefCell<SteeringAcceleration<T>>>;
    // fn set_enabled(&mut self, is_enabled : bool);
}

#[cfg(test)]
mod test {
    use super::SteeringAcceleration;
    use nalgebra::Vector3;
    #[test]
    fn is_zero_positive() {
        let mut acceleration = SteeringAcceleration::new(Vector3::new(1.0f32, 2.0, 3.0), 5.0f32);
        acceleration.set_zero();
        assert!(acceleration.is_zero());
    }

    #[test]
    fn is_zero_negative() {
        let acceleration = SteeringAcceleration::new(Vector3::new(1.0f32, 2.0, 3.0), 5.0f32);
        assert_eq!(acceleration.is_zero(), false);
    }

    #[test]
    fn add() {
        let mut acceleration = SteeringAcceleration::new(Vector3::new(1.0f32, 1.0, 1.0), 1.0f32);
        let acceleration2 = SteeringAcceleration::new(Vector3::new(1.0f32, 1.0, 1.0), 1.0f32);
        acceleration.add(acceleration2);
        assert_eq!(
            SteeringAcceleration::new(Vector3::new(2.0f32, 2.0, 2.0), 2.0f32),
            acceleration
        );
    }

    #[test]
    fn scl() {
        let mut acceleration = SteeringAcceleration::new(Vector3::new(1.0f32, 1.0, 1.0), 1.0f32);
        acceleration.scl(2.0f32);
        assert_eq!(
            SteeringAcceleration::new(Vector3::new(2.0f32, 2.0, 2.0), 2.0),
            acceleration
        );
    }

    #[test]
    fn calculate_square_magnitude() {
        let acceleration = SteeringAcceleration::new(Vector3::new(2.0f32, 2.0, 2.0), 2.0f32);
        assert_eq!(16f32, acceleration.calculate_square_magnitude());
    }

    #[test]
    fn calculate_magnitude() {
        let acceleration = SteeringAcceleration::new(Vector3::new(2.0f32, 2.0, 2.0), 2.0f32);
        assert_eq!(4f32, acceleration.calculate_magnitude());
    }

    #[test]
    fn mul_add() {
        let mut acceleration = SteeringAcceleration::new(Vector3::new(1.0f32, 1.0, 1.0), 1.0);
        let acceleration2 = SteeringAcceleration::new(Vector3::new(1.0f32, 1.0, 1.0), 1.0);
        acceleration.mul_add(acceleration2, 2.0);
        assert_eq!(
            SteeringAcceleration::new(Vector3::new(3.0f32, 3.0, 3.0), 3.0),
            acceleration
        );
    }
}