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 /** 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_value; 085 private final double m_cos; 086 private final double m_sin; 087 088 /** Constructs a Rotation2d with a default angle of 0 degrees. */ 089 public Rotation2d() { 090 m_value = 0.0; 091 m_cos = 1.0; 092 m_sin = 0.0; 093 } 094 095 /** 096 * Constructs a Rotation2d with the given radian value. 097 * 098 * @param value The value of the angle in radians. 099 */ 100 @JsonCreator 101 public Rotation2d(@JsonProperty(required = true, value = "radians") double value) { 102 m_value = value; 103 m_cos = Math.cos(value); 104 m_sin = Math.sin(value); 105 } 106 107 /** 108 * Constructs a Rotation2d with the given x and y (cosine and sine) components. 109 * 110 * @param x The x component or cosine of the rotation. 111 * @param y The y component or sine of the rotation. 112 */ 113 public Rotation2d(double x, double y) { 114 double magnitude = Math.hypot(x, y); 115 if (magnitude > 1e-6) { 116 m_sin = y / magnitude; 117 m_cos = x / magnitude; 118 } else { 119 m_sin = 0.0; 120 m_cos = 1.0; 121 } 122 m_value = Math.atan2(m_sin, m_cos); 123 } 124 125 /** 126 * Constructs a Rotation2d with the given angle. 127 * 128 * @param angle The angle of the rotation. 129 */ 130 public Rotation2d(Measure<Angle> angle) { 131 this(angle.in(Radians)); 132 } 133 134 /** 135 * Constructs and returns a Rotation2d with the given radian value. 136 * 137 * @param radians The value of the angle in radians. 138 * @return The rotation object with the desired angle value. 139 */ 140 public static Rotation2d fromRadians(double radians) { 141 return new Rotation2d(radians); 142 } 143 144 /** 145 * Constructs and returns a Rotation2d with the given degree value. 146 * 147 * @param degrees The value of the angle in degrees. 148 * @return The rotation object with the desired angle value. 149 */ 150 public static Rotation2d fromDegrees(double degrees) { 151 return new Rotation2d(Math.toRadians(degrees)); 152 } 153 154 /** 155 * Constructs and returns a Rotation2d with the given number of rotations. 156 * 157 * @param rotations The value of the angle in rotations. 158 * @return The rotation object with the desired angle value. 159 */ 160 public static Rotation2d fromRotations(double rotations) { 161 return new Rotation2d(Units.rotationsToRadians(rotations)); 162 } 163 164 /** 165 * Adds two rotations together, with the result being bounded between -pi and pi. 166 * 167 * <p>For example, <code>Rotation2d.fromDegrees(30).plus(Rotation2d.fromDegrees(60))</code> equals 168 * <code>Rotation2d(Math.PI/2.0)</code> 169 * 170 * @param other The rotation to add. 171 * @return The sum of the two rotations. 172 */ 173 public Rotation2d plus(Rotation2d other) { 174 return rotateBy(other); 175 } 176 177 /** 178 * Subtracts the new rotation from the current rotation and returns the new rotation. 179 * 180 * <p>For example, <code>Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100))</code> 181 * equals <code>Rotation2d(-Math.PI/2.0)</code> 182 * 183 * @param other The rotation to subtract. 184 * @return The difference between the two rotations. 185 */ 186 public Rotation2d minus(Rotation2d other) { 187 return rotateBy(other.unaryMinus()); 188 } 189 190 /** 191 * Takes the inverse of the current rotation. This is simply the negative of the current angular 192 * value. 193 * 194 * @return The inverse of the current rotation. 195 */ 196 public Rotation2d unaryMinus() { 197 return new Rotation2d(-m_value); 198 } 199 200 /** 201 * Multiplies the current rotation by a scalar. 202 * 203 * @param scalar The scalar. 204 * @return The new scaled Rotation2d. 205 */ 206 public Rotation2d times(double scalar) { 207 return new Rotation2d(m_value * scalar); 208 } 209 210 /** 211 * Divides the current rotation by a scalar. 212 * 213 * @param scalar The scalar. 214 * @return The new scaled Rotation2d. 215 */ 216 public Rotation2d div(double scalar) { 217 return times(1.0 / scalar); 218 } 219 220 /** 221 * Adds the new rotation to the current rotation using a rotation matrix. 222 * 223 * <p>The matrix multiplication is as follows: 224 * 225 * <pre> 226 * [cos_new] [other.cos, -other.sin][cos] 227 * [sin_new] = [other.sin, other.cos][sin] 228 * value_new = atan2(sin_new, cos_new) 229 * </pre> 230 * 231 * @param other The rotation to rotate by. 232 * @return The new rotated Rotation2d. 233 */ 234 public Rotation2d rotateBy(Rotation2d other) { 235 return new Rotation2d( 236 m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos); 237 } 238 239 /** 240 * Returns the measure of the Rotation2d. 241 * 242 * @return The measure of the Rotation2d. 243 */ 244 public Measure<Angle> getMeasure() { 245 return Radians.of(getRadians()); 246 } 247 248 /** 249 * Returns the radian value of the Rotation2d. 250 * 251 * @return The radian value of the Rotation2d. 252 * @see edu.wpi.first.math.MathUtil#angleModulus(double) to constrain the angle within (-pi, pi] 253 */ 254 @JsonProperty 255 public double getRadians() { 256 return m_value; 257 } 258 259 /** 260 * Returns the degree value of the Rotation2d. 261 * 262 * @return The degree value of the Rotation2d. 263 * @see edu.wpi.first.math.MathUtil#inputModulus(double, double, double) to constrain the angle 264 * within (-180, 180] 265 */ 266 public double getDegrees() { 267 return Math.toDegrees(m_value); 268 } 269 270 /** 271 * Returns the number of rotations of the Rotation2d. 272 * 273 * @return The number of rotations of the Rotation2d. 274 */ 275 public double getRotations() { 276 return Units.radiansToRotations(m_value); 277 } 278 279 /** 280 * Returns the cosine of the Rotation2d. 281 * 282 * @return The cosine of the Rotation2d. 283 */ 284 public double getCos() { 285 return m_cos; 286 } 287 288 /** 289 * Returns the sine of the Rotation2d. 290 * 291 * @return The sine of the Rotation2d. 292 */ 293 public double getSin() { 294 return m_sin; 295 } 296 297 /** 298 * Returns the tangent of the Rotation2d. 299 * 300 * @return The tangent of the Rotation2d. 301 */ 302 public double getTan() { 303 return m_sin / m_cos; 304 } 305 306 @Override 307 public String toString() { 308 return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value)); 309 } 310 311 /** 312 * Checks equality between this Rotation2d and another object. 313 * 314 * @param obj The other object. 315 * @return Whether the two objects are equal or not. 316 */ 317 @Override 318 public boolean equals(Object obj) { 319 if (obj instanceof Rotation2d) { 320 var other = (Rotation2d) obj; 321 return Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9; 322 } 323 return false; 324 } 325 326 @Override 327 public int hashCode() { 328 return Objects.hash(m_value); 329 } 330 331 @Override 332 public Rotation2d interpolate(Rotation2d endValue, double t) { 333 return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); 334 } 335 336 /** Rotation2d protobuf for serialization. */ 337 public static final Rotation2dProto proto = new Rotation2dProto(); 338 339 /** Rotation2d struct for serialization. */ 340 public static final Rotation2dStruct struct = new Rotation2dStruct(); 341}