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