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}