diff --git a/src/turtle.rs b/src/turtle.rs index df0b5f74..f920d315 100644 --- a/src/turtle.rs +++ b/src/turtle.rs @@ -989,9 +989,6 @@ impl Turtle { /// /// If the coordinates are the same as the turtle's current position, no rotation takes place. /// Always rotates the least amount necessary in order to face the given point. - /// - /// ## UNSTABLE - /// This feature is currently unstable and completely buggy. Do not use it until it is fixed. pub fn turn_towards(&mut self, target: Point) { let target_x = target[0]; let target_y = target[1]; @@ -1000,19 +997,24 @@ impl Turtle { let x = position[0]; let y = position[1]; + // If the target is (approximately) on the turtle don't turn if (target_x - x).abs() < 0.1 && (target_y - y).abs() < 0.1 { return; } let heading = self.window.fetch_turtle().heading; + // Calculate the target angle to reach let angle = (target_y - y).atan2(target_x - x); let angle = Radians::from_radians_value(angle); + // Calculate how much turning will be needed (angle - heading) + // And clamp it make sure the turtle doesn't turn more than 360 degrees let angle = (angle - heading) % radians::TWO_PI; // Try to rotate as little as possible let angle = if angle.abs() > radians::PI { - // Using signum to deal with negative angles properly - angle.signum()*(radians::TWO_PI - angle.abs()) + // Use signum to make sure the angle has the right sign + // And the turtle turns the right way + -angle.signum()*(radians::TWO_PI - angle.abs()) } else { angle @@ -1156,4 +1158,22 @@ mod tests { assert_eq!(turtle.position()[1].round(), 100.0); assert_eq!(turtle.heading(), 51.0); } + + #[test] + fn turn_towards() { + let mut turtle = Turtle::new(); + + // Turn from each cardinal direction to each cardinal direction + for n in 0..16 as u32 { + let original_angle = radians::TWO_PI * n as f64 / 16.0; + for i in 0..16 as u32 { + turtle.turn_towards([original_angle.cos(), original_angle.sin()]); + assert_eq!(turtle.heading().ceil(), original_angle.to_degrees().ceil()); + + let target_angle = radians::TWO_PI * i as f64 / 16.0; + turtle.turn_towards([target_angle.cos(), target_angle.sin()]); + assert_eq!(turtle.heading().ceil(), target_angle.to_degrees().ceil()); + } + } + } }