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.Meters; 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.VecBuilder; 015import edu.wpi.first.math.Vector; 016import edu.wpi.first.math.geometry.proto.Translation2dProto; 017import edu.wpi.first.math.geometry.struct.Translation2dStruct; 018import edu.wpi.first.math.interpolation.Interpolatable; 019import edu.wpi.first.math.numbers.N2; 020import edu.wpi.first.units.Distance; 021import edu.wpi.first.units.Measure; 022import edu.wpi.first.util.protobuf.ProtobufSerializable; 023import edu.wpi.first.util.struct.StructSerializable; 024import java.util.Collections; 025import java.util.Comparator; 026import java.util.List; 027import java.util.Objects; 028 029/** 030 * Represents a translation in 2D space. This object can be used to represent a point or a vector. 031 * 032 * <p>This assumes that you are using conventional mathematical axes. When the robot is at the 033 * origin facing in the positive X direction, forward is positive X and left is positive Y. 034 */ 035@JsonIgnoreProperties(ignoreUnknown = true) 036@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) 037public class Translation2d 038 implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable { 039 /** 040 * A preallocated Translation2d representing the origin. 041 * 042 * <p>This exists to avoid allocations for common translations. 043 */ 044 public static final Translation2d kZero = new Translation2d(); 045 046 private final double m_x; 047 private final double m_y; 048 049 /** Constructs a Translation2d with X and Y components equal to zero. */ 050 public Translation2d() { 051 this(0.0, 0.0); 052 } 053 054 /** 055 * Constructs a Translation2d with the X and Y components equal to the provided values. 056 * 057 * @param x The x component of the translation. 058 * @param y The y component of the translation. 059 */ 060 @JsonCreator 061 public Translation2d( 062 @JsonProperty(required = true, value = "x") double x, 063 @JsonProperty(required = true, value = "y") double y) { 064 m_x = x; 065 m_y = y; 066 } 067 068 /** 069 * Constructs a Translation2d with the provided distance and angle. This is essentially converting 070 * from polar coordinates to Cartesian coordinates. 071 * 072 * @param distance The distance from the origin to the end of the translation. 073 * @param angle The angle between the x-axis and the translation vector. 074 */ 075 public Translation2d(double distance, Rotation2d angle) { 076 m_x = distance * angle.getCos(); 077 m_y = distance * angle.getSin(); 078 } 079 080 /** 081 * Constructs a Translation2d with the X and Y components equal to the provided values. The X and 082 * Y components will be converted to and tracked as meters. 083 * 084 * @param x The x component of the translation. 085 * @param y The y component of the translation. 086 */ 087 public Translation2d(Measure<Distance> x, Measure<Distance> y) { 088 this(x.in(Meters), y.in(Meters)); 089 } 090 091 /** 092 * Constructs a Translation2d from the provided translation vector's X and Y components. The 093 * values are assumed to be in meters. 094 * 095 * @param vector The translation vector to represent. 096 */ 097 public Translation2d(Vector<N2> vector) { 098 this(vector.get(0), vector.get(1)); 099 } 100 101 /** 102 * Calculates the distance between two translations in 2D space. 103 * 104 * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²). 105 * 106 * @param other The translation to compute the distance to. 107 * @return The distance between the two translations. 108 */ 109 public double getDistance(Translation2d other) { 110 return Math.hypot(other.m_x - m_x, other.m_y - m_y); 111 } 112 113 /** 114 * Returns the X component of the translation. 115 * 116 * @return The X component of the translation. 117 */ 118 @JsonProperty 119 public double getX() { 120 return m_x; 121 } 122 123 /** 124 * Returns the Y component of the translation. 125 * 126 * @return The Y component of the translation. 127 */ 128 @JsonProperty 129 public double getY() { 130 return m_y; 131 } 132 133 /** 134 * Returns a vector representation of this translation. 135 * 136 * @return A Vector representation of this translation. 137 */ 138 public Vector<N2> toVector() { 139 return VecBuilder.fill(m_x, m_y); 140 } 141 142 /** 143 * Returns the norm, or distance from the origin to the translation. 144 * 145 * @return The norm of the translation. 146 */ 147 public double getNorm() { 148 return Math.hypot(m_x, m_y); 149 } 150 151 /** 152 * Returns the angle this translation forms with the positive X axis. 153 * 154 * @return The angle of the translation 155 */ 156 public Rotation2d getAngle() { 157 return new Rotation2d(m_x, m_y); 158 } 159 160 /** 161 * Applies a rotation to the translation in 2D space. 162 * 163 * <p>This multiplies the translation vector by a counterclockwise rotation matrix of the given 164 * angle. 165 * 166 * <pre> 167 * [x_new] [other.cos, -other.sin][x] 168 * [y_new] = [other.sin, other.cos][y] 169 * </pre> 170 * 171 * <p>For example, rotating a Translation2d of <2, 0> by 90 degrees will return a 172 * Translation2d of <0, 2>. 173 * 174 * @param other The rotation to rotate the translation by. 175 * @return The new rotated translation. 176 */ 177 public Translation2d rotateBy(Rotation2d other) { 178 return new Translation2d( 179 m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos()); 180 } 181 182 /** 183 * Rotates this translation around another translation in 2D space. 184 * 185 * <pre> 186 * [x_new] [rot.cos, -rot.sin][x - other.x] [other.x] 187 * [y_new] = [rot.sin, rot.cos][y - other.y] + [other.y] 188 * </pre> 189 * 190 * @param other The other translation to rotate around. 191 * @param rot The rotation to rotate the translation by. 192 * @return The new rotated translation. 193 */ 194 public Translation2d rotateAround(Translation2d other, Rotation2d rot) { 195 return new Translation2d( 196 (m_x - other.getX()) * rot.getCos() - (m_y - other.getY()) * rot.getSin() + other.getX(), 197 (m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY()); 198 } 199 200 /** 201 * Returns the sum of two translations in 2D space. 202 * 203 * <p>For example, Translation3d(1.0, 2.5) + Translation3d(2.0, 5.5) = Translation3d{3.0, 8.0). 204 * 205 * @param other The translation to add. 206 * @return The sum of the translations. 207 */ 208 public Translation2d plus(Translation2d other) { 209 return new Translation2d(m_x + other.m_x, m_y + other.m_y); 210 } 211 212 /** 213 * Returns the difference between two translations. 214 * 215 * <p>For example, Translation2d(5.0, 4.0) - Translation2d(1.0, 2.0) = Translation2d(4.0, 2.0). 216 * 217 * @param other The translation to subtract. 218 * @return The difference between the two translations. 219 */ 220 public Translation2d minus(Translation2d other) { 221 return new Translation2d(m_x - other.m_x, m_y - other.m_y); 222 } 223 224 /** 225 * Returns the inverse of the current translation. This is equivalent to rotating by 180 degrees, 226 * flipping the point over both axes, or negating all components of the translation. 227 * 228 * @return The inverse of the current translation. 229 */ 230 public Translation2d unaryMinus() { 231 return new Translation2d(-m_x, -m_y); 232 } 233 234 /** 235 * Returns the translation multiplied by a scalar. 236 * 237 * <p>For example, Translation2d(2.0, 2.5) * 2 = Translation2d(4.0, 5.0). 238 * 239 * @param scalar The scalar to multiply by. 240 * @return The scaled translation. 241 */ 242 public Translation2d times(double scalar) { 243 return new Translation2d(m_x * scalar, m_y * scalar); 244 } 245 246 /** 247 * Returns the translation divided by a scalar. 248 * 249 * <p>For example, Translation3d(2.0, 2.5) / 2 = Translation3d(1.0, 1.25). 250 * 251 * @param scalar The scalar to multiply by. 252 * @return The reference to the new mutated object. 253 */ 254 public Translation2d div(double scalar) { 255 return new Translation2d(m_x / scalar, m_y / scalar); 256 } 257 258 /** 259 * Returns the nearest Translation2d from a list of translations. 260 * 261 * @param translations The list of translations. 262 * @return The nearest Translation2d from the list. 263 */ 264 public Translation2d nearest(List<Translation2d> translations) { 265 return Collections.min(translations, Comparator.comparing(this::getDistance)); 266 } 267 268 @Override 269 public String toString() { 270 return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y); 271 } 272 273 /** 274 * Checks equality between this Translation2d and another object. 275 * 276 * @param obj The other object. 277 * @return Whether the two objects are equal or not. 278 */ 279 @Override 280 public boolean equals(Object obj) { 281 return obj instanceof Translation2d other 282 && Math.abs(other.m_x - m_x) < 1E-9 283 && Math.abs(other.m_y - m_y) < 1E-9; 284 } 285 286 @Override 287 public int hashCode() { 288 return Objects.hash(m_x, m_y); 289 } 290 291 @Override 292 public Translation2d interpolate(Translation2d endValue, double t) { 293 return new Translation2d( 294 MathUtil.interpolate(this.getX(), endValue.getX(), t), 295 MathUtil.interpolate(this.getY(), endValue.getY(), t)); 296 } 297 298 /** Translation2d protobuf for serialization. */ 299 public static final Translation2dProto proto = new Translation2dProto(); 300 301 /** Translation2d struct for serialization. */ 302 public static final Translation2dStruct struct = new Translation2dStruct(); 303}