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