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