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.Translation3dProto;
017import edu.wpi.first.math.geometry.struct.Translation3dStruct;
018import edu.wpi.first.math.interpolation.Interpolatable;
019import edu.wpi.first.math.numbers.N3;
020import edu.wpi.first.units.measure.Distance;
021import edu.wpi.first.util.protobuf.ProtobufSerializable;
022import edu.wpi.first.util.struct.StructSerializable;
023import java.util.Objects;
024
025/**
026 * Represents a translation in 3D space. This object can be used to represent a point or a vector.
027 *
028 * <p>This assumes that you are using conventional mathematical axes. When the robot is at the
029 * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
030 * positive Z.
031 */
032@JsonIgnoreProperties(ignoreUnknown = true)
033@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
034public class Translation3d
035    implements Interpolatable<Translation3d>, ProtobufSerializable, StructSerializable {
036  /**
037   * A preallocated Translation3d representing the origin.
038   *
039   * <p>This exists to avoid allocations for common translations.
040   */
041  public static final Translation3d kZero = new Translation3d();
042
043  private final double m_x;
044  private final double m_y;
045  private final double m_z;
046
047  /** Constructs a Translation3d with X, Y, and Z components equal to zero. */
048  public Translation3d() {
049    this(0.0, 0.0, 0.0);
050  }
051
052  /**
053   * Constructs a Translation3d with the X, Y, and Z components equal to the provided values.
054   *
055   * @param x The x component of the translation.
056   * @param y The y component of the translation.
057   * @param z The z component of the translation.
058   */
059  @JsonCreator
060  public Translation3d(
061      @JsonProperty(required = true, value = "x") double x,
062      @JsonProperty(required = true, value = "y") double y,
063      @JsonProperty(required = true, value = "z") double z) {
064    m_x = x;
065    m_y = y;
066    m_z = z;
067  }
068
069  /**
070   * Constructs a Translation3d with the provided distance and angle. This is essentially converting
071   * from polar coordinates to Cartesian coordinates.
072   *
073   * @param distance The distance from the origin to the end of the translation.
074   * @param angle The angle between the x-axis and the translation vector.
075   */
076  public Translation3d(double distance, Rotation3d angle) {
077    final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle);
078    m_x = rectangular.getX();
079    m_y = rectangular.getY();
080    m_z = rectangular.getZ();
081  }
082
083  /**
084   * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. The
085   * components will be converted to and tracked as meters.
086   *
087   * @param x The x component of the translation.
088   * @param y The y component of the translation.
089   * @param z The z component of the translation.
090   */
091  public Translation3d(Distance x, Distance y, Distance z) {
092    this(x.in(Meters), y.in(Meters), z.in(Meters));
093  }
094
095  /**
096   * Constructs a 3D translation from a 2D translation in the X-Y plane.
097   *
098   * @param translation The 2D translation.
099   * @see Pose3d#Pose3d(Pose2d)
100   * @see Transform3d#Transform3d(Transform2d)
101   */
102  public Translation3d(Translation2d translation) {
103    this(translation.getX(), translation.getY(), 0.0);
104  }
105
106  /**
107   * Constructs a Translation3d from a 3D translation vector. The values are assumed to be in
108   * meters.
109   *
110   * @param vector The translation vector.
111   */
112  public Translation3d(Vector<N3> vector) {
113    this(vector.get(0), vector.get(1), vector.get(2));
114  }
115
116  /**
117   * Calculates the distance between two translations in 3D space.
118   *
119   * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
120   *
121   * @param other The translation to compute the distance to.
122   * @return The distance between the two translations.
123   */
124  public double getDistance(Translation3d other) {
125    return Math.sqrt(
126        Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
127  }
128
129  /**
130   * Returns the X component of the translation.
131   *
132   * @return The X component of the translation.
133   */
134  @JsonProperty
135  public double getX() {
136    return m_x;
137  }
138
139  /**
140   * Returns the Y component of the translation.
141   *
142   * @return The Y component of the translation.
143   */
144  @JsonProperty
145  public double getY() {
146    return m_y;
147  }
148
149  /**
150   * Returns the Z component of the translation.
151   *
152   * @return The Z component of the translation.
153   */
154  @JsonProperty
155  public double getZ() {
156    return m_z;
157  }
158
159  /**
160   * Returns the X component of the translation in a measure.
161   *
162   * @return The x component of the translation in a measure.
163   */
164  public Distance getMeasureX() {
165    return Meters.of(m_x);
166  }
167
168  /**
169   * Returns the Y component of the translation in a measure.
170   *
171   * @return The y component of the translation in a measure.
172   */
173  public Distance getMeasureY() {
174    return Meters.of(m_y);
175  }
176
177  /**
178   * Returns the Z component of the translation in a measure.
179   *
180   * @return The z component of the translation in a measure.
181   */
182  public Distance getMeasureZ() {
183    return Meters.of(m_z);
184  }
185
186  /**
187   * Returns a 2D translation vector representation of this translation.
188   *
189   * @return A 2D translation vector representation of this translation.
190   */
191  public Vector<N3> toVector() {
192    return VecBuilder.fill(m_x, m_y, m_z);
193  }
194
195  /**
196   * Returns the norm, or distance from the origin to the translation.
197   *
198   * @return The norm of the translation.
199   */
200  public double getNorm() {
201    return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
202  }
203
204  /**
205   * Applies a rotation to the translation in 3D space.
206   *
207   * <p>For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees around the Z axis
208   * will return a Translation3d of &lt;0, 2, 0&gt;.
209   *
210   * @param other The rotation to rotate the translation by.
211   * @return The new rotated translation.
212   */
213  public Translation3d rotateBy(Rotation3d other) {
214    final var p = new Quaternion(0.0, m_x, m_y, m_z);
215    final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse());
216    return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
217  }
218
219  /**
220   * Rotates this translation around another translation in 3D space.
221   *
222   * @param other The other translation to rotate around.
223   * @param rot The rotation to rotate the translation by.
224   * @return The new rotated translation.
225   */
226  public Translation3d rotateAround(Translation3d other, Rotation3d rot) {
227    return this.minus(other).rotateBy(rot).plus(other);
228  }
229
230  /**
231   * Returns a Translation2d representing this Translation3d projected into the X-Y plane.
232   *
233   * @return A Translation2d representing this Translation3d projected into the X-Y plane.
234   */
235  public Translation2d toTranslation2d() {
236    return new Translation2d(m_x, m_y);
237  }
238
239  /**
240   * Returns the sum of two translations in 3D space.
241   *
242   * <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) =
243   * Translation3d{3.0, 8.0, 11.0).
244   *
245   * @param other The translation to add.
246   * @return The sum of the translations.
247   */
248  public Translation3d plus(Translation3d other) {
249    return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z);
250  }
251
252  /**
253   * Returns the difference between two translations.
254   *
255   * <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) =
256   * Translation3d(4.0, 2.0, 0.0).
257   *
258   * @param other The translation to subtract.
259   * @return The difference between the two translations.
260   */
261  public Translation3d minus(Translation3d other) {
262    return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z);
263  }
264
265  /**
266   * Returns the inverse of the current translation. This is equivalent to negating all components
267   * of the translation.
268   *
269   * @return The inverse of the current translation.
270   */
271  public Translation3d unaryMinus() {
272    return new Translation3d(-m_x, -m_y, -m_z);
273  }
274
275  /**
276   * Returns the translation multiplied by a scalar.
277   *
278   * <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0).
279   *
280   * @param scalar The scalar to multiply by.
281   * @return The scaled translation.
282   */
283  public Translation3d times(double scalar) {
284    return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar);
285  }
286
287  /**
288   * Returns the translation divided by a scalar.
289   *
290   * <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25).
291   *
292   * @param scalar The scalar to multiply by.
293   * @return The reference to the new mutated object.
294   */
295  public Translation3d div(double scalar) {
296    return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
297  }
298
299  @Override
300  public String toString() {
301    return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);
302  }
303
304  /**
305   * Checks equality between this Translation3d and another object.
306   *
307   * @param obj The other object.
308   * @return Whether the two objects are equal or not.
309   */
310  @Override
311  public boolean equals(Object obj) {
312    return obj instanceof Translation3d other
313        && Math.abs(other.m_x - m_x) < 1E-9
314        && Math.abs(other.m_y - m_y) < 1E-9
315        && Math.abs(other.m_z - m_z) < 1E-9;
316  }
317
318  @Override
319  public int hashCode() {
320    return Objects.hash(m_x, m_y, m_z);
321  }
322
323  @Override
324  public Translation3d interpolate(Translation3d endValue, double t) {
325    return new Translation3d(
326        MathUtil.interpolate(this.getX(), endValue.getX(), t),
327        MathUtil.interpolate(this.getY(), endValue.getY(), t),
328        MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
329  }
330
331  /** Translation3d protobuf for serialization. */
332  public static final Translation3dProto proto = new Translation3dProto();
333
334  /** Translation3d struct for serialization. */
335  public static final Translation3dStruct struct = new Translation3dStruct();
336}