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