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 com.fasterxml.jackson.annotation.JsonAutoDetect; 008import com.fasterxml.jackson.annotation.JsonCreator; 009import com.fasterxml.jackson.annotation.JsonIgnoreProperties; 010import com.fasterxml.jackson.annotation.JsonProperty; 011import edu.wpi.first.math.MathSharedStore; 012import edu.wpi.first.math.MathUtil; 013import edu.wpi.first.math.Matrix; 014import edu.wpi.first.math.Nat; 015import edu.wpi.first.math.VecBuilder; 016import edu.wpi.first.math.Vector; 017import edu.wpi.first.math.geometry.proto.Rotation3dProto; 018import edu.wpi.first.math.geometry.struct.Rotation3dStruct; 019import edu.wpi.first.math.interpolation.Interpolatable; 020import edu.wpi.first.math.numbers.N3; 021import edu.wpi.first.util.protobuf.ProtobufSerializable; 022import edu.wpi.first.util.struct.StructSerializable; 023import java.util.Objects; 024import org.ejml.dense.row.factory.DecompositionFactory_DDRM; 025 026/** A rotation in a 3D coordinate frame represented by a quaternion. */ 027@JsonIgnoreProperties(ignoreUnknown = true) 028@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) 029public class Rotation3d 030 implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable { 031 /** 032 * A preallocated Rotation3d representing no rotation. 033 * 034 * <p>This exists to avoid allocations for common rotations. 035 */ 036 public static final Rotation3d kZero = new Rotation3d(); 037 038 private final Quaternion m_q; 039 040 /** Constructs a Rotation3d representing no rotation. */ 041 public Rotation3d() { 042 m_q = new Quaternion(); 043 } 044 045 /** 046 * Constructs a Rotation3d from a quaternion. 047 * 048 * @param q The quaternion. 049 */ 050 @JsonCreator 051 public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) { 052 m_q = q.normalize(); 053 } 054 055 /** 056 * Constructs a Rotation3d from extrinsic roll, pitch, and yaw. 057 * 058 * <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather 059 * than the body frame. 060 * 061 * <p>Angles are measured counterclockwise with the rotation axis pointing "out of the page". If 062 * you point your right thumb along the positive axis direction, your fingers curl in the 063 * direction of positive rotation. 064 * 065 * @param roll The counterclockwise rotation angle around the X axis (roll) in radians. 066 * @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians. 067 * @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians. 068 */ 069 public Rotation3d(double roll, double pitch, double yaw) { 070 // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion 071 double cr = Math.cos(roll * 0.5); 072 double sr = Math.sin(roll * 0.5); 073 074 double cp = Math.cos(pitch * 0.5); 075 double sp = Math.sin(pitch * 0.5); 076 077 double cy = Math.cos(yaw * 0.5); 078 double sy = Math.sin(yaw * 0.5); 079 080 m_q = 081 new Quaternion( 082 cr * cp * cy + sr * sp * sy, 083 sr * cp * cy - cr * sp * sy, 084 cr * sp * cy + sr * cp * sy, 085 cr * cp * sy - sr * sp * cy); 086 } 087 088 /** 089 * Constructs a Rotation3d with the given rotation vector representation. This representation is 090 * equivalent to axis-angle, where the normalized axis is multiplied by the rotation around the 091 * axis in radians. 092 * 093 * @param rvec The rotation vector. 094 */ 095 public Rotation3d(Vector<N3> rvec) { 096 this(rvec, rvec.norm()); 097 } 098 099 /** 100 * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be 101 * normalized. 102 * 103 * @param axis The rotation axis. 104 * @param angleRadians The rotation around the axis in radians. 105 */ 106 public Rotation3d(Vector<N3> axis, double angleRadians) { 107 double norm = axis.norm(); 108 if (norm == 0.0) { 109 m_q = new Quaternion(); 110 return; 111 } 112 113 // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition 114 var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0)); 115 m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0)); 116 } 117 118 /** 119 * Constructs a Rotation3d from a rotation matrix. 120 * 121 * @param rotationMatrix The rotation matrix. 122 * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal. 123 */ 124 public Rotation3d(Matrix<N3, N3> rotationMatrix) { 125 final var R = rotationMatrix; 126 127 // Require that the rotation matrix is special orthogonal. This is true if 128 // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). 129 if (R.times(R.transpose()).minus(Matrix.eye(Nat.N3())).normF() > 1e-9) { 130 var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n'; 131 MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); 132 throw new IllegalArgumentException(msg); 133 } 134 if (Math.abs(R.det() - 1.0) > 1e-9) { 135 var msg = 136 "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n" 137 + R.getStorage().toString() 138 + '\n'; 139 MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); 140 throw new IllegalArgumentException(msg); 141 } 142 143 // Turn rotation matrix into a quaternion 144 // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ 145 double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2); 146 double w; 147 double x; 148 double y; 149 double z; 150 151 if (trace > 0.0) { 152 double s = 0.5 / Math.sqrt(trace + 1.0); 153 w = 0.25 / s; 154 x = (R.get(2, 1) - R.get(1, 2)) * s; 155 y = (R.get(0, 2) - R.get(2, 0)) * s; 156 z = (R.get(1, 0) - R.get(0, 1)) * s; 157 } else { 158 if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) { 159 double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2)); 160 w = (R.get(2, 1) - R.get(1, 2)) / s; 161 x = 0.25 * s; 162 y = (R.get(0, 1) + R.get(1, 0)) / s; 163 z = (R.get(0, 2) + R.get(2, 0)) / s; 164 } else if (R.get(1, 1) > R.get(2, 2)) { 165 double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2)); 166 w = (R.get(0, 2) - R.get(2, 0)) / s; 167 x = (R.get(0, 1) + R.get(1, 0)) / s; 168 y = 0.25 * s; 169 z = (R.get(1, 2) + R.get(2, 1)) / s; 170 } else { 171 double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1)); 172 w = (R.get(1, 0) - R.get(0, 1)) / s; 173 x = (R.get(0, 2) + R.get(2, 0)) / s; 174 y = (R.get(1, 2) + R.get(2, 1)) / s; 175 z = 0.25 * s; 176 } 177 } 178 179 m_q = new Quaternion(w, x, y, z); 180 } 181 182 /** 183 * Constructs a Rotation3d that rotates the initial vector onto the final vector. 184 * 185 * <p>This is useful for turning a 3D vector (final) into an orientation relative to a coordinate 186 * system vector (initial). 187 * 188 * @param initial The initial vector. 189 * @param last The final vector. 190 */ 191 public Rotation3d(Vector<N3> initial, Vector<N3> last) { 192 double dot = initial.dot(last); 193 double normProduct = initial.norm() * last.norm(); 194 double dotNorm = dot / normProduct; 195 196 if (dotNorm > 1.0 - 1E-9) { 197 // If the dot product is 1, the two vectors point in the same direction so 198 // there's no rotation. The default initialization of m_q will work. 199 m_q = new Quaternion(); 200 } else if (dotNorm < -1.0 + 1E-9) { 201 // If the dot product is -1, the two vectors point in opposite directions 202 // so a 180 degree rotation is required. Any orthogonal vector can be used 203 // for it. Q in the QR decomposition is an orthonormal basis, so it 204 // contains orthogonal unit vectors. 205 var X = VecBuilder.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0)); 206 final var qr = DecompositionFactory_DDRM.qr(3, 1); 207 qr.decompose(X.getStorage().getMatrix()); 208 final var Q = qr.getQ(null, false); 209 210 // w = cos(θ/2) = cos(90°) = 0 211 // 212 // For x, y, and z, we use the second column of Q because the first is 213 // parallel instead of orthogonal. The third column would also work. 214 m_q = new Quaternion(0.0, Q.get(0, 1), Q.get(1, 1), Q.get(2, 1)); 215 } else { 216 // initial x last 217 var axis = 218 VecBuilder.fill( 219 initial.get(1, 0) * last.get(2, 0) - last.get(1, 0) * initial.get(2, 0), 220 last.get(0, 0) * initial.get(2, 0) - initial.get(0, 0) * last.get(2, 0), 221 initial.get(0, 0) * last.get(1, 0) - last.get(0, 0) * initial.get(1, 0)); 222 223 // https://stackoverflow.com/a/11741520 224 m_q = 225 new Quaternion(normProduct + dot, axis.get(0, 0), axis.get(1, 0), axis.get(2, 0)) 226 .normalize(); 227 } 228 } 229 230 /** 231 * Adds two rotations together. 232 * 233 * @param other The rotation to add. 234 * @return The sum of the two rotations. 235 */ 236 public Rotation3d plus(Rotation3d other) { 237 return rotateBy(other); 238 } 239 240 /** 241 * Subtracts the new rotation from the current rotation and returns the new rotation. 242 * 243 * @param other The rotation to subtract. 244 * @return The difference between the two rotations. 245 */ 246 public Rotation3d minus(Rotation3d other) { 247 return rotateBy(other.unaryMinus()); 248 } 249 250 /** 251 * Takes the inverse of the current rotation. 252 * 253 * @return The inverse of the current rotation. 254 */ 255 public Rotation3d unaryMinus() { 256 return new Rotation3d(m_q.inverse()); 257 } 258 259 /** 260 * Multiplies the current rotation by a scalar. 261 * 262 * @param scalar The scalar. 263 * @return The new scaled Rotation3d. 264 */ 265 public Rotation3d times(double scalar) { 266 // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp 267 if (m_q.getW() >= 0.0) { 268 return new Rotation3d( 269 VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()), 270 2.0 * scalar * Math.acos(m_q.getW())); 271 } else { 272 return new Rotation3d( 273 VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()), 274 2.0 * scalar * Math.acos(-m_q.getW())); 275 } 276 } 277 278 /** 279 * Divides the current rotation by a scalar. 280 * 281 * @param scalar The scalar. 282 * @return The new scaled Rotation3d. 283 */ 284 public Rotation3d div(double scalar) { 285 return times(1.0 / scalar); 286 } 287 288 /** 289 * Adds the new rotation to the current rotation. The other rotation is applied extrinsically, 290 * which means that it rotates around the global axes. For example, {@code new 291 * Rotation3d(Units.degreesToRadians(90), 0, 0).rotateBy(new Rotation3d(0, 292 * Units.degreesToRadians(45), 0))} rotates by 90 degrees around the +X axis and then by 45 293 * degrees around the global +Y axis. (This is equivalent to {@code new 294 * Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(45), 0)}) 295 * 296 * @param other The extrinsic rotation to rotate by. 297 * @return The new rotated Rotation3d. 298 */ 299 public Rotation3d rotateBy(Rotation3d other) { 300 return new Rotation3d(other.m_q.times(m_q)); 301 } 302 303 /** 304 * Returns the quaternion representation of the Rotation3d. 305 * 306 * @return The quaternion representation of the Rotation3d. 307 */ 308 @JsonProperty(value = "quaternion") 309 public Quaternion getQuaternion() { 310 return m_q; 311 } 312 313 /** 314 * Returns the counterclockwise rotation angle around the X axis (roll) in radians. 315 * 316 * @return The counterclockwise rotation angle around the X axis (roll) in radians. 317 */ 318 public double getX() { 319 final var w = m_q.getW(); 320 final var x = m_q.getX(); 321 final var y = m_q.getY(); 322 final var z = m_q.getZ(); 323 324 // wpimath/algorithms.md 325 final var cxcy = 1.0 - 2.0 * (x * x + y * y); 326 final var sxcy = 2.0 * (w * x + y * z); 327 final var cy_sq = cxcy * cxcy + sxcy * sxcy; 328 if (cy_sq > 1e-20) { 329 return Math.atan2(sxcy, cxcy); 330 } else { 331 return 0.0; 332 } 333 } 334 335 /** 336 * Returns the counterclockwise rotation angle around the Y axis (pitch) in radians. 337 * 338 * @return The counterclockwise rotation angle around the Y axis (pitch) in radians. 339 */ 340 public double getY() { 341 final var w = m_q.getW(); 342 final var x = m_q.getX(); 343 final var y = m_q.getY(); 344 final var z = m_q.getZ(); 345 346 // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion 347 double ratio = 2.0 * (w * y - z * x); 348 if (Math.abs(ratio) >= 1.0) { 349 return Math.copySign(Math.PI / 2.0, ratio); 350 } else { 351 return Math.asin(ratio); 352 } 353 } 354 355 /** 356 * Returns the counterclockwise rotation angle around the Z axis (yaw) in radians. 357 * 358 * @return The counterclockwise rotation angle around the Z axis (yaw) in radians. 359 */ 360 public double getZ() { 361 final var w = m_q.getW(); 362 final var x = m_q.getX(); 363 final var y = m_q.getY(); 364 final var z = m_q.getZ(); 365 366 // wpimath/algorithms.md 367 final var cycz = 1.0 - 2.0 * (y * y + z * z); 368 final var cysz = 2.0 * (w * z + x * y); 369 final var cy_sq = cycz * cycz + cysz * cysz; 370 if (cy_sq > 1e-20) { 371 return Math.atan2(cysz, cycz); 372 } else { 373 return Math.atan2(2.0 * w * z, w * w - z * z); 374 } 375 } 376 377 /** 378 * Returns the axis in the axis-angle representation of this rotation. 379 * 380 * @return The axis in the axis-angle representation. 381 */ 382 public Vector<N3> getAxis() { 383 double norm = 384 Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ()); 385 if (norm == 0.0) { 386 return VecBuilder.fill(0.0, 0.0, 0.0); 387 } else { 388 return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm); 389 } 390 } 391 392 /** 393 * Returns the angle in radians in the axis-angle representation of this rotation. 394 * 395 * @return The angle in radians in the axis-angle representation of this rotation. 396 */ 397 public double getAngle() { 398 double norm = 399 Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ()); 400 return 2.0 * Math.atan2(norm, m_q.getW()); 401 } 402 403 /** 404 * Returns a Rotation2d representing this Rotation3d projected into the X-Y plane. 405 * 406 * @return A Rotation2d representing this Rotation3d projected into the X-Y plane. 407 */ 408 public Rotation2d toRotation2d() { 409 return new Rotation2d(getZ()); 410 } 411 412 @Override 413 public String toString() { 414 return String.format("Rotation3d(%s)", m_q); 415 } 416 417 /** 418 * Checks equality between this Rotation3d and another object. 419 * 420 * @param obj The other object. 421 * @return Whether the two objects are equal or not. 422 */ 423 @Override 424 public boolean equals(Object obj) { 425 if (obj instanceof Rotation3d) { 426 var other = (Rotation3d) obj; 427 return Math.abs(Math.abs(m_q.dot(other.m_q)) - m_q.norm() * other.m_q.norm()) < 1e-9; 428 } 429 return false; 430 } 431 432 @Override 433 public int hashCode() { 434 return Objects.hash(m_q); 435 } 436 437 @Override 438 public Rotation3d interpolate(Rotation3d endValue, double t) { 439 return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); 440 } 441 442 /** Rotation3d protobuf for serialization. */ 443 public static final Rotation3dProto proto = new Rotation3dProto(); 444 445 /** Rotation3d struct for serialization. */ 446 public static final Rotation3dStruct struct = new Rotation3dStruct(); 447}