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