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.MatBuilder;
014import edu.wpi.first.math.MathSharedStore;
015import edu.wpi.first.math.MathUtil;
016import edu.wpi.first.math.Matrix;
017import edu.wpi.first.math.Nat;
018import edu.wpi.first.math.geometry.proto.Rotation2dProto;
019import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
020import edu.wpi.first.math.interpolation.Interpolatable;
021import edu.wpi.first.math.numbers.N2;
022import edu.wpi.first.math.util.Units;
023import edu.wpi.first.units.measure.Angle;
024import edu.wpi.first.util.protobuf.ProtobufSerializable;
025import edu.wpi.first.util.struct.StructSerializable;
026import java.util.Objects;
027
028/**
029 * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
030 */
031@JsonIgnoreProperties(ignoreUnknown = true)
032@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
033public class Rotation2d
034    implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable {
035  /**
036   * A preallocated Rotation2d representing no rotation.
037   *
038   * <p>This exists to avoid allocations for common rotations.
039   */
040  public static final Rotation2d kZero = new Rotation2d();
041
042  /**
043   * A preallocated Rotation2d representing a clockwise rotation by π/2 rad (90°).
044   *
045   * <p>This exists to avoid allocations for common rotations.
046   */
047  public static final Rotation2d kCW_Pi_2 = new Rotation2d(-Math.PI / 2);
048
049  /**
050   * A preallocated Rotation2d representing a clockwise rotation by 90° (π/2 rad).
051   *
052   * <p>This exists to avoid allocations for common rotations.
053   */
054  public static final Rotation2d kCW_90deg = kCW_Pi_2;
055
056  /**
057   * A preallocated Rotation2d representing a counterclockwise rotation by π/2 rad (90°).
058   *
059   * <p>This exists to avoid allocations for common rotations.
060   */
061  public static final Rotation2d kCCW_Pi_2 = new Rotation2d(Math.PI / 2);
062
063  /**
064   * A preallocated Rotation2d representing a counterclockwise rotation by 90° (π/2 rad).
065   *
066   * <p>This exists to avoid allocations for common rotations.
067   */
068  public static final Rotation2d kCCW_90deg = kCCW_Pi_2;
069
070  /**
071   * A preallocated Rotation2d representing a counterclockwise rotation by π rad (180°).
072   *
073   * <p>This exists to avoid allocations for common rotations.
074   */
075  public static final Rotation2d kPi = new Rotation2d(Math.PI);
076
077  /**
078   * A preallocated Rotation2d representing a counterclockwise rotation by 180° (π rad).
079   *
080   * <p>This exists to avoid allocations for common rotations.
081   */
082  public static final Rotation2d k180deg = kPi;
083
084  private final double m_cos;
085  private final double m_sin;
086
087  /** Constructs a Rotation2d with a default angle of 0 degrees. */
088  public Rotation2d() {
089    m_cos = 1.0;
090    m_sin = 0.0;
091  }
092
093  /**
094   * Constructs a Rotation2d with the given radian value.
095   *
096   * @param value The value of the angle in radians.
097   */
098  @JsonCreator
099  public Rotation2d(@JsonProperty(required = true, value = "radians") double value) {
100    m_cos = Math.cos(value);
101    m_sin = Math.sin(value);
102  }
103
104  /**
105   * Constructs a Rotation2d with the given x and y (cosine and sine) components.
106   *
107   * @param x The x component or cosine of the rotation.
108   * @param y The y component or sine of the rotation.
109   */
110  public Rotation2d(double x, double y) {
111    double magnitude = Math.hypot(x, y);
112    if (magnitude > 1e-6) {
113      m_cos = x / magnitude;
114      m_sin = y / magnitude;
115    } else {
116      m_cos = 1.0;
117      m_sin = 0.0;
118      MathSharedStore.reportError(
119          "x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace());
120    }
121  }
122
123  /**
124   * Constructs a Rotation2d with the given angle.
125   *
126   * @param angle The angle of the rotation.
127   */
128  public Rotation2d(Angle angle) {
129    this(angle.in(Radians));
130  }
131
132  /**
133   * Constructs a Rotation2d from a rotation matrix.
134   *
135   * @param rotationMatrix The rotation matrix.
136   * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal.
137   */
138  public Rotation2d(Matrix<N2, N2> rotationMatrix) {
139    final var R = rotationMatrix;
140
141    // Require that the rotation matrix is special orthogonal. This is true if
142    // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
143    if (R.times(R.transpose()).minus(Matrix.eye(Nat.N2())).normF() > 1e-9) {
144      var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n';
145      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
146      throw new IllegalArgumentException(msg);
147    }
148    if (Math.abs(R.det() - 1.0) > 1e-9) {
149      var msg =
150          "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n"
151              + R.getStorage().toString()
152              + '\n';
153      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
154      throw new IllegalArgumentException(msg);
155    }
156
157    // R = [cosθ  −sinθ]
158    //     [sinθ   cosθ]
159    m_cos = R.get(0, 0);
160    m_sin = R.get(1, 0);
161  }
162
163  /**
164   * Constructs and returns a Rotation2d with the given radian value.
165   *
166   * @param radians The value of the angle in radians.
167   * @return The rotation object with the desired angle value.
168   */
169  public static Rotation2d fromRadians(double radians) {
170    return new Rotation2d(radians);
171  }
172
173  /**
174   * Constructs and returns a Rotation2d with the given degree value.
175   *
176   * @param degrees The value of the angle in degrees.
177   * @return The rotation object with the desired angle value.
178   */
179  public static Rotation2d fromDegrees(double degrees) {
180    return new Rotation2d(Math.toRadians(degrees));
181  }
182
183  /**
184   * Constructs and returns a Rotation2d with the given number of rotations.
185   *
186   * @param rotations The value of the angle in rotations.
187   * @return The rotation object with the desired angle value.
188   */
189  public static Rotation2d fromRotations(double rotations) {
190    return new Rotation2d(Units.rotationsToRadians(rotations));
191  }
192
193  /**
194   * Adds two rotations together, with the result being bounded between -π and π.
195   *
196   * <p>For example, <code>Rotation2d.fromDegrees(30).plus(Rotation2d.fromDegrees(60))</code> equals
197   * <code>Rotation2d(Math.PI/2.0)</code>
198   *
199   * @param other The rotation to add.
200   * @return The sum of the two rotations.
201   */
202  public Rotation2d plus(Rotation2d other) {
203    return rotateBy(other);
204  }
205
206  /**
207   * Subtracts the new rotation from the current rotation and returns the new rotation.
208   *
209   * <p>For example, <code>Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100))</code>
210   * equals <code>Rotation2d(-Math.PI/2.0)</code>
211   *
212   * @param other The rotation to subtract.
213   * @return The difference between the two rotations.
214   */
215  public Rotation2d minus(Rotation2d other) {
216    return rotateBy(other.unaryMinus());
217  }
218
219  /**
220   * Takes the inverse of the current rotation. This is simply the negative of the current angular
221   * value.
222   *
223   * @return The inverse of the current rotation.
224   */
225  public Rotation2d unaryMinus() {
226    return new Rotation2d(m_cos, -m_sin);
227  }
228
229  /**
230   * Multiplies the current rotation by a scalar.
231   *
232   * @param scalar The scalar.
233   * @return The new scaled Rotation2d.
234   */
235  public Rotation2d times(double scalar) {
236    return new Rotation2d(getRadians() * scalar);
237  }
238
239  /**
240   * Divides the current rotation by a scalar.
241   *
242   * @param scalar The scalar.
243   * @return The new scaled Rotation2d.
244   */
245  public Rotation2d div(double scalar) {
246    return times(1.0 / scalar);
247  }
248
249  /**
250   * Adds the new rotation to the current rotation using a rotation matrix.
251   *
252   * <p>The matrix multiplication is as follows:
253   *
254   * <pre>
255   * [cos_new]   [other.cos, -other.sin][cos]
256   * [sin_new] = [other.sin,  other.cos][sin]
257   * value_new = atan2(sin_new, cos_new)
258   * </pre>
259   *
260   * @param other The rotation to rotate by.
261   * @return The new rotated Rotation2d.
262   */
263  public Rotation2d rotateBy(Rotation2d other) {
264    return new Rotation2d(
265        m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
266  }
267
268  /**
269   * Returns matrix representation of this rotation.
270   *
271   * @return Matrix representation of this rotation.
272   */
273  public Matrix<N2, N2> toMatrix() {
274    // R = [cosθ  −sinθ]
275    //     [sinθ   cosθ]
276    return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos);
277  }
278
279  /**
280   * Returns the measure of the Rotation2d.
281   *
282   * @return The measure of the Rotation2d.
283   */
284  public Angle getMeasure() {
285    return Radians.of(getRadians());
286  }
287
288  /**
289   * Returns the radian value of the Rotation2d constrained within [-π, π].
290   *
291   * @return The radian value of the Rotation2d constrained within [-π, π].
292   */
293  @JsonProperty
294  public double getRadians() {
295    return Math.atan2(m_sin, m_cos);
296  }
297
298  /**
299   * Returns the degree value of the Rotation2d constrained within [-180, 180].
300   *
301   * @return The degree value of the Rotation2d constrained within [-180, 180].
302   */
303  public double getDegrees() {
304    return Math.toDegrees(getRadians());
305  }
306
307  /**
308   * Returns the number of rotations of the Rotation2d.
309   *
310   * @return The number of rotations of the Rotation2d.
311   */
312  public double getRotations() {
313    return Units.radiansToRotations(getRadians());
314  }
315
316  /**
317   * Returns the cosine of the Rotation2d.
318   *
319   * @return The cosine of the Rotation2d.
320   */
321  public double getCos() {
322    return m_cos;
323  }
324
325  /**
326   * Returns the sine of the Rotation2d.
327   *
328   * @return The sine of the Rotation2d.
329   */
330  public double getSin() {
331    return m_sin;
332  }
333
334  /**
335   * Returns the tangent of the Rotation2d.
336   *
337   * @return The tangent of the Rotation2d.
338   */
339  public double getTan() {
340    return m_sin / m_cos;
341  }
342
343  @Override
344  public String toString() {
345    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", getRadians(), getDegrees());
346  }
347
348  /**
349   * Checks equality between this Rotation2d and another object.
350   *
351   * @param obj The other object.
352   * @return Whether the two objects are equal or not.
353   */
354  @Override
355  public boolean equals(Object obj) {
356    return obj instanceof Rotation2d other
357        && Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
358  }
359
360  @Override
361  public int hashCode() {
362    return Objects.hash(getRadians());
363  }
364
365  @Override
366  public Rotation2d interpolate(Rotation2d endValue, double t) {
367    return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
368  }
369
370  /** Rotation2d protobuf for serialization. */
371  public static final Rotation2dProto proto = new Rotation2dProto();
372
373  /** Rotation2d struct for serialization. */
374  public static final Rotation2dStruct struct = new Rotation2dStruct();
375}