001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.geometry;
006
007import com.fasterxml.jackson.annotation.JsonAutoDetect;
008import com.fasterxml.jackson.annotation.JsonCreator;
009import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
010import com.fasterxml.jackson.annotation.JsonProperty;
011import edu.wpi.first.math.MathUtil;
012import edu.wpi.first.math.interpolation.Interpolatable;
013import edu.wpi.first.math.util.Units;
014import java.util.Objects;
015
016/**
017 * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
018 *
019 * <p>The angle is continuous, that is if a Rotation2d is constructed with 361 degrees, it will
020 * return 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the
021 * rotations as it sweeps past from 360 to 0 on the second time around.
022 */
023@JsonIgnoreProperties(ignoreUnknown = true)
024@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
025public class Rotation2d implements Interpolatable<Rotation2d> {
026  private final double m_value;
027  private final double m_cos;
028  private final double m_sin;
029
030  /** Constructs a Rotation2d with a default angle of 0 degrees. */
031  public Rotation2d() {
032    m_value = 0.0;
033    m_cos = 1.0;
034    m_sin = 0.0;
035  }
036
037  /**
038   * Constructs a Rotation2d with the given radian value.
039   *
040   * @param value The value of the angle in radians.
041   */
042  @JsonCreator
043  public Rotation2d(@JsonProperty(required = true, value = "radians") double value) {
044    m_value = value;
045    m_cos = Math.cos(value);
046    m_sin = Math.sin(value);
047  }
048
049  /**
050   * Constructs a Rotation2d with the given x and y (cosine and sine) components.
051   *
052   * @param x The x component or cosine of the rotation.
053   * @param y The y component or sine of the rotation.
054   */
055  public Rotation2d(double x, double y) {
056    double magnitude = Math.hypot(x, y);
057    if (magnitude > 1e-6) {
058      m_sin = y / magnitude;
059      m_cos = x / magnitude;
060    } else {
061      m_sin = 0.0;
062      m_cos = 1.0;
063    }
064    m_value = Math.atan2(m_sin, m_cos);
065  }
066
067  /**
068   * Constructs and returns a Rotation2d with the given radian value.
069   *
070   * @param radians The value of the angle in radians.
071   * @return The rotation object with the desired angle value.
072   */
073  public static Rotation2d fromRadians(double radians) {
074    return new Rotation2d(radians);
075  }
076
077  /**
078   * Constructs and returns a Rotation2d with the given degree value.
079   *
080   * @param degrees The value of the angle in degrees.
081   * @return The rotation object with the desired angle value.
082   */
083  public static Rotation2d fromDegrees(double degrees) {
084    return new Rotation2d(Math.toRadians(degrees));
085  }
086
087  /**
088   * Constructs and returns a Rotation2d with the given number of rotations.
089   *
090   * @param rotations The value of the angle in rotations.
091   * @return The rotation object with the desired angle value.
092   */
093  public static Rotation2d fromRotations(double rotations) {
094    return new Rotation2d(Units.rotationsToRadians(rotations));
095  }
096
097  /**
098   * Adds two rotations together, with the result being bounded between -pi and pi.
099   *
100   * <p>For example, <code>Rotation2d.fromDegrees(30).plus(Rotation2d.fromDegrees(60))</code> equals
101   * <code>Rotation2d(Math.PI/2.0)</code>
102   *
103   * @param other The rotation to add.
104   * @return The sum of the two rotations.
105   */
106  public Rotation2d plus(Rotation2d other) {
107    return rotateBy(other);
108  }
109
110  /**
111   * Subtracts the new rotation from the current rotation and returns the new rotation.
112   *
113   * <p>For example, <code>Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100))</code>
114   * equals <code>Rotation2d(-Math.PI/2.0)</code>
115   *
116   * @param other The rotation to subtract.
117   * @return The difference between the two rotations.
118   */
119  public Rotation2d minus(Rotation2d other) {
120    return rotateBy(other.unaryMinus());
121  }
122
123  /**
124   * Takes the inverse of the current rotation. This is simply the negative of the current angular
125   * value.
126   *
127   * @return The inverse of the current rotation.
128   */
129  public Rotation2d unaryMinus() {
130    return new Rotation2d(-m_value);
131  }
132
133  /**
134   * Multiplies the current rotation by a scalar.
135   *
136   * @param scalar The scalar.
137   * @return The new scaled Rotation2d.
138   */
139  public Rotation2d times(double scalar) {
140    return new Rotation2d(m_value * scalar);
141  }
142
143  /**
144   * Divides the current rotation by a scalar.
145   *
146   * @param scalar The scalar.
147   * @return The new scaled Rotation2d.
148   */
149  public Rotation2d div(double scalar) {
150    return times(1.0 / scalar);
151  }
152
153  /**
154   * Adds the new rotation to the current rotation using a rotation matrix.
155   *
156   * <p>The matrix multiplication is as follows:
157   *
158   * <pre>
159   * [cos_new]   [other.cos, -other.sin][cos]
160   * [sin_new] = [other.sin,  other.cos][sin]
161   * value_new = atan2(sin_new, cos_new)
162   * </pre>
163   *
164   * @param other The rotation to rotate by.
165   * @return The new rotated Rotation2d.
166   */
167  public Rotation2d rotateBy(Rotation2d other) {
168    return new Rotation2d(
169        m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
170  }
171
172  /**
173   * Returns the radian value of the Rotation2d.
174   *
175   * @return The radian value of the Rotation2d.
176   * @see edu.wpi.first.math.MathUtil#angleModulus(double) to constrain the angle within (-pi, pi]
177   */
178  @JsonProperty
179  public double getRadians() {
180    return m_value;
181  }
182
183  /**
184   * Returns the degree value of the Rotation2d.
185   *
186   * @return The degree value of the Rotation2d.
187   * @see edu.wpi.first.math.MathUtil#inputModulus(double, double, double) to constrain the angle
188   *     within (-180, 180]
189   */
190  public double getDegrees() {
191    return Math.toDegrees(m_value);
192  }
193
194  /**
195   * Returns the number of rotations of the Rotation2d.
196   *
197   * @return The number of rotations of the Rotation2d.
198   */
199  public double getRotations() {
200    return Units.radiansToRotations(m_value);
201  }
202
203  /**
204   * Returns the cosine of the Rotation2d.
205   *
206   * @return The cosine of the Rotation2d.
207   */
208  public double getCos() {
209    return m_cos;
210  }
211
212  /**
213   * Returns the sine of the Rotation2d.
214   *
215   * @return The sine of the Rotation2d.
216   */
217  public double getSin() {
218    return m_sin;
219  }
220
221  /**
222   * Returns the tangent of the Rotation2d.
223   *
224   * @return The tangent of the Rotation2d.
225   */
226  public double getTan() {
227    return m_sin / m_cos;
228  }
229
230  @Override
231  public String toString() {
232    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
233  }
234
235  /**
236   * Checks equality between this Rotation2d and another object.
237   *
238   * @param obj The other object.
239   * @return Whether the two objects are equal or not.
240   */
241  @Override
242  public boolean equals(Object obj) {
243    if (obj instanceof Rotation2d) {
244      var other = (Rotation2d) obj;
245      return Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
246    }
247    return false;
248  }
249
250  @Override
251  public int hashCode() {
252    return Objects.hash(m_value);
253  }
254
255  @Override
256  public Rotation2d interpolate(Rotation2d endValue, double t) {
257    return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
258  }
259}