Source code

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