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