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