Rotation2d.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
package org.wpilib.math.geometry;
 
import static org.wpilib.math.util.UnitConversions.radiansToRotations;
import static org.wpilib.math.util.UnitConversions.rotationsToRadians;
import static org.wpilib.units.Units.Radians;
 
import io.avaje.jsonb.Json;
import java.util.Objects;
import org.wpilib.math.geometry.proto.Rotation2dProto;
import org.wpilib.math.geometry.struct.Rotation2dStruct;
import org.wpilib.math.interpolation.Interpolatable;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.util.MathSharedStore;
import org.wpilib.math.util.Nat;
import org.wpilib.units.measure.Angle;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;

@Json
public final class Rotation2d
    implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable {
  public static final Rotation2d ZERO = new Rotation2d();

  public static final Rotation2d CW_PI_2 = new Rotation2d(-Math.PI / 2);

  public static final Rotation2d CW_90DEG = CW_PI_2;

  public static final Rotation2d CCW_PI_2 = new Rotation2d(Math.PI / 2);

  public static final Rotation2d CCW_90DEG = CCW_PI_2;

  public static final Rotation2d PI = new Rotation2d(Math.PI);

  public static final Rotation2d k180deg = PI;
 
  @Json.Ignore private final double m_cos;
  @Json.Ignore private final double m_sin;

  public Rotation2d() {
    m_cos = 1.0;
    m_sin = 0.0;
  }

  @Json.Creator
  public Rotation2d(@Json.Alias(value = "radians") double value) {
    m_cos = Math.cos(value);
    m_sin = Math.sin(value);
  }

  public Rotation2d(double x, double y) {
    double magnitude = Math.hypot(x, y);
    if (magnitude > 1e-6) {
      m_cos = x / magnitude;
      m_sin = y / magnitude;
    } else {
      m_cos = 1.0;
      m_sin = 0.0;
      MathSharedStore.reportError(
          "x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace());
    }
  }

  public Rotation2d(Angle angle) {
    this(angle.in(Radians));
  }

  public Rotation2d(Matrix<N2, N2> rotationMatrix) {
    final var R = rotationMatrix;
 
    // Require that the rotation matrix is special orthogonal. This is true if
    // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
    if (R.times(R.transpose()).minus(Matrix.eye(Nat.N2())).normF() > 1e-9) {
      var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n';
      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
      throw new IllegalArgumentException(msg);
    }
    if (Math.abs(R.det() - 1.0) > 1e-9) {
      var msg =
          "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n"
              + R.getStorage().toString()
              + '\n';
      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
      throw new IllegalArgumentException(msg);
    }
 
    // R = [cosθ  −sinθ]
    //     [sinθ   cosθ]
    m_cos = R.get(0, 0);
    m_sin = R.get(1, 0);
  }

  public static Rotation2d fromRadians(double radians) {
    return new Rotation2d(radians);
  }

  public static Rotation2d fromDegrees(double degrees) {
    return new Rotation2d(Math.toRadians(degrees));
  }

  public static Rotation2d fromRotations(double rotations) {
    return new Rotation2d(rotationsToRadians(rotations));
  }

  public Rotation2d plus(Rotation2d other) {
    return rotateBy(other);
  }

  public Rotation2d minus(Rotation2d other) {
    return rotateBy(other.unaryMinus());
  }

  public Rotation2d unaryMinus() {
    return new Rotation2d(m_cos, -m_sin);
  }

  public Rotation2d times(double scalar) {
    return new Rotation2d(getRadians() * scalar);
  }

  public Rotation2d div(double scalar) {
    return times(1.0 / scalar);
  }

  public Rotation2d rotateBy(Rotation2d other) {
    return new Rotation2d(
        m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
  }

  public Rotation2d relativeTo(Rotation2d other) {
    return rotateBy(other.unaryMinus());
  }

  public Matrix<N2, N2> toMatrix() {
    // R = [cosθ  −sinθ]
    //     [sinθ   cosθ]
    return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos);
  }

  public Angle getMeasure() {
    return Radians.of(getRadians());
  }

  @Json.Property("radians")
  public double getRadians() {
    return Math.atan2(m_sin, m_cos);
  }

  public double getDegrees() {
    return Math.toDegrees(getRadians());
  }

  public double getRotations() {
    return radiansToRotations(getRadians());
  }

  public double getCos() {
    return m_cos;
  }

  public double getSin() {
    return m_sin;
  }

  public double getTan() {
    return m_sin / m_cos;
  }
 
  @Override
  public String toString() {
    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", getRadians(), getDegrees());
  }

  @Override
  public boolean equals(Object obj) {
    return obj instanceof Rotation2d other
        && Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
  }
 
  @Override
  public int hashCode() {
    return Objects.hash(getRadians());
  }
 
  @Override
  public Rotation2d interpolate(Rotation2d endValue, double t) {
    return plus(endValue.minus(this).times(t));
  }

  public static final Rotation2dProto proto = new Rotation2dProto();

  public static final Rotation2dStruct struct = new Rotation2dStruct();
}