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