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