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 * Returns the sum of two translations in 2D space. 184 * 185 * <p>For example, Translation3d(1.0, 2.5) + Translation3d(2.0, 5.5) = Translation3d{3.0, 8.0). 186 * 187 * @param other The translation to add. 188 * @return The sum of the translations. 189 */ 190 public Translation2d plus(Translation2d other) { 191 return new Translation2d(m_x + other.m_x, m_y + other.m_y); 192 } 193 194 /** 195 * Returns the difference between two translations. 196 * 197 * <p>For example, Translation2d(5.0, 4.0) - Translation2d(1.0, 2.0) = Translation2d(4.0, 2.0). 198 * 199 * @param other The translation to subtract. 200 * @return The difference between the two translations. 201 */ 202 public Translation2d minus(Translation2d other) { 203 return new Translation2d(m_x - other.m_x, m_y - other.m_y); 204 } 205 206 /** 207 * Returns the inverse of the current translation. This is equivalent to rotating by 180 degrees, 208 * flipping the point over both axes, or negating all components of the translation. 209 * 210 * @return The inverse of the current translation. 211 */ 212 public Translation2d unaryMinus() { 213 return new Translation2d(-m_x, -m_y); 214 } 215 216 /** 217 * Returns the translation multiplied by a scalar. 218 * 219 * <p>For example, Translation2d(2.0, 2.5) * 2 = Translation2d(4.0, 5.0). 220 * 221 * @param scalar The scalar to multiply by. 222 * @return The scaled translation. 223 */ 224 public Translation2d times(double scalar) { 225 return new Translation2d(m_x * scalar, m_y * scalar); 226 } 227 228 /** 229 * Returns the translation divided by a scalar. 230 * 231 * <p>For example, Translation3d(2.0, 2.5) / 2 = Translation3d(1.0, 1.25). 232 * 233 * @param scalar The scalar to multiply by. 234 * @return The reference to the new mutated object. 235 */ 236 public Translation2d div(double scalar) { 237 return new Translation2d(m_x / scalar, m_y / scalar); 238 } 239 240 /** 241 * Returns the nearest Translation2d from a list of translations. 242 * 243 * @param translations The list of translations. 244 * @return The nearest Translation2d from the list. 245 */ 246 public Translation2d nearest(List<Translation2d> translations) { 247 return Collections.min(translations, Comparator.comparing(this::getDistance)); 248 } 249 250 @Override 251 public String toString() { 252 return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y); 253 } 254 255 /** 256 * Checks equality between this Translation2d and another object. 257 * 258 * @param obj The other object. 259 * @return Whether the two objects are equal or not. 260 */ 261 @Override 262 public boolean equals(Object obj) { 263 if (obj instanceof Translation2d) { 264 return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9 265 && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9; 266 } 267 return false; 268 } 269 270 @Override 271 public int hashCode() { 272 return Objects.hash(m_x, m_y); 273 } 274 275 @Override 276 public Translation2d interpolate(Translation2d endValue, double t) { 277 return new Translation2d( 278 MathUtil.interpolate(this.getX(), endValue.getX(), t), 279 MathUtil.interpolate(this.getY(), endValue.getY(), t)); 280 } 281 282 /** Translation2d protobuf for serialization. */ 283 public static final Translation2dProto proto = new Translation2dProto(); 284 285 /** Translation2d struct for serialization. */ 286 public static final Translation2dStruct struct = new Translation2dStruct(); 287}