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.VecBuilder;
012import edu.wpi.first.math.Vector;
013import edu.wpi.first.math.geometry.proto.QuaternionProto;
014import edu.wpi.first.math.geometry.struct.QuaternionStruct;
015import edu.wpi.first.math.numbers.N3;
016import edu.wpi.first.util.protobuf.ProtobufSerializable;
017import edu.wpi.first.util.struct.StructSerializable;
018import java.util.Objects;
019
020/** Represents a quaternion. */
021@JsonIgnoreProperties(ignoreUnknown = true)
022@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
023public class Quaternion implements ProtobufSerializable, StructSerializable {
024  // Scalar r in versor form
025  private final double m_w;
026
027  // Vector v in versor form
028  private final double m_x;
029  private final double m_y;
030  private final double m_z;
031
032  /** Constructs a quaternion with a default angle of 0 degrees. */
033  public Quaternion() {
034    m_w = 1.0;
035    m_x = 0.0;
036    m_y = 0.0;
037    m_z = 0.0;
038  }
039
040  /**
041   * Constructs a quaternion with the given components.
042   *
043   * @param w W component of the quaternion.
044   * @param x X component of the quaternion.
045   * @param y Y component of the quaternion.
046   * @param z Z component of the quaternion.
047   */
048  @JsonCreator
049  public Quaternion(
050      @JsonProperty(required = true, value = "W") double w,
051      @JsonProperty(required = true, value = "X") double x,
052      @JsonProperty(required = true, value = "Y") double y,
053      @JsonProperty(required = true, value = "Z") double z) {
054    m_w = w;
055    m_x = x;
056    m_y = y;
057    m_z = z;
058  }
059
060  /**
061   * Adds another quaternion to this quaternion entrywise.
062   *
063   * @param other The other quaternion.
064   * @return The quaternion sum.
065   */
066  public Quaternion plus(Quaternion other) {
067    return new Quaternion(
068        getW() + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
069  }
070
071  /**
072   * Subtracts another quaternion from this quaternion entrywise.
073   *
074   * @param other The other quaternion.
075   * @return The quaternion difference.
076   */
077  public Quaternion minus(Quaternion other) {
078    return new Quaternion(
079        getW() - other.getW(), getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ());
080  }
081
082  /**
083   * Divides by a scalar.
084   *
085   * @param scalar The value to scale each component by.
086   * @return The scaled quaternion.
087   */
088  public Quaternion divide(double scalar) {
089    return new Quaternion(getW() / scalar, getX() / scalar, getY() / scalar, getZ() / scalar);
090  }
091
092  /**
093   * Multiplies with a scalar.
094   *
095   * @param scalar The value to scale each component by.
096   * @return The scaled quaternion.
097   */
098  public Quaternion times(double scalar) {
099    return new Quaternion(getW() * scalar, getX() * scalar, getY() * scalar, getZ() * scalar);
100  }
101
102  /**
103   * Multiply with another quaternion.
104   *
105   * @param other The other quaternion.
106   * @return The quaternion product.
107   */
108  public Quaternion times(Quaternion other) {
109    // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
110    final var r1 = m_w;
111    final var r2 = other.m_w;
112
113    // v₁ ⋅ v₂
114    double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
115
116    // v₁ x v₂
117    double cross_x = m_y * other.m_z - other.m_y * m_z;
118    double cross_y = other.m_x * m_z - m_x * other.m_z;
119    double cross_z = m_x * other.m_y - other.m_x * m_y;
120
121    return new Quaternion(
122        // r = r₁r₂ − v₁ ⋅ v₂
123        r1 * r2 - dot,
124        // v = r₁v₂ + r₂v₁ + v₁ x v₂
125        r1 * other.m_x + r2 * m_x + cross_x,
126        r1 * other.m_y + r2 * m_y + cross_y,
127        r1 * other.m_z + r2 * m_z + cross_z);
128  }
129
130  @Override
131  public String toString() {
132    return String.format("Quaternion(%s, %s, %s, %s)", getW(), getX(), getY(), getZ());
133  }
134
135  /**
136   * Checks equality between this Quaternion and another object.
137   *
138   * @param obj The other object.
139   * @return Whether the two objects are equal or not.
140   */
141  @Override
142  public boolean equals(Object obj) {
143    if (obj instanceof Quaternion) {
144      var other = (Quaternion) obj;
145
146      return Math.abs(dot(other) - norm() * other.norm()) < 1e-9
147          && Math.abs(norm() - other.norm()) < 1e-9;
148    }
149    return false;
150  }
151
152  @Override
153  public int hashCode() {
154    return Objects.hash(m_w, m_x, m_y, m_z);
155  }
156
157  /**
158   * Returns the conjugate of the quaternion.
159   *
160   * @return The conjugate quaternion.
161   */
162  public Quaternion conjugate() {
163    return new Quaternion(getW(), -getX(), -getY(), -getZ());
164  }
165
166  /**
167   * Returns the elementwise product of two quaternions.
168   *
169   * @param other The other quaternion.
170   * @return The dot product of two quaternions.
171   */
172  public double dot(final Quaternion other) {
173    return getW() * other.getW()
174        + getX() * other.getX()
175        + getY() * other.getY()
176        + getZ() * other.getZ();
177  }
178
179  /**
180   * Returns the inverse of the quaternion.
181   *
182   * @return The inverse quaternion.
183   */
184  public Quaternion inverse() {
185    var norm = norm();
186    return conjugate().divide(norm * norm);
187  }
188
189  /**
190   * Calculates the L2 norm of the quaternion.
191   *
192   * @return The L2 norm.
193   */
194  public double norm() {
195    return Math.sqrt(dot(this));
196  }
197
198  /**
199   * Normalizes the quaternion.
200   *
201   * @return The normalized quaternion.
202   */
203  public Quaternion normalize() {
204    double norm = norm();
205    if (norm == 0.0) {
206      return new Quaternion();
207    } else {
208      return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
209    }
210  }
211
212  /**
213   * Rational power of a quaternion.
214   *
215   * @param t the power to raise this quaternion to.
216   * @return The quaternion power
217   */
218  public Quaternion pow(double t) {
219    // q^t = e^(ln(q^t)) = e^(t * ln(q))
220    return this.log().times(t).exp();
221  }
222
223  /**
224   * Matrix exponential of a quaternion.
225   *
226   * @param adjustment the "Twist" that will be applied to this quaternion.
227   * @return The quaternion product of exp(adjustment) * this
228   */
229  public Quaternion exp(Quaternion adjustment) {
230    return adjustment.exp().times(this);
231  }
232
233  /**
234   * Matrix exponential of a quaternion.
235   *
236   * <p>source: wpimath/algorithms.md
237   *
238   * <p>If this quaternion is in 𝖘𝖔(3) and you are looking for an element of SO(3), use {@link
239   * fromRotationVector}
240   *
241   * @return The Matrix exponential of this quaternion.
242   */
243  public Quaternion exp() {
244    var scalar = Math.exp(getW());
245
246    var axial_magnitude = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
247    var cosine = Math.cos(axial_magnitude);
248
249    double axial_scalar;
250
251    if (axial_magnitude < 1e-9) {
252      // Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(n⁶)
253      var axial_magnitude_sq = axial_magnitude * axial_magnitude;
254      var axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
255      axial_scalar = 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
256    } else {
257      axial_scalar = Math.sin(axial_magnitude) / axial_magnitude;
258    }
259
260    return new Quaternion(
261        cosine * scalar,
262        getX() * axial_scalar * scalar,
263        getY() * axial_scalar * scalar,
264        getZ() * axial_scalar * scalar);
265  }
266
267  /**
268   * Log operator of a quaternion.
269   *
270   * @param end The quaternion to map this quaternion onto.
271   * @return The "Twist" that maps this quaternion to the argument.
272   */
273  public Quaternion log(Quaternion end) {
274    return end.times(this.inverse()).log();
275  }
276
277  /**
278   * The Log operator of a general quaternion.
279   *
280   * <p>source: wpimath/algorithms.md
281   *
282   * <p>If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), use {@link
283   * toRotationVector}
284   *
285   * @return The logarithm of this quaternion.
286   */
287  public Quaternion log() {
288    var norm = norm();
289    var scalar = Math.log(norm);
290
291    var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
292
293    var s_norm = getW() / norm;
294
295    if (Math.abs(s_norm + 1) < 1e-9) {
296      return new Quaternion(scalar, -Math.PI, 0, 0);
297    }
298
299    double v_scalar;
300
301    if (v_norm < 1e-9) {
302      // Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴)
303      v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
304    } else {
305      v_scalar = Math.atan2(v_norm, getW()) / v_norm;
306    }
307
308    return new Quaternion(scalar, v_scalar * getX(), v_scalar * getY(), v_scalar * getZ());
309  }
310
311  /**
312   * Returns W component of the quaternion.
313   *
314   * @return W component of the quaternion.
315   */
316  @JsonProperty(value = "W")
317  public double getW() {
318    return m_w;
319  }
320
321  /**
322   * Returns X component of the quaternion.
323   *
324   * @return X component of the quaternion.
325   */
326  @JsonProperty(value = "X")
327  public double getX() {
328    return m_x;
329  }
330
331  /**
332   * Returns Y component of the quaternion.
333   *
334   * @return Y component of the quaternion.
335   */
336  @JsonProperty(value = "Y")
337  public double getY() {
338    return m_y;
339  }
340
341  /**
342   * Returns Z component of the quaternion.
343   *
344   * @return Z component of the quaternion.
345   */
346  @JsonProperty(value = "Z")
347  public double getZ() {
348    return m_z;
349  }
350
351  /**
352   * Returns the quaternion representation of this rotation vector.
353   *
354   * <p>This is also the exp operator of 𝖘𝖔(3).
355   *
356   * <p>source: wpimath/algorithms.md
357   *
358   * @param rvec The rotation vector.
359   * @return The quaternion representation of this rotation vector.
360   */
361  public static Quaternion fromRotationVector(Vector<N3> rvec) {
362    double theta = rvec.norm();
363
364    double cos = Math.cos(theta / 2);
365
366    double axial_scalar;
367
368    if (theta < 1e-9) {
369      // taylor series expansion of sin(θ/2) / θ = 1/2 - θ²/48 + O(θ⁴)
370      axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
371    } else {
372      axial_scalar = Math.sin(theta / 2) / theta;
373    }
374
375    return new Quaternion(
376        cos,
377        axial_scalar * rvec.get(0, 0),
378        axial_scalar * rvec.get(1, 0),
379        axial_scalar * rvec.get(2, 0));
380  }
381
382  /**
383   * Returns the rotation vector representation of this quaternion.
384   *
385   * <p>This is also the log operator of SO(3).
386   *
387   * @return The rotation vector representation of this quaternion.
388   */
389  public Vector<N3> toRotationVector() {
390    // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
391    // Sound State Representation through Encapsulation of Manifolds"
392    //
393    // https://arxiv.org/pdf/1107.1119.pdf
394    double norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
395
396    double coeff;
397    if (norm < 1e-9) {
398      coeff = 2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW());
399    } else {
400      if (getW() < 0.0) {
401        coeff = 2.0 * Math.atan2(-norm, -getW()) / norm;
402      } else {
403        coeff = 2.0 * Math.atan2(norm, getW()) / norm;
404      }
405    }
406
407    return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ());
408  }
409
410  /** Quaternion protobuf for serialization. */
411  public static final QuaternionProto proto = new QuaternionProto();
412
413  /** Quaternion struct for serialization. */
414  public static final QuaternionStruct struct = new QuaternionStruct();
415}