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

use super::super::SteeringBehavior;
use super::super::HasSteeringBehavior;
use super::super::{SteeringAcceleration, SteeringAccelerationCalculator};
use alga::general::Real;
use alga::general::AbstractModule;

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

/// Seek behavior calculates the maximum linear valocity to reach the target location
#[builder(pattern = "immutable")]
#[derive(Builder)]
pub struct Seek<T>
where
    T: Real,
{
    /// common steering behavior attributes
    pub behavior: RefCell<SteeringBehavior<T>>,
}

impl<T: Real> HasSteeringBehavior<T> for Seek<T> {
    fn get_steering_behavior(&mut self) -> RefMut<SteeringBehavior<T>> {
        self.behavior.borrow_mut()
    }
}

impl<T: Real> SteeringAccelerationCalculator<T> for Seek<T> {
    fn calculate_real_steering(
        &self,
        steering_acceleration: Rc<RefCell<SteeringAcceleration<T>>>,
    ) -> Rc<RefCell<SteeringAcceleration<T>>> {
        let behavior = self.behavior.borrow().clone();
        let position_diff = behavior.target.borrow().get_position() -
            *behavior.owner.borrow().get_position();
        steering_acceleration.borrow_mut().linear =
            position_diff.normalize().multiply_by(match self.behavior
                .borrow()
                .limiter {
                Some(ref a) => (*a).borrow().get_max_linear_acceleration(),
                None => T::one(),
            });
        steering_acceleration.borrow_mut().angular = T::zero();
        steering_acceleration
    }
}

#[cfg(test)]
mod test {
    use super::Seek;
    use super::super::test_common::TestSteerable;
    use super::SteeringBehavior;
    use super::SteeringAccelerationCalculator;
    use super::SteeringAcceleration;
    use nalgebra::Vector3;
    use std::cell::RefCell;
    use std::rc::Rc;

    #[test]
    fn test_same_location() {
        let test_target = TestSteerable::new();
        let test_owner = TestSteerable::new();

        let mut test_behavior = Seek {
            behavior: RefCell::new(SteeringBehavior {
                enabled: true,
                limiter: None,

                target: Rc::new(RefCell::new(test_target)),
                owner: Rc::new(RefCell::new(test_owner)),
            }),
        };

        let sa = Rc::new(RefCell::new(SteeringAcceleration::default()));

        let acceleration_result = test_behavior.calculate_steering(sa);
        // assert_eq!(Vector3::new(0.0f32,0.0,0.0), acceleration_result.linear);
        assert_eq!(0.0f32, acceleration_result.borrow().angular);
    }

    #[test]
    fn test_one_dimension() {
        let mut test_target = TestSteerable::new();
        let mut test_owner = TestSteerable::new();

        test_target.set_position(Vector3::new(1.0f32, 0.0, 0.0));
        test_owner.set_position(Vector3::new(0.0f32, 0.0, 0.0));

        let mut test_behavior = Seek {
            behavior: RefCell::new(SteeringBehavior {
                enabled: true,
                limiter: None,
                target: Rc::new(RefCell::new(test_target)),
                owner: Rc::new(RefCell::new(test_owner)),
            }),
        };

        let sa = Rc::new(RefCell::new(SteeringAcceleration::default()));

        let acceleration_result = test_behavior.calculate_steering(sa);
        assert_eq!(
            Vector3::new(1.0f32, 0.0, 0.0),
            acceleration_result.borrow().linear
        );
        assert_eq!(0.0f32, acceleration_result.borrow().angular);
    }
}