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 static edu.wpi.first.units.Units.Meters; 008 009import com.fasterxml.jackson.annotation.JsonAutoDetect; 010import com.fasterxml.jackson.annotation.JsonCreator; 011import com.fasterxml.jackson.annotation.JsonIgnoreProperties; 012import com.fasterxml.jackson.annotation.JsonProperty; 013import edu.wpi.first.math.MathUtil; 014import edu.wpi.first.math.geometry.proto.Translation3dProto; 015import edu.wpi.first.math.geometry.struct.Translation3dStruct; 016import edu.wpi.first.math.interpolation.Interpolatable; 017import edu.wpi.first.units.Distance; 018import edu.wpi.first.units.Measure; 019import java.util.Objects; 020 021/** 022 * Represents a translation in 3D space. This object can be used to represent a point or a vector. 023 * 024 * <p>This assumes that you are using conventional mathematical axes. When the robot is at the 025 * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is 026 * positive Z. 027 */ 028@JsonIgnoreProperties(ignoreUnknown = true) 029@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) 030public class Translation3d implements Interpolatable<Translation3d> { 031 private final double m_x; 032 private final double m_y; 033 private final double m_z; 034 035 /** Constructs a Translation3d with X, Y, and Z components equal to zero. */ 036 public Translation3d() { 037 this(0.0, 0.0, 0.0); 038 } 039 040 /** 041 * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. 042 * 043 * @param x The x component of the translation. 044 * @param y The y component of the translation. 045 * @param z The z component of the translation. 046 */ 047 @JsonCreator 048 public Translation3d( 049 @JsonProperty(required = true, value = "x") double x, 050 @JsonProperty(required = true, value = "y") double y, 051 @JsonProperty(required = true, value = "z") double z) { 052 m_x = x; 053 m_y = y; 054 m_z = z; 055 } 056 057 /** 058 * Constructs a Translation3d with the provided distance and angle. This is essentially converting 059 * from polar coordinates to Cartesian coordinates. 060 * 061 * @param distance The distance from the origin to the end of the translation. 062 * @param angle The angle between the x-axis and the translation vector. 063 */ 064 public Translation3d(double distance, Rotation3d angle) { 065 final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle); 066 m_x = rectangular.getX(); 067 m_y = rectangular.getY(); 068 m_z = rectangular.getZ(); 069 } 070 071 /** 072 * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. The 073 * components will be converted to and tracked as meters. 074 * 075 * @param x The x component of the translation. 076 * @param y The y component of the translation. 077 * @param z The z component of the translation. 078 */ 079 public Translation3d(Measure<Distance> x, Measure<Distance> y, Measure<Distance> z) { 080 this(x.in(Meters), y.in(Meters), z.in(Meters)); 081 } 082 083 /** 084 * Calculates the distance between two translations in 3D space. 085 * 086 * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²). 087 * 088 * @param other The translation to compute the distance to. 089 * @return The distance between the two translations. 090 */ 091 public double getDistance(Translation3d other) { 092 return Math.sqrt( 093 Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2)); 094 } 095 096 /** 097 * Returns the X component of the translation. 098 * 099 * @return The X component of the translation. 100 */ 101 @JsonProperty 102 public double getX() { 103 return m_x; 104 } 105 106 /** 107 * Returns the Y component of the translation. 108 * 109 * @return The Y component of the translation. 110 */ 111 @JsonProperty 112 public double getY() { 113 return m_y; 114 } 115 116 /** 117 * Returns the Z component of the translation. 118 * 119 * @return The Z component of the translation. 120 */ 121 @JsonProperty 122 public double getZ() { 123 return m_z; 124 } 125 126 /** 127 * Returns the norm, or distance from the origin to the translation. 128 * 129 * @return The norm of the translation. 130 */ 131 public double getNorm() { 132 return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z); 133 } 134 135 /** 136 * Applies a rotation to the translation in 3D space. 137 * 138 * <p>For example, rotating a Translation3d of <2, 0, 0> by 90 degrees around the Z axis 139 * will return a Translation3d of <0, 2, 0>. 140 * 141 * @param other The rotation to rotate the translation by. 142 * @return The new rotated translation. 143 */ 144 public Translation3d rotateBy(Rotation3d other) { 145 final var p = new Quaternion(0.0, m_x, m_y, m_z); 146 final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse()); 147 return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ()); 148 } 149 150 /** 151 * Returns a Translation2d representing this Translation3d projected into the X-Y plane. 152 * 153 * @return A Translation2d representing this Translation3d projected into the X-Y plane. 154 */ 155 public Translation2d toTranslation2d() { 156 return new Translation2d(m_x, m_y); 157 } 158 159 /** 160 * Returns the sum of two translations in 3D space. 161 * 162 * <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) = 163 * Translation3d{3.0, 8.0, 11.0). 164 * 165 * @param other The translation to add. 166 * @return The sum of the translations. 167 */ 168 public Translation3d plus(Translation3d other) { 169 return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z); 170 } 171 172 /** 173 * Returns the difference between two translations. 174 * 175 * <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) = 176 * Translation3d(4.0, 2.0, 0.0). 177 * 178 * @param other The translation to subtract. 179 * @return The difference between the two translations. 180 */ 181 public Translation3d minus(Translation3d other) { 182 return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z); 183 } 184 185 /** 186 * Returns the inverse of the current translation. This is equivalent to negating all components 187 * of the translation. 188 * 189 * @return The inverse of the current translation. 190 */ 191 public Translation3d unaryMinus() { 192 return new Translation3d(-m_x, -m_y, -m_z); 193 } 194 195 /** 196 * Returns the translation multiplied by a scalar. 197 * 198 * <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0). 199 * 200 * @param scalar The scalar to multiply by. 201 * @return The scaled translation. 202 */ 203 public Translation3d times(double scalar) { 204 return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar); 205 } 206 207 /** 208 * Returns the translation divided by a scalar. 209 * 210 * <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25). 211 * 212 * @param scalar The scalar to multiply by. 213 * @return The reference to the new mutated object. 214 */ 215 public Translation3d div(double scalar) { 216 return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar); 217 } 218 219 @Override 220 public String toString() { 221 return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z); 222 } 223 224 /** 225 * Checks equality between this Translation3d and another object. 226 * 227 * @param obj The other object. 228 * @return Whether the two objects are equal or not. 229 */ 230 @Override 231 public boolean equals(Object obj) { 232 if (obj instanceof Translation3d) { 233 return Math.abs(((Translation3d) obj).m_x - m_x) < 1E-9 234 && Math.abs(((Translation3d) obj).m_y - m_y) < 1E-9 235 && Math.abs(((Translation3d) obj).m_z - m_z) < 1E-9; 236 } 237 return false; 238 } 239 240 @Override 241 public int hashCode() { 242 return Objects.hash(m_x, m_y, m_z); 243 } 244 245 @Override 246 public Translation3d interpolate(Translation3d endValue, double t) { 247 return new Translation3d( 248 MathUtil.interpolate(this.getX(), endValue.getX(), t), 249 MathUtil.interpolate(this.getY(), endValue.getY(), t), 250 MathUtil.interpolate(this.getZ(), endValue.getZ(), t)); 251 } 252 253 public static final Translation3dStruct struct = new Translation3dStruct(); 254 public static final Translation3dProto proto = new Translation3dProto(); 255}