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