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