AligningKinematicAlgorithm implements AligningMotionAlgorithm, Serializable, Cloneable { @Override public double calculate(Vector2D<?, ?> orientation, double angularSpeed, double maxAngularSpeed, Vector2D<?, ?> target) { final double angle = orientation.signedAngle(target); if (Math.abs(angle) <= this.stopAngle) { return 0; } return MathUtil.clamp(angle, -maxAngularSpeed, maxAngularSpeed); } AligningKinematicAlgorithm(double stopAngle); @Pure @Override AligningKinematicAlgorithm clone(); @Override double calculate(Vector2D<?, ?> orientation, double angularSpeed, double maxAngularSpeed, Vector2D<?, ?> target);  }
@Test public void calculate_far() { assertEpsilonEquals(-1.33971, this.aligning.calculate(new Vector2d(-1, 2), 3, 4, new Vector2d(6, 5))); }
@Test public void calculate_close() { assertZero(this.aligning.calculate(new Vector2d(6.1, 4.8), 3, 4, new Vector2d(6, 5))); }