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