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   * Scales 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   * Composes two transformations.
061   *
062   * @param other The transform to compose with this one.
063   * @return The composition of the two transformations.
064   */
065  public Transform2d plus(Transform2d other) {
066    return new Transform2d(new Pose2d(), new Pose2d().transformBy(this).transformBy(other));
067  }
068
069  /**
070   * Returns the translation component of the transformation.
071   *
072   * @return The translational component of the transform.
073   */
074  public Translation2d getTranslation() {
075    return m_translation;
076  }
077
078  /**
079   * Returns the X component of the transformation's translation.
080   *
081   * @return The x component of the transformation's translation.
082   */
083  public double getX() {
084    return m_translation.getX();
085  }
086
087  /**
088   * Returns the Y component of the transformation's translation.
089   *
090   * @return The y component of the transformation's translation.
091   */
092  public double getY() {
093    return m_translation.getY();
094  }
095
096  /**
097   * Returns the rotational component of the transformation.
098   *
099   * @return Reference to the rotational component of the transform.
100   */
101  public Rotation2d getRotation() {
102    return m_rotation;
103  }
104
105  /**
106   * Invert the transformation. This is useful for undoing a transformation.
107   *
108   * @return The inverted transformation.
109   */
110  public Transform2d inverse() {
111    // We are rotating the difference between the translations
112    // using a clockwise rotation matrix. This transforms the global
113    // delta into a local delta (relative to the initial pose).
114    return new Transform2d(
115        getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
116        getRotation().unaryMinus());
117  }
118
119  @Override
120  public String toString() {
121    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
122  }
123
124  /**
125   * Checks equality between this Transform2d and another object.
126   *
127   * @param obj The other object.
128   * @return Whether the two objects are equal or not.
129   */
130  @Override
131  public boolean equals(Object obj) {
132    if (obj instanceof Transform2d) {
133      return ((Transform2d) obj).m_translation.equals(m_translation)
134          && ((Transform2d) obj).m_rotation.equals(m_rotation);
135    }
136    return false;
137  }
138
139  @Override
140  public int hashCode() {
141    return Objects.hash(m_translation, m_rotation);
142  }
143}