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