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