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.VecBuilder; 015import edu.wpi.first.math.Vector; 016import edu.wpi.first.math.geometry.proto.Translation3dProto; 017import edu.wpi.first.math.geometry.struct.Translation3dStruct; 018import edu.wpi.first.math.interpolation.Interpolatable; 019import edu.wpi.first.math.numbers.N3; 020import edu.wpi.first.units.measure.Distance; 021import edu.wpi.first.util.protobuf.ProtobufSerializable; 022import edu.wpi.first.util.struct.StructSerializable; 023import java.util.Collection; 024import java.util.Collections; 025import java.util.Comparator; 026import java.util.Objects; 027 028/** 029 * Represents a translation in 3D space. This object can be used to represent a point or a vector. 030 * 031 * <p>This assumes that you are using conventional mathematical axes. When the robot is at the 032 * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is 033 * positive Z. 034 */ 035@JsonIgnoreProperties(ignoreUnknown = true) 036@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) 037public class Translation3d 038 implements Interpolatable<Translation3d>, ProtobufSerializable, StructSerializable { 039 /** 040 * A preallocated Translation3d representing the origin. 041 * 042 * <p>This exists to avoid allocations for common translations. 043 */ 044 public static final Translation3d kZero = new Translation3d(); 045 046 private final double m_x; 047 private final double m_y; 048 private final double m_z; 049 050 /** Constructs a Translation3d with X, Y, and Z components equal to zero. */ 051 public Translation3d() { 052 this(0.0, 0.0, 0.0); 053 } 054 055 /** 056 * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. 057 * 058 * @param x The x component of the translation. 059 * @param y The y component of the translation. 060 * @param z The z component of the translation. 061 */ 062 @JsonCreator 063 public Translation3d( 064 @JsonProperty(required = true, value = "x") double x, 065 @JsonProperty(required = true, value = "y") double y, 066 @JsonProperty(required = true, value = "z") double z) { 067 m_x = x; 068 m_y = y; 069 m_z = z; 070 } 071 072 /** 073 * Constructs a Translation3d with the provided distance and angle. This is essentially converting 074 * from polar coordinates to Cartesian coordinates. 075 * 076 * @param distance The distance from the origin to the end of the translation. 077 * @param angle The angle between the x-axis and the translation vector. 078 */ 079 public Translation3d(double distance, Rotation3d angle) { 080 final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle); 081 m_x = rectangular.getX(); 082 m_y = rectangular.getY(); 083 m_z = rectangular.getZ(); 084 } 085 086 /** 087 * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. The 088 * components will be converted to and tracked as meters. 089 * 090 * @param x The x component of the translation. 091 * @param y The y component of the translation. 092 * @param z The z component of the translation. 093 */ 094 public Translation3d(Distance x, Distance y, Distance z) { 095 this(x.in(Meters), y.in(Meters), z.in(Meters)); 096 } 097 098 /** 099 * Constructs a 3D translation from a 2D translation in the X-Y plane. 100 * 101 * @param translation The 2D translation. 102 * @see Pose3d#Pose3d(Pose2d) 103 * @see Transform3d#Transform3d(Transform2d) 104 */ 105 public Translation3d(Translation2d translation) { 106 this(translation.getX(), translation.getY(), 0.0); 107 } 108 109 /** 110 * Constructs a Translation3d from a 3D translation vector. The values are assumed to be in 111 * meters. 112 * 113 * @param vector The translation vector. 114 */ 115 public Translation3d(Vector<N3> vector) { 116 this(vector.get(0), vector.get(1), vector.get(2)); 117 } 118 119 /** 120 * Calculates the distance between two translations in 3D space. 121 * 122 * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²). 123 * 124 * @param other The translation to compute the distance to. 125 * @return The distance between the two translations. 126 */ 127 public double getDistance(Translation3d other) { 128 return Math.sqrt( 129 Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2)); 130 } 131 132 /** 133 * Returns the X component of the translation. 134 * 135 * @return The X component of the translation. 136 */ 137 @JsonProperty 138 public double getX() { 139 return m_x; 140 } 141 142 /** 143 * Returns the Y component of the translation. 144 * 145 * @return The Y component of the translation. 146 */ 147 @JsonProperty 148 public double getY() { 149 return m_y; 150 } 151 152 /** 153 * Returns the Z component of the translation. 154 * 155 * @return The Z component of the translation. 156 */ 157 @JsonProperty 158 public double getZ() { 159 return m_z; 160 } 161 162 /** 163 * Returns the X component of the translation in a measure. 164 * 165 * @return The x component of the translation in a measure. 166 */ 167 public Distance getMeasureX() { 168 return Meters.of(m_x); 169 } 170 171 /** 172 * Returns the Y component of the translation in a measure. 173 * 174 * @return The y component of the translation in a measure. 175 */ 176 public Distance getMeasureY() { 177 return Meters.of(m_y); 178 } 179 180 /** 181 * Returns the Z component of the translation in a measure. 182 * 183 * @return The z component of the translation in a measure. 184 */ 185 public Distance getMeasureZ() { 186 return Meters.of(m_z); 187 } 188 189 /** 190 * Returns a 2D translation vector representation of this translation. 191 * 192 * @return A 2D translation vector representation of this translation. 193 */ 194 public Vector<N3> toVector() { 195 return VecBuilder.fill(m_x, m_y, m_z); 196 } 197 198 /** 199 * Returns the norm, or distance from the origin to the translation. 200 * 201 * @return The norm of the translation. 202 */ 203 public double getNorm() { 204 return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z); 205 } 206 207 /** 208 * Applies a rotation to the translation in 3D space. 209 * 210 * <p>For example, rotating a Translation3d of <2, 0, 0> by 90 degrees around the Z axis 211 * will return a Translation3d of <0, 2, 0>. 212 * 213 * @param other The rotation to rotate the translation by. 214 * @return The new rotated translation. 215 */ 216 public Translation3d rotateBy(Rotation3d other) { 217 final var p = new Quaternion(0.0, m_x, m_y, m_z); 218 final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse()); 219 return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ()); 220 } 221 222 /** 223 * Rotates this translation around another translation in 3D space. 224 * 225 * @param other The other translation to rotate around. 226 * @param rot The rotation to rotate the translation by. 227 * @return The new rotated translation. 228 */ 229 public Translation3d rotateAround(Translation3d other, Rotation3d rot) { 230 return this.minus(other).rotateBy(rot).plus(other); 231 } 232 233 /** 234 * Returns a Translation2d representing this Translation3d projected into the X-Y plane. 235 * 236 * @return A Translation2d representing this Translation3d projected into the X-Y plane. 237 */ 238 public Translation2d toTranslation2d() { 239 return new Translation2d(m_x, m_y); 240 } 241 242 /** 243 * Returns the sum of two translations in 3D space. 244 * 245 * <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) = 246 * Translation3d{3.0, 8.0, 11.0). 247 * 248 * @param other The translation to add. 249 * @return The sum of the translations. 250 */ 251 public Translation3d plus(Translation3d other) { 252 return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z); 253 } 254 255 /** 256 * Returns the difference between two translations. 257 * 258 * <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) = 259 * Translation3d(4.0, 2.0, 0.0). 260 * 261 * @param other The translation to subtract. 262 * @return The difference between the two translations. 263 */ 264 public Translation3d minus(Translation3d other) { 265 return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z); 266 } 267 268 /** 269 * Returns the inverse of the current translation. This is equivalent to negating all components 270 * of the translation. 271 * 272 * @return The inverse of the current translation. 273 */ 274 public Translation3d unaryMinus() { 275 return new Translation3d(-m_x, -m_y, -m_z); 276 } 277 278 /** 279 * Returns the translation multiplied by a scalar. 280 * 281 * <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0). 282 * 283 * @param scalar The scalar to multiply by. 284 * @return The scaled translation. 285 */ 286 public Translation3d times(double scalar) { 287 return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar); 288 } 289 290 /** 291 * Returns the translation divided by a scalar. 292 * 293 * <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25). 294 * 295 * @param scalar The scalar to multiply by. 296 * @return The reference to the new mutated object. 297 */ 298 public Translation3d div(double scalar) { 299 return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar); 300 } 301 302 /** 303 * Returns the nearest Translation3d from a collection of translations. 304 * 305 * @param translations The collection of translations to find the nearest. 306 * @return The nearest Translation3d from the collection. 307 */ 308 public Translation3d nearest(Collection<Translation3d> translations) { 309 return Collections.min(translations, Comparator.comparing(this::getDistance)); 310 } 311 312 @Override 313 public String toString() { 314 return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z); 315 } 316 317 /** 318 * Checks equality between this Translation3d and another object. 319 * 320 * @param obj The other object. 321 * @return Whether the two objects are equal or not. 322 */ 323 @Override 324 public boolean equals(Object obj) { 325 return obj instanceof Translation3d other 326 && Math.abs(other.m_x - m_x) < 1E-9 327 && Math.abs(other.m_y - m_y) < 1E-9 328 && Math.abs(other.m_z - m_z) < 1E-9; 329 } 330 331 @Override 332 public int hashCode() { 333 return Objects.hash(m_x, m_y, m_z); 334 } 335 336 @Override 337 public Translation3d interpolate(Translation3d endValue, double t) { 338 return new Translation3d( 339 MathUtil.interpolate(this.getX(), endValue.getX(), t), 340 MathUtil.interpolate(this.getY(), endValue.getY(), t), 341 MathUtil.interpolate(this.getZ(), endValue.getZ(), t)); 342 } 343 344 /** Translation3d protobuf for serialization. */ 345 public static final Translation3dProto proto = new Translation3dProto(); 346 347 /** Translation3d struct for serialization. */ 348 public static final Translation3dStruct struct = new Translation3dStruct(); 349}