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/**
010 * A change in distance along a 2D arc since the last pose update. We can use ideas from
011 * differential calculus to create new Pose2d objects from a Twist2d and vice versa.
012 *
013 * <p>A Twist can be used to represent a difference between two poses.
014 */
015public class Twist2d {
016  /** Linear "dx" component. */
017  public double dx;
018
019  /** Linear "dy" component. */
020  public double dy;
021
022  /** Angular "dtheta" component (radians). */
023  public double dtheta;
024
025  public Twist2d() {}
026
027  /**
028   * Constructs a Twist2d with the given values.
029   *
030   * @param dx Change in x direction relative to robot.
031   * @param dy Change in y direction relative to robot.
032   * @param dtheta Change in angle relative to robot.
033   */
034  public Twist2d(double dx, double dy, double dtheta) {
035    this.dx = dx;
036    this.dy = dy;
037    this.dtheta = dtheta;
038  }
039
040  @Override
041  public String toString() {
042    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
043  }
044
045  /**
046   * Checks equality between this Twist2d and another object.
047   *
048   * @param obj The other object.
049   * @return Whether the two objects are equal or not.
050   */
051  @Override
052  public boolean equals(Object obj) {
053    if (obj instanceof Twist2d) {
054      return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
055          && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
056          && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
057    }
058    return false;
059  }
060
061  @Override
062  public int hashCode() {
063    return Objects.hash(dx, dy, dtheta);
064  }
065}