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.numbers.N3;
014import java.util.Objects;
015
016@JsonIgnoreProperties(ignoreUnknown = true)
017@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
018public class Quaternion {
019  private final double m_r;
020  private final Vector<N3> m_v;
021
022  /** Constructs a quaternion with a default angle of 0 degrees. */
023  public Quaternion() {
024    m_r = 1.0;
025    m_v = VecBuilder.fill(0.0, 0.0, 0.0);
026  }
027
028  /**
029   * Constructs a quaternion with the given components.
030   *
031   * @param w W component of the quaternion.
032   * @param x X component of the quaternion.
033   * @param y Y component of the quaternion.
034   * @param z Z component of the quaternion.
035   */
036  @JsonCreator
037  public Quaternion(
038      @JsonProperty(required = true, value = "W") double w,
039      @JsonProperty(required = true, value = "X") double x,
040      @JsonProperty(required = true, value = "Y") double y,
041      @JsonProperty(required = true, value = "Z") double z) {
042    m_r = w;
043    m_v = VecBuilder.fill(x, y, z);
044  }
045
046  /**
047   * Multiply with another quaternion.
048   *
049   * @param other The other quaternion.
050   * @return The quaternion product.
051   */
052  public Quaternion times(Quaternion other) {
053    // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
054    final var r1 = m_r;
055    final var v1 = m_v;
056    final var r2 = other.m_r;
057    final var v2 = other.m_v;
058
059    // v₁ x v₂
060    var cross =
061        VecBuilder.fill(
062            v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0),
063            v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0),
064            v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0));
065
066    // v = r₁v₂ + r₂v₁ + v₁ x v₂
067    final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
068
069    return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0));
070  }
071
072  @Override
073  public String toString() {
074    return String.format(
075        "Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
076  }
077
078  /**
079   * Checks equality between this Quaternion and another object.
080   *
081   * @param obj The other object.
082   * @return Whether the two objects are equal or not.
083   */
084  @Override
085  public boolean equals(Object obj) {
086    if (obj instanceof Quaternion) {
087      var other = (Quaternion) obj;
088
089      return Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
090    }
091    return false;
092  }
093
094  @Override
095  public int hashCode() {
096    return Objects.hash(m_r, m_v);
097  }
098
099  /**
100   * Returns the inverse of the quaternion.
101   *
102   * @return The inverse quaternion.
103   */
104  public Quaternion inverse() {
105    return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
106  }
107
108  /**
109   * Normalizes the quaternion.
110   *
111   * @return The normalized quaternion.
112   */
113  public Quaternion normalize() {
114    double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
115    if (norm == 0.0) {
116      return new Quaternion();
117    } else {
118      return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
119    }
120  }
121
122  /**
123   * Returns W component of the quaternion.
124   *
125   * @return W component of the quaternion.
126   */
127  @JsonProperty(value = "W")
128  public double getW() {
129    return m_r;
130  }
131
132  /**
133   * Returns X component of the quaternion.
134   *
135   * @return X component of the quaternion.
136   */
137  @JsonProperty(value = "X")
138  public double getX() {
139    return m_v.get(0, 0);
140  }
141
142  /**
143   * Returns Y component of the quaternion.
144   *
145   * @return Y component of the quaternion.
146   */
147  @JsonProperty(value = "Y")
148  public double getY() {
149    return m_v.get(1, 0);
150  }
151
152  /**
153   * Returns Z component of the quaternion.
154   *
155   * @return Z component of the quaternion.
156   */
157  @JsonProperty(value = "Z")
158  public double getZ() {
159    return m_v.get(2, 0);
160  }
161
162  /**
163   * Returns the rotation vector representation of this quaternion.
164   *
165   * <p>This is also the log operator of SO(3).
166   *
167   * @return The rotation vector representation of this quaternion.
168   */
169  public Vector<N3> toRotationVector() {
170    // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
171    // Sound State Representation through Encapsulation of Manifolds"
172    //
173    // https://arxiv.org/pdf/1107.1119.pdf
174    double norm = m_v.norm();
175
176    if (norm < 1e-9) {
177      return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
178    } else {
179      if (getW() < 0.0) {
180        return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
181      } else {
182        return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
183      }
184    }
185  }
186}