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 Pose2d. */
010public class Transform2d {
011  private final Translation2d m_translation;
012  private final Rotation2d 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 Transform2d(Pose2d initial, Pose2d 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 Transform2d(Translation2d translation, Rotation2d rotation) {
039    m_translation = translation;
040    m_rotation = rotation;
041  }
042
043  /** Constructs the identity transform -- maps an initial pose to itself. */
044  public Transform2d() {
045    m_translation = new Translation2d();
046    m_rotation = new Rotation2d();
047  }
048
049  /**
050   * Multiplies the transform by the scalar.
051   *
052   * @param scalar The scalar.
053   * @return The scaled Transform2d.
054   */
055  public Transform2d times(double scalar) {
056    return new Transform2d(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 Transform2d.
064   */
065  public Transform2d 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 Transform2d plus(Transform2d other) {
076    return new Transform2d(new Pose2d(), new Pose2d().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 Translation2d 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 rotational component of the transformation.
108   *
109   * @return Reference to the rotational component of the transform.
110   */
111  public Rotation2d getRotation() {
112    return m_rotation;
113  }
114
115  /**
116   * Invert the transformation. This is useful for undoing a transformation.
117   *
118   * @return The inverted transformation.
119   */
120  public Transform2d inverse() {
121    // We are rotating the difference between the translations
122    // using a clockwise rotation matrix. This transforms the global
123    // delta into a local delta (relative to the initial pose).
124    return new Transform2d(
125        getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
126        getRotation().unaryMinus());
127  }
128
129  @Override
130  public String toString() {
131    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
132  }
133
134  /**
135   * Checks equality between this Transform2d and another object.
136   *
137   * @param obj The other object.
138   * @return Whether the two objects are equal or not.
139   */
140  @Override
141  public boolean equals(Object obj) {
142    if (obj instanceof Transform2d) {
143      return ((Transform2d) obj).m_translation.equals(m_translation)
144          && ((Transform2d) obj).m_rotation.equals(m_rotation);
145    }
146    return false;
147  }
148
149  @Override
150  public int hashCode() {
151    return Objects.hash(m_translation, m_rotation);
152  }
153}