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