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