1 package org.djutils.base;
2
3 /**
4 * AngleUtil has some base methods to deal with angles, such as normalization between -PI and PI, and between 0 and 2*PI. <br>
5 * <br>
6 * Copyright (c) 2020-2024 Delft University of Technology, Jaffalaan 5, 2628 BX Delft, the Netherlands. All rights reserved. See
7 * for project information <a href="https://djutils.org" target="_blank"> https://djutils.org</a>. The DJUTILS project is
8 * distributed under a three-clause BSD-style license, which can be found at
9 * <a href="https://djutils.org/docs/license.html" target="_blank"> https://djutils.org/docs/license.html</a>. <br>
10 * @author <a href="https://www.tudelft.nl/averbraeck">Alexander Verbraeck</a>
11 * @author <a href="https://www.tudelft.nl/pknoppers">Peter Knoppers</a>
12 */
13 public final class AngleUtil
14 {
15 /** Utility constructor. */
16 private AngleUtil()
17 {
18 // Utility constructor
19 }
20
21 /** The value 2π. */
22 public static final double PI2 = 2.0 * Math.PI;
23
24 /**
25 * Normalize an angle (in radians) in a 2π wide interval around π, resulting in angles in the 0 to 2π interval. An
26 * angle value of NaN or Infinity returns NaN.
27 * @param angle double; the angle in radians to normalize
28 * @return double; the normalized angle in radians
29 */
30 public static double normalizeAroundPi(final double angle)
31 {
32 return angle - PI2 * Math.floor(angle / PI2);
33 }
34
35 /**
36 * Normalize an angle (in radians) in a 2π wide interval around 0, resulting in angles in the -π to π interval. An
37 * angle value of NaN or Infinity returns NaN.
38 * @param angle double; the angle in radians to normalize
39 * @return double; the normalized angle in radians
40 */
41 public static double normalizeAroundZero(final double angle)
42 {
43 return angle + PI2 * Math.floor((-angle + Math.PI) / PI2);
44 }
45
46 /**
47 * Return whether two angles (in radians) are less than epsilon apart. The method returns true, for instance when
48 * epsilonEquals(-Math.PI, Math.PI, 1E-6) is called, since the -PI and +PI angles have the same angle.
49 * @param angle1 double; the first angle in radians
50 * @param angle2 double; the second angle in radians
51 * @param epsilon double; the precision
52 * @return whether the two angles are less than epsilon apart
53 */
54 public static boolean epsilonEquals(final double angle1, final double angle2, final double epsilon)
55 {
56 if (Math.abs(angle1 - angle2) < epsilon)
57 {
58 return true;
59 }
60 double diff = Math.abs(normalizeAroundZero(angle2) - normalizeAroundZero(angle1));
61 return diff < epsilon || Math.abs(PI2 - diff) < epsilon;
62 }
63
64 /**
65 * Interpolate between two angles that will first be normalized between 0 and 2π, and take the interpolated angle when
66 * going clockwise from the first angle to the second angle. The fraction indicates where the angle between angle1 and
67 * angle2 will be. A fraction of 0.0 returns angle1, a fraction of 1.0 returns angle2, and a fraction of 0.5 returns the
68 * shortest angle halfway. A fraction of less than 0 or larger than 1 can be used for extrapolation. The resulting angle is
69 * normalized between -π and π. This means that the halfway interpolation between 0 and 2π and between 2π and 0
70 * are both 0, since the two angles are the same. Likewise, the halfway interpolation between -π and π is +/- π
71 * where the sign depends on rounding.
72 * @param angle1 double; the first angle in radians
73 * @param angle2 double; the second angle in radians
74 * @param fraction double; the fraction for interpolation; 0.5 is halfway
75 * @return the normalized angle between angle1 and angle2
76 */
77 public static double interpolateClockwise(final double angle1, final double angle2, final double fraction)
78 {
79 double a1 = normalizeAroundPi(angle1);
80 double a2 = normalizeAroundPi(angle2);
81 if (a2 < a1)
82 {
83 a2 += PI2;
84 }
85 return normalizeAroundZero((1.0 - fraction) * a1 + fraction * a2);
86 }
87
88 /**
89 * Interpolate between two angles taking the <i>shortest way</i>.
90 * @param angle1 double; the first angle in radians
91 * @param angle2 double; the second angle in radians
92 * @param fraction double; the fraction for interpolation; 0.5 is halfway, 0.0 returns angle1, 1.0 returns angle2.
93 * @return double; The interpolated angle, normalized around zero. If any input angle is not normalized around zero the
94 * result will still be normalized, and the exact equality at fraction is 0.0 or 1.0 may not hold. When the
95 * difference between angle1 and angle2 is very close to an odd multiple of PI, the <i>shortest way</i> between
96 * those angles is ill-defined. The result of this method reflects this (fundamental) problem.
97 */
98 public static double interpolateShortest(final double angle1, final double angle2, final double fraction)
99 {
100 double result;
101 if (fraction == 1.0)
102 {
103 result = angle2; // Make sure that the promise of the exact result holds
104 }
105 else
106 {
107 double difference = angle2 - angle1;
108 if (difference < -Math.PI)
109 {
110 difference = difference - PI2 * Math.ceil((difference - Math.PI) / PI2);
111 }
112 if (difference > Math.PI)
113 {
114 difference = difference - PI2 * Math.floor((difference + Math.PI) / PI2);
115 }
116 result = angle1 + fraction * difference;
117 }
118 return normalizeAroundZero(result);
119 }
120
121 }