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