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 java.util.Objects;
008
009/** Represents a transformation for a Pose3d. */
010public class Transform3d {
011  private final Translation3d m_translation;
012  private final Rotation3d m_rotation;
013
014  /**
015   * Constructs the transform that maps the initial pose to the final pose.
016   *
017   * @param initial The initial pose for the transformation.
018   * @param last The final pose for the transformation.
019   */
020  public Transform3d(Pose3d initial, Pose3d last) {
021    // We are rotating the difference between the translations
022    // using a clockwise rotation matrix. This transforms the global
023    // delta into a local delta (relative to the initial pose).
024    m_translation =
025        last.getTranslation()
026            .minus(initial.getTranslation())
027            .rotateBy(initial.getRotation().unaryMinus());
028
029    m_rotation = last.getRotation().minus(initial.getRotation());
030  }
031
032  /**
033   * Constructs a transform with the given translation and rotation components.
034   *
035   * @param translation Translational component of the transform.
036   * @param rotation Rotational component of the transform.
037   */
038  public Transform3d(Translation3d translation, Rotation3d rotation) {
039    m_translation = translation;
040    m_rotation = rotation;
041  }
042
043  /** Constructs the identity transform -- maps an initial pose to itself. */
044  public Transform3d() {
045    m_translation = new Translation3d();
046    m_rotation = new Rotation3d();
047  }
048
049  /**
050   * Multiplies the transform by the scalar.
051   *
052   * @param scalar The scalar.
053   * @return The scaled Transform3d.
054   */
055  public Transform3d times(double scalar) {
056    return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar));
057  }
058
059  /**
060   * Divides the transform by the scalar.
061   *
062   * @param scalar The scalar.
063   * @return The scaled Transform3d.
064   */
065  public Transform3d div(double scalar) {
066    return times(1.0 / scalar);
067  }
068
069  /**
070   * Composes two transformations.
071   *
072   * @param other The transform to compose with this one.
073   * @return The composition of the two transformations.
074   */
075  public Transform3d plus(Transform3d other) {
076    return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other));
077  }
078
079  /**
080   * Returns the translation component of the transformation.
081   *
082   * @return The translational component of the transform.
083   */
084  public Translation3d getTranslation() {
085    return m_translation;
086  }
087
088  /**
089   * Returns the X component of the transformation's translation.
090   *
091   * @return The x component of the transformation's translation.
092   */
093  public double getX() {
094    return m_translation.getX();
095  }
096
097  /**
098   * Returns the Y component of the transformation's translation.
099   *
100   * @return The y component of the transformation's translation.
101   */
102  public double getY() {
103    return m_translation.getY();
104  }
105
106  /**
107   * Returns the Z component of the transformation's translation.
108   *
109   * @return The z component of the transformation's translation.
110   */
111  public double getZ() {
112    return m_translation.getZ();
113  }
114
115  /**
116   * Returns the rotational component of the transformation.
117   *
118   * @return Reference to the rotational component of the transform.
119   */
120  public Rotation3d getRotation() {
121    return m_rotation;
122  }
123
124  /**
125   * Invert the transformation. This is useful for undoing a transformation.
126   *
127   * @return The inverted transformation.
128   */
129  public Transform3d inverse() {
130    // We are rotating the difference between the translations
131    // using a clockwise rotation matrix. This transforms the global
132    // delta into a local delta (relative to the initial pose).
133    return new Transform3d(
134        getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
135        getRotation().unaryMinus());
136  }
137
138  @Override
139  public String toString() {
140    return String.format("Transform3d(%s, %s)", m_translation, m_rotation);
141  }
142
143  /**
144   * Checks equality between this Transform3d and another object.
145   *
146   * @param obj The other object.
147   * @return Whether the two objects are equal or not.
148   */
149  @Override
150  public boolean equals(Object obj) {
151    if (obj instanceof Transform3d) {
152      return ((Transform3d) obj).m_translation.equals(m_translation)
153          && ((Transform3d) obj).m_rotation.equals(m_rotation);
154    }
155    return false;
156  }
157
158  @Override
159  public int hashCode() {
160    return Objects.hash(m_translation, m_rotation);
161  }
162}