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