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}