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