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 3D arc since the last pose update. We can use ideas from
011 * differential calculus to create new Pose3d objects from a Twist3d and vice versa.
012 *
013 * <p>A Twist can be used to represent a difference between two poses.
014 */
015public class Twist3d {
016  /** Linear "dx" component. */
017  public double dx;
018
019  /** Linear "dy" component. */
020  public double dy;
021
022  /** Linear "dz" component. */
023  public double dz;
024
025  /** Rotation vector x component (radians). */
026  public double rx;
027
028  /** Rotation vector y component (radians). */
029  public double ry;
030
031  /** Rotation vector z component (radians). */
032  public double rz;
033
034  public Twist3d() {}
035
036  /**
037   * Constructs a Twist3d with the given values.
038   *
039   * @param dx Change in x direction relative to robot.
040   * @param dy Change in y direction relative to robot.
041   * @param dz Change in z direction relative to robot.
042   * @param rx Rotation vector x component.
043   * @param ry Rotation vector y component.
044   * @param rz Rotation vector z component.
045   */
046  public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) {
047    this.dx = dx;
048    this.dy = dy;
049    this.dz = dz;
050    this.rx = rx;
051    this.ry = ry;
052    this.rz = rz;
053  }
054
055  @Override
056  public String toString() {
057    return String.format(
058        "Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, rX: %.2f, rY: %.2f, rZ: %.2f)",
059        dx, dy, dz, rx, ry, rz);
060  }
061
062  /**
063   * Checks equality between this Twist3d and another object.
064   *
065   * @param obj The other object.
066   * @return Whether the two objects are equal or not.
067   */
068  @Override
069  public boolean equals(Object obj) {
070    if (obj instanceof Twist3d) {
071      return Math.abs(((Twist3d) obj).dx - dx) < 1E-9
072          && Math.abs(((Twist3d) obj).dy - dy) < 1E-9
073          && Math.abs(((Twist3d) obj).dz - dz) < 1E-9
074          && Math.abs(((Twist3d) obj).rx - rx) < 1E-9
075          && Math.abs(((Twist3d) obj).ry - ry) < 1E-9
076          && Math.abs(((Twist3d) obj).rz - rz) < 1E-9;
077    }
078    return false;
079  }
080
081  @Override
082  public int hashCode() {
083    return Objects.hash(dx, dy, dz, rx, ry, rz);
084  }
085}