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