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