mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[wpimath] Expand Quaternion class with additional operators (#5600)
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
@@ -351,3 +351,109 @@ When calculating a\_z:
|
||||
```
|
||||
|
||||
Note that this reuses the cos(a\_y) cos(a\_z) and cos(a\_y) sin(a\_z) terms needed to calculate a\_z.
|
||||
|
||||
## Quaternion Exponential
|
||||
|
||||
We will take it as given that a quaternion has scalar and vector components `𝑞 = s + 𝑣⃗`, with vector component 𝑣⃗ consisting of a unit vector and magnitude `𝑣⃗ = θ * v̂`.
|
||||
|
||||
```
|
||||
𝑞 = s + 𝑣⃗
|
||||
|
||||
𝑣⃗ = θ * v̂
|
||||
|
||||
exp(𝑞) = exp(s + 𝑣⃗)
|
||||
exp(𝑞) = exp(s) * exp(𝑣⃗)
|
||||
exp(𝑞) = exp(s) * exp(θ * v̂)
|
||||
```
|
||||
|
||||
Applying euler's identity:
|
||||
|
||||
```
|
||||
exp(θ * v̂) = cos(θ) + sin(θ) * v̂
|
||||
```
|
||||
|
||||
Gives us:
|
||||
```
|
||||
exp(𝑞) = exp(s) * [cos(θ) + sin(θ) * v̂]
|
||||
```
|
||||
|
||||
Rearranging `𝑣⃗ = θ * v̂` we can solve for v̂: `v̂ = 𝑣⃗ / θ`
|
||||
|
||||
```
|
||||
exp(𝑞) = exp(s) * [cos(θ) + sin(θ) / θ * 𝑣⃗]
|
||||
```
|
||||
|
||||
## Quaternion Logarithm
|
||||
|
||||
We will take it as a given that for a given quaternion of the form `𝑞 = s + 𝑣⃗`, we can calculate the exponential: `exp(𝑞) = exp(s) * [cos(θ) + sin(θ) / θ * 𝑣⃗]` where `θ = ||𝑣⃗||`.
|
||||
|
||||
Additionally, `exp(log(𝑞)) = q` for a given value of `log(𝑞)`. There are multiple solutions to `log(𝑞)` caused by the imaginary axes in 𝑣⃗, discussed here: https://en.wikipedia.org/wiki/Complex_logarithm
|
||||
|
||||
We will demonstrate the principal solution of `log(𝑞)` satisfying `exp(log(𝑞)) = q`.
|
||||
This being `log(𝑞) = log(||𝑞||) + atan2(θ, s) / θ * 𝑣⃗`, is the principal solution to `log(𝑞)` because the function `atan2(θ, s)` returns the principal value corresponding to its arguments.
|
||||
|
||||
Proof: `log(𝑞) = log(||𝑞||) + atan2(θ, s) / θ * 𝑣⃗` satisfies `exp(log(𝑞)) = q`.
|
||||
|
||||
```
|
||||
exp(log(𝑞)) = exp(log(||𝑞||) + atan2(θ, s) / θ * 𝑣⃗)
|
||||
|
||||
|
||||
exp(log(𝑞)) = exp(log(||𝑞||)) * exp(atan2(θ, s) / θ * 𝑣⃗)
|
||||
|
||||
Substitutions:
|
||||
𝑣⃗ = θ * v̂:
|
||||
exp(log(||𝑞||)) = ||𝑞||
|
||||
exp(log(𝑞)) = ||𝑞|| * exp(atan2(θ, s) * v̂)
|
||||
|
||||
exp(log(𝑞)) = ||𝑞|| * [cos(atan2(θ, s)) + sin(atan2(θ, s)) * v̂]
|
||||
|
||||
Substitutions:
|
||||
cos(atan2(θ, s)) = s / √(θ² + s²)
|
||||
sin(atan2(θ, s)) = θ / √(θ² + s²)
|
||||
|
||||
exp(log(𝑞)) = ||𝑞|| * [s / √(θ² + s²) + θ / √(θ² + s²) * v̂]
|
||||
|
||||
√(θ² + s²) = ||𝑞||
|
||||
|
||||
exp(log(𝑞)) = ||𝑞|| * [s / ||𝑞|| + θ / ||𝑞|| * v̂]
|
||||
exp(log(𝑞)) = s + θ * v̂
|
||||
|
||||
exp(log(𝑞)) = s + 𝑣⃗
|
||||
|
||||
exp(log(𝑞)) = 𝑞
|
||||
```
|
||||
|
||||
## Unit Quaternion in SO(3) from Rotation Vector in 𝖘𝖔(3)
|
||||
|
||||
We will take it as a given that members of 𝖘𝖔(3) take the form `𝑣⃗ = θ * v̂`, representing a rotation θ around a unit axis v̂.
|
||||
|
||||
We additionally take it as a given that quaternions in SO(3) are of the form `𝑞 = cos(θ / 2) + sin(θ / 2) * v̂`, representing a rotation of θ around unit axis v̂.
|
||||
|
||||
```
|
||||
θ = ||𝑣⃗||
|
||||
v̂ = 𝑣⃗ / θ
|
||||
|
||||
𝑞 = cos(θ / 2) + sin(θ / 2) * v̂
|
||||
𝑞 = cos(||𝑣⃗|| / 2) + sin(||𝑣⃗|| / 2) / ||𝑣⃗|| * 𝑣⃗
|
||||
```
|
||||
|
||||
## Rotation vector in 𝖘𝖔(3) from Unit Quaternion in SO(3)
|
||||
|
||||
We will take it as a given that members of 𝖘𝖔(3) take the form `𝑟⃗ = θ * r̂`, representing a rotation θ around a unit axis r̂.
|
||||
|
||||
We additionally take it as a given that quaternions in SO(3) are of the form `𝑞 = s + 𝑣⃗ = cos(θ / 2) + sin(θ / 2) * v̂`, representing a rotation of θ around unit axis v̂.
|
||||
|
||||
```
|
||||
s + 𝑣⃗ = cos(θ / 2) + sin(θ / 2) * v̂
|
||||
s = cos(θ / 2)
|
||||
𝑣⃗ = sin(θ / 2) * v̂
|
||||
||𝑣⃗|| = sin(θ / 2)
|
||||
|
||||
θ / 2 = atan2(||𝑣⃗||, s)
|
||||
θ = 2 * atan2(||𝑣⃗||, s)
|
||||
|
||||
r̂ = 𝑣⃗ / ||𝑣⃗||
|
||||
|
||||
𝑟⃗ = θ * r̂
|
||||
𝑟⃗ = 2 * atan2(||𝑣⃗||, s) / ||𝑣⃗|| * 𝑣⃗
|
||||
```
|
||||
|
||||
@@ -52,6 +52,48 @@ public class Quaternion {
|
||||
m_z = z;
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds another quaternion to this quaternion entrywise.
|
||||
*
|
||||
* @param other The other quaternion.
|
||||
* @return The quaternion sum.
|
||||
*/
|
||||
public Quaternion plus(Quaternion other) {
|
||||
return new Quaternion(
|
||||
getW() + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtracts another quaternion from this quaternion entrywise.
|
||||
*
|
||||
* @param other The other quaternion.
|
||||
* @return The quaternion difference.
|
||||
*/
|
||||
public Quaternion minus(Quaternion other) {
|
||||
return new Quaternion(
|
||||
getW() - other.getW(), getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ());
|
||||
}
|
||||
|
||||
/**
|
||||
* Divides by a scalar.
|
||||
*
|
||||
* @param scalar The value to scale each component by.
|
||||
* @return The scaled quaternion.
|
||||
*/
|
||||
public Quaternion divide(double scalar) {
|
||||
return new Quaternion(getW() / scalar, getX() / scalar, getY() / scalar, getZ() / scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiplies with a scalar.
|
||||
*
|
||||
* @param scalar The value to scale each component by.
|
||||
* @return The scaled quaternion.
|
||||
*/
|
||||
public Quaternion times(double scalar) {
|
||||
return new Quaternion(getW() * scalar, getX() * scalar, getY() * scalar, getZ() * scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiply with another quaternion.
|
||||
*
|
||||
@@ -96,12 +138,8 @@ public class Quaternion {
|
||||
if (obj instanceof Quaternion) {
|
||||
var other = (Quaternion) obj;
|
||||
|
||||
return Math.abs(
|
||||
getW() * other.getW()
|
||||
+ getX() * other.getX()
|
||||
+ getY() * other.getY()
|
||||
+ getZ() * other.getZ())
|
||||
> 1.0 - 1E-9;
|
||||
return Math.abs(dot(other) - norm() * other.norm()) < 1e-9
|
||||
&& Math.abs(norm() - other.norm()) < 1e-9;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -111,13 +149,45 @@ public class Quaternion {
|
||||
return Objects.hash(m_w, m_x, m_y, m_z);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the conjugate of the quaternion.
|
||||
*
|
||||
* @return The conjugate quaternion.
|
||||
*/
|
||||
public Quaternion conjugate() {
|
||||
return new Quaternion(getW(), -getX(), -getY(), -getZ());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the elementwise product of two quaternions.
|
||||
*
|
||||
* @param other The other quaternion.
|
||||
* @return The dot product of two quaternions.
|
||||
*/
|
||||
public double dot(final Quaternion other) {
|
||||
return getW() * other.getW()
|
||||
+ getX() * other.getX()
|
||||
+ getY() * other.getY()
|
||||
+ getZ() * other.getZ();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the inverse of the quaternion.
|
||||
*
|
||||
* @return The inverse quaternion.
|
||||
*/
|
||||
public Quaternion inverse() {
|
||||
return new Quaternion(getW(), -getX(), -getY(), -getZ());
|
||||
var norm = norm();
|
||||
return conjugate().divide(norm * norm);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the L2 norm of the quaternion.
|
||||
*
|
||||
* @return The L2 norm.
|
||||
*/
|
||||
public double norm() {
|
||||
return Math.sqrt(dot(this));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -126,7 +196,7 @@ public class Quaternion {
|
||||
* @return The normalized quaternion.
|
||||
*/
|
||||
public Quaternion normalize() {
|
||||
double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
|
||||
double norm = norm();
|
||||
if (norm == 0.0) {
|
||||
return new Quaternion();
|
||||
} else {
|
||||
@@ -134,6 +204,104 @@ public class Quaternion {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Rational power of a quaternion.
|
||||
*
|
||||
* @param t the power to raise this quaternion to.
|
||||
* @return The quaternion power
|
||||
*/
|
||||
public Quaternion pow(double t) {
|
||||
// q^t = e^(ln(q^t)) = e^(t * ln(q))
|
||||
return this.log().times(t).exp();
|
||||
}
|
||||
|
||||
/**
|
||||
* Matrix exponential of a quaternion.
|
||||
*
|
||||
* @param adjustment the "Twist" that will be applied to this quaternion.
|
||||
* @return The quaternion product of exp(adjustment) * this
|
||||
*/
|
||||
public Quaternion exp(Quaternion adjustment) {
|
||||
return adjustment.exp().times(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Matrix exponential of a quaternion.
|
||||
*
|
||||
* <p>source: wpimath/algorithms.md
|
||||
*
|
||||
* <p>If this quaternion is in 𝖘𝖔(3) and you are looking for an element of SO(3), use {@link
|
||||
* fromRotationVector}
|
||||
*
|
||||
* @return The Matrix exponential of this quaternion.
|
||||
*/
|
||||
public Quaternion exp() {
|
||||
var scalar = Math.exp(getW());
|
||||
|
||||
var axial_magnitude = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
|
||||
var cosine = Math.cos(axial_magnitude);
|
||||
|
||||
double axial_scalar;
|
||||
|
||||
if (axial_magnitude < 1e-9) {
|
||||
// Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(n⁶)
|
||||
var axial_magnitude_sq = axial_magnitude * axial_magnitude;
|
||||
var axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
|
||||
axial_scalar = 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
|
||||
} else {
|
||||
axial_scalar = Math.sin(axial_magnitude) / axial_magnitude;
|
||||
}
|
||||
|
||||
return new Quaternion(
|
||||
cosine * scalar,
|
||||
getX() * axial_scalar * scalar,
|
||||
getY() * axial_scalar * scalar,
|
||||
getZ() * axial_scalar * scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Log operator of a quaternion.
|
||||
*
|
||||
* @param end The quaternion to map this quaternion onto.
|
||||
* @return The "Twist" that maps this quaternion to the argument.
|
||||
*/
|
||||
public Quaternion log(Quaternion end) {
|
||||
return end.times(this.inverse()).log();
|
||||
}
|
||||
|
||||
/**
|
||||
* The Log operator of a general quaternion.
|
||||
*
|
||||
* <p>source: wpimath/algorithms.md
|
||||
*
|
||||
* <p>If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), use {@link
|
||||
* toRotationVector}
|
||||
*
|
||||
* @return The logarithm of this quaternion.
|
||||
*/
|
||||
public Quaternion log() {
|
||||
var scalar = Math.log(norm());
|
||||
|
||||
var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
|
||||
|
||||
var s_norm = getW() / norm();
|
||||
|
||||
if (Math.abs(s_norm + 1) < 1e-9) {
|
||||
return new Quaternion(scalar, -Math.PI, 0, 0);
|
||||
}
|
||||
|
||||
double v_scalar;
|
||||
|
||||
if (v_norm < 1e-9) {
|
||||
// Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴)
|
||||
v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
|
||||
} else {
|
||||
v_scalar = Math.atan2(v_norm, getW()) / v_norm;
|
||||
}
|
||||
|
||||
return new Quaternion(scalar, v_scalar * getX(), v_scalar * getY(), v_scalar * getZ());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns W component of the quaternion.
|
||||
*
|
||||
@@ -174,6 +342,37 @@ public class Quaternion {
|
||||
return m_z;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the quaternion representation of this rotation vector.
|
||||
*
|
||||
* <p>This is also the exp operator of 𝖘𝖔(3).
|
||||
*
|
||||
* <p>source: wpimath/algorithms.md
|
||||
*
|
||||
* @param rvec The rotation vector.
|
||||
* @return The quaternion representation of this rotation vector.
|
||||
*/
|
||||
public static Quaternion fromRotationVector(Vector<N3> rvec) {
|
||||
double theta = rvec.norm();
|
||||
|
||||
double cos = Math.cos(theta / 2);
|
||||
|
||||
double axial_scalar;
|
||||
|
||||
if (theta < 1e-9) {
|
||||
// taylor series expansion of sin(θ/2) / θ = 1/2 - θ²/48 + O(θ⁴)
|
||||
axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
|
||||
} else {
|
||||
axial_scalar = Math.sin(theta / 2) / theta;
|
||||
}
|
||||
|
||||
return new Quaternion(
|
||||
cos,
|
||||
axial_scalar * rvec.get(0, 0),
|
||||
axial_scalar * rvec.get(1, 0),
|
||||
axial_scalar * rvec.get(2, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotation vector representation of this quaternion.
|
||||
*
|
||||
|
||||
@@ -420,7 +420,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Rotation3d) {
|
||||
var other = (Rotation3d) obj;
|
||||
return m_q.equals(other.m_q);
|
||||
return Math.abs(Math.abs(m_q.dot(other.m_q)) - m_q.norm() * other.m_q.norm()) < 1e-9;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
|
||||
#include "frc/geometry/Quaternion.h"
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <wpi/json.h>
|
||||
|
||||
using namespace frc;
|
||||
@@ -11,6 +13,42 @@ using namespace frc;
|
||||
Quaternion::Quaternion(double w, double x, double y, double z)
|
||||
: m_r{w}, m_v{x, y, z} {}
|
||||
|
||||
Quaternion Quaternion::operator+(const Quaternion& other) const {
|
||||
return Quaternion{
|
||||
m_r + other.m_r,
|
||||
m_v(0) + other.m_v(0),
|
||||
m_v(1) + other.m_v(1),
|
||||
m_v(2) + other.m_v(2),
|
||||
};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator-(const Quaternion& other) const {
|
||||
return Quaternion{
|
||||
m_r - other.m_r,
|
||||
m_v(0) - other.m_v(0),
|
||||
m_v(1) - other.m_v(1),
|
||||
m_v(2) - other.m_v(2),
|
||||
};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator*(const double other) const {
|
||||
return Quaternion{
|
||||
m_r * other,
|
||||
m_v(0) * other,
|
||||
m_v(1) * other,
|
||||
m_v(2) * other,
|
||||
};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator/(const double other) const {
|
||||
return Quaternion{
|
||||
m_r / other,
|
||||
m_v(0) / other,
|
||||
m_v(1) / other,
|
||||
m_v(2) / other,
|
||||
};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator*(const Quaternion& other) const {
|
||||
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
|
||||
const auto& r1 = m_r;
|
||||
@@ -33,22 +71,95 @@ Quaternion Quaternion::operator*(const Quaternion& other) const {
|
||||
}
|
||||
|
||||
bool Quaternion::operator==(const Quaternion& other) const {
|
||||
return std::abs(W() * other.W() + m_v.dot(other.m_v)) > 1.0 - 1E-9;
|
||||
return std::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
|
||||
std::abs(Norm() - other.Norm()) < 1e-9;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Inverse() const {
|
||||
Quaternion Quaternion::Conjugate() const {
|
||||
return Quaternion{W(), -X(), -Y(), -Z()};
|
||||
}
|
||||
|
||||
double Quaternion::Dot(const Quaternion& other) const {
|
||||
return W() * other.W() + m_v.dot(other.m_v);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Inverse() const {
|
||||
double norm = Norm();
|
||||
return Conjugate() / (norm * norm);
|
||||
}
|
||||
|
||||
double Quaternion::Norm() const {
|
||||
return std::sqrt(Dot(*this));
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Normalize() const {
|
||||
double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
|
||||
double norm = Norm();
|
||||
if (norm == 0.0) {
|
||||
return Quaternion{};
|
||||
} else {
|
||||
return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
|
||||
return Quaternion{W(), X(), Y(), Z()} / norm;
|
||||
}
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Pow(const double other) const {
|
||||
return (Log() * other).Exp();
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Exp(const Quaternion& other) const {
|
||||
return other.Exp() * *this;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Exp() const {
|
||||
double scalar = std::exp(m_r);
|
||||
|
||||
double axial_magnitude = m_v.norm();
|
||||
double cosine = std::cos(axial_magnitude);
|
||||
|
||||
double axial_scalar;
|
||||
|
||||
if (axial_magnitude < 1e-9) {
|
||||
// Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶)
|
||||
double axial_magnitude_sq = axial_magnitude * axial_magnitude;
|
||||
double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
|
||||
axial_scalar =
|
||||
1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
|
||||
} else {
|
||||
axial_scalar = std::sin(axial_magnitude) / axial_magnitude;
|
||||
}
|
||||
|
||||
return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
|
||||
Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Log(const Quaternion& other) const {
|
||||
return (other * Inverse()).Log();
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Log() const {
|
||||
double scalar = std::log(Norm());
|
||||
|
||||
double v_norm = m_v.norm();
|
||||
|
||||
double s_norm = W() / Norm();
|
||||
|
||||
if (std::abs(s_norm + 1) < 1e-9) {
|
||||
return Quaternion{scalar, -std::numbers::pi, 0, 0};
|
||||
}
|
||||
|
||||
double v_scalar;
|
||||
|
||||
if (v_norm < 1e-9) {
|
||||
// Taylor series expansion of atan2(y / x) / y around y = 0 = 1/x -
|
||||
// y^2/3*x^3 + O(y^4)
|
||||
v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
|
||||
} else {
|
||||
v_scalar = std::atan2(v_norm, W()) / v_norm;
|
||||
}
|
||||
|
||||
return Quaternion{scalar, v_scalar * m_v(0), v_scalar * m_v(1),
|
||||
v_scalar * m_v(2)};
|
||||
}
|
||||
|
||||
double Quaternion::W() const {
|
||||
return m_r;
|
||||
}
|
||||
@@ -83,6 +194,30 @@ Eigen::Vector3d Quaternion::ToRotationVector() const {
|
||||
}
|
||||
}
|
||||
|
||||
Quaternion Quaternion::FromRotationVector(const Eigen::Vector3d& rvec) {
|
||||
// 𝑣⃗ = θ * v̂
|
||||
// v̂ = 𝑣⃗ / θ
|
||||
|
||||
// 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
|
||||
// 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
|
||||
|
||||
double theta = rvec.norm();
|
||||
double cos = std::cos(theta / 2);
|
||||
|
||||
double axial_scalar;
|
||||
|
||||
if (theta < 1e-9) {
|
||||
// taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 +
|
||||
// O(θ⁴)
|
||||
axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
|
||||
} else {
|
||||
axial_scalar = std::sin(theta / 2) / theta;
|
||||
}
|
||||
|
||||
return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
|
||||
axial_scalar * rvec(2)};
|
||||
}
|
||||
|
||||
void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
|
||||
json = wpi::json{{"W", quaternion.W()},
|
||||
{"X", quaternion.X()},
|
||||
|
||||
@@ -174,6 +174,11 @@ Rotation3d Rotation3d::operator/(double scalar) const {
|
||||
return *this * (1.0 / scalar);
|
||||
}
|
||||
|
||||
bool Rotation3d::operator==(const Rotation3d& other) const {
|
||||
return std::abs(std::abs(m_q.Dot(other.m_q)) -
|
||||
m_q.Norm() * other.m_q.Norm()) < 1e-9;
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
|
||||
return Rotation3d{other.m_q * m_q};
|
||||
}
|
||||
|
||||
@@ -27,6 +27,34 @@ class WPILIB_DLLEXPORT Quaternion {
|
||||
*/
|
||||
Quaternion(double w, double x, double y, double z);
|
||||
|
||||
/**
|
||||
* Adds with another quaternion.
|
||||
*
|
||||
* @param other the other quaternion
|
||||
*/
|
||||
Quaternion operator+(const Quaternion& other) const;
|
||||
|
||||
/**
|
||||
* Subtracts another quaternion.
|
||||
*
|
||||
* @param other the other quaternion
|
||||
*/
|
||||
Quaternion operator-(const Quaternion& other) const;
|
||||
|
||||
/**
|
||||
* Multiples with a scalar value.
|
||||
*
|
||||
* @param other the scalar value
|
||||
*/
|
||||
Quaternion operator*(const double other) const;
|
||||
|
||||
/**
|
||||
* Divides by a scalar value.
|
||||
*
|
||||
* @param other the scalar value
|
||||
*/
|
||||
Quaternion operator/(const double other) const;
|
||||
|
||||
/**
|
||||
* Multiply with another quaternion.
|
||||
*
|
||||
@@ -42,6 +70,16 @@ class WPILIB_DLLEXPORT Quaternion {
|
||||
*/
|
||||
bool operator==(const Quaternion& other) const;
|
||||
|
||||
/**
|
||||
* Returns the elementwise product of two quaternions.
|
||||
*/
|
||||
double Dot(const Quaternion& other) const;
|
||||
|
||||
/**
|
||||
* Returns the conjugate of the quaternion.
|
||||
*/
|
||||
Quaternion Conjugate() const;
|
||||
|
||||
/**
|
||||
* Returns the inverse of the quaternion.
|
||||
*/
|
||||
@@ -52,6 +90,52 @@ class WPILIB_DLLEXPORT Quaternion {
|
||||
*/
|
||||
Quaternion Normalize() const;
|
||||
|
||||
/**
|
||||
* Calculates the L2 norm of the quaternion.
|
||||
*/
|
||||
double Norm() const;
|
||||
|
||||
/**
|
||||
* Calculates this quaternion raised to a power.
|
||||
*
|
||||
* @param t the power to raise this quaternion to.
|
||||
*/
|
||||
Quaternion Pow(const double t) const;
|
||||
|
||||
/**
|
||||
* Matrix exponential of a quaternion.
|
||||
*
|
||||
* @param other the "Twist" that will be applied to this quaternion.
|
||||
*/
|
||||
Quaternion Exp(const Quaternion& other) const;
|
||||
|
||||
/**
|
||||
* Matrix exponential of a quaternion.
|
||||
*
|
||||
* source: wpimath/algorithms.md
|
||||
*
|
||||
* If this quaternion is in 𝖘𝖔(3) and you are looking for an element of
|
||||
* SO(3), use FromRotationVector
|
||||
*/
|
||||
Quaternion Exp() const;
|
||||
|
||||
/**
|
||||
* Log operator of a quaternion.
|
||||
*
|
||||
* @param other The quaternion to map this quaternion onto
|
||||
*/
|
||||
Quaternion Log(const Quaternion& other) const;
|
||||
|
||||
/**
|
||||
* Log operator of a quaternion.
|
||||
*
|
||||
* source: wpimath/algorithms.md
|
||||
*
|
||||
* If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3),
|
||||
* use ToRotationVector
|
||||
*/
|
||||
Quaternion Log() const;
|
||||
|
||||
/**
|
||||
* Returns W component of the quaternion.
|
||||
*/
|
||||
@@ -79,6 +163,15 @@ class WPILIB_DLLEXPORT Quaternion {
|
||||
*/
|
||||
Eigen::Vector3d ToRotationVector() const;
|
||||
|
||||
/**
|
||||
* Returns the quaternion representation of this rotation vector.
|
||||
*
|
||||
* This is also the exp operator of 𝖘𝖔(3).
|
||||
*
|
||||
* source: wpimath/algorithms.md
|
||||
*/
|
||||
static Quaternion FromRotationVector(const Eigen::Vector3d& rvec);
|
||||
|
||||
private:
|
||||
// Scalar r in versor form
|
||||
double m_r = 1.0;
|
||||
|
||||
@@ -132,7 +132,7 @@ class WPILIB_DLLEXPORT Rotation3d {
|
||||
/**
|
||||
* Checks equality between this Rotation3d and another object.
|
||||
*/
|
||||
bool operator==(const Rotation3d&) const = default;
|
||||
bool operator==(const Rotation3d&) const;
|
||||
|
||||
/**
|
||||
* Adds the new rotation to the current rotation. The other rotation is
|
||||
|
||||
@@ -4,7 +4,9 @@
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
@@ -14,37 +16,91 @@ class QuaternionTest {
|
||||
void testInit() {
|
||||
// Identity
|
||||
var q1 = new Quaternion();
|
||||
assertEquals(1.0, q1.getW());
|
||||
assertEquals(0.0, q1.getX());
|
||||
assertEquals(0.0, q1.getY());
|
||||
assertEquals(0.0, q1.getZ());
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, q1.getW()),
|
||||
() -> assertEquals(0.0, q1.getX()),
|
||||
() -> assertEquals(0.0, q1.getY()),
|
||||
() -> assertEquals(0.0, q1.getZ()));
|
||||
|
||||
// Normalized
|
||||
var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5);
|
||||
assertEquals(0.5, q2.getW());
|
||||
assertEquals(0.5, q2.getX());
|
||||
assertEquals(0.5, q2.getY());
|
||||
assertEquals(0.5, q2.getZ());
|
||||
assertAll(
|
||||
() -> assertEquals(0.5, q2.getW()),
|
||||
() -> assertEquals(0.5, q2.getX()),
|
||||
() -> assertEquals(0.5, q2.getY()),
|
||||
() -> assertEquals(0.5, q2.getZ()));
|
||||
|
||||
// Unnormalized
|
||||
var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5);
|
||||
assertEquals(0.75, q3.getW());
|
||||
assertEquals(0.3, q3.getX());
|
||||
assertEquals(0.4, q3.getY());
|
||||
assertEquals(0.5, q3.getZ());
|
||||
assertAll(
|
||||
() -> assertEquals(0.75, q3.getW()),
|
||||
() -> assertEquals(0.3, q3.getX()),
|
||||
() -> assertEquals(0.4, q3.getY()),
|
||||
() -> assertEquals(0.5, q3.getZ()));
|
||||
|
||||
q3 = q3.normalize();
|
||||
var q3_norm = q3.normalize();
|
||||
double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
|
||||
assertEquals(0.75 / norm, q3.getW());
|
||||
assertEquals(0.3 / norm, q3.getX());
|
||||
assertEquals(0.4 / norm, q3.getY());
|
||||
assertEquals(0.5 / norm, q3.getZ());
|
||||
assertEquals(
|
||||
1.0,
|
||||
q3.getW() * q3.getW()
|
||||
+ q3.getX() * q3.getX()
|
||||
+ q3.getY() * q3.getY()
|
||||
+ q3.getZ() * q3.getZ());
|
||||
assertAll(
|
||||
() -> assertEquals(0.75 / norm, q3_norm.getW()),
|
||||
() -> assertEquals(0.3 / norm, q3_norm.getX()),
|
||||
() -> assertEquals(0.4 / norm, q3_norm.getY()),
|
||||
() -> assertEquals(0.5 / norm, q3_norm.getZ()),
|
||||
() -> assertEquals(1.0, q3_norm.dot(q3_norm)));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testAddition() {
|
||||
var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
|
||||
var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
|
||||
|
||||
var sum = q.plus(p);
|
||||
assertAll(
|
||||
() -> assertEquals(q.getW() + p.getW(), sum.getW()),
|
||||
() -> assertEquals(q.getX() + p.getX(), sum.getX()),
|
||||
() -> assertEquals(q.getY() + p.getY(), sum.getY()),
|
||||
() -> assertEquals(q.getZ() + p.getZ(), sum.getZ()));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testSubtraction() {
|
||||
var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
|
||||
var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
|
||||
|
||||
var difference = q.minus(p);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(q.getW() - p.getW(), difference.getW()),
|
||||
() -> assertEquals(q.getX() - p.getX(), difference.getX()),
|
||||
() -> assertEquals(q.getY() - p.getY(), difference.getY()),
|
||||
() -> assertEquals(q.getZ() - p.getZ(), difference.getZ()));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testScalarMultiplication() {
|
||||
var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
|
||||
var scalar = 2;
|
||||
|
||||
var product = q.times(scalar);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(q.getW() * scalar, product.getW()),
|
||||
() -> assertEquals(q.getX() * scalar, product.getX()),
|
||||
() -> assertEquals(q.getY() * scalar, product.getY()),
|
||||
() -> assertEquals(q.getZ() * scalar, product.getZ()));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testScalarDivision() {
|
||||
var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
|
||||
var scalar = 2;
|
||||
|
||||
var product = q.divide(scalar);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(q.getW() / scalar, product.getW()),
|
||||
() -> assertEquals(q.getX() / scalar, product.getX()),
|
||||
() -> assertEquals(q.getY() / scalar, product.getY()),
|
||||
() -> assertEquals(q.getZ() / scalar, product.getZ()));
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -59,31 +115,131 @@ class QuaternionTest {
|
||||
// 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
|
||||
// produce a 90° CCW Y rotation
|
||||
var expected = yRot;
|
||||
var actual = zRot.times(yRot).times(xRot);
|
||||
assertEquals(expected.getW(), actual.getW(), 1e-9);
|
||||
assertEquals(expected.getX(), actual.getX(), 1e-9);
|
||||
assertEquals(expected.getY(), actual.getY(), 1e-9);
|
||||
assertEquals(expected.getZ(), actual.getZ(), 1e-9);
|
||||
final var actual = zRot.times(yRot).times(xRot);
|
||||
assertAll(
|
||||
() -> assertEquals(expected.getW(), actual.getW(), 1e-9),
|
||||
() -> assertEquals(expected.getX(), actual.getX(), 1e-9),
|
||||
() -> assertEquals(expected.getY(), actual.getY(), 1e-9),
|
||||
() -> assertEquals(expected.getZ(), actual.getZ(), 1e-9));
|
||||
|
||||
// Identity
|
||||
var q =
|
||||
new Quaternion(
|
||||
0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594);
|
||||
actual = q.times(q.inverse());
|
||||
assertEquals(1.0, actual.getW());
|
||||
assertEquals(0.0, actual.getX());
|
||||
assertEquals(0.0, actual.getY());
|
||||
assertEquals(0.0, actual.getZ());
|
||||
final var actual2 = q.times(q.inverse());
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, actual2.getW()),
|
||||
() -> assertEquals(0.0, actual2.getX()),
|
||||
() -> assertEquals(0.0, actual2.getY()),
|
||||
() -> assertEquals(0.0, actual2.getZ()));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testConjugate() {
|
||||
var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
|
||||
var inv = q.conjugate();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(q.getW(), inv.getW()),
|
||||
() -> assertEquals(-q.getX(), inv.getX()),
|
||||
() -> assertEquals(-q.getY(), inv.getY()),
|
||||
() -> assertEquals(-q.getZ(), inv.getZ()));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInverse() {
|
||||
var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
|
||||
var inv = q.inverse();
|
||||
var norm = q.norm();
|
||||
|
||||
assertEquals(q.getW(), inv.getW());
|
||||
assertEquals(-q.getX(), inv.getX());
|
||||
assertEquals(-q.getY(), inv.getY());
|
||||
assertEquals(-q.getZ(), inv.getZ());
|
||||
assertAll(
|
||||
() -> assertEquals(q.getW() / (norm * norm), inv.getW(), 1e-10),
|
||||
() -> assertEquals(-q.getX() / (norm * norm), inv.getX(), 1e-10),
|
||||
() -> assertEquals(-q.getY() / (norm * norm), inv.getY(), 1e-10),
|
||||
() -> assertEquals(-q.getZ() / (norm * norm), inv.getZ(), 1e-10));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testNorm() {
|
||||
var q = new Quaternion(3, 4, 12, 84);
|
||||
|
||||
// pythagorean triples (3, 4, 5), (5, 12, 13), (13, 84, 85)
|
||||
assertEquals(q.norm(), 85, 1e-10);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testExponential() {
|
||||
var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
|
||||
var q_exp =
|
||||
new Quaternion(
|
||||
2.81211398529184, -0.392521193481878, -0.588781790222817, -0.785042386963756);
|
||||
|
||||
assertEquals(q_exp, q.exp());
|
||||
}
|
||||
|
||||
@Test
|
||||
void testLogarithm() {
|
||||
var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
|
||||
var q_log =
|
||||
new Quaternion(1.7959088706354, 0.515190292664085, 0.772785438996128, 1.03038058532817);
|
||||
|
||||
assertEquals(q_log, q.log());
|
||||
|
||||
var zero = new Quaternion(0, 0, 0, 0);
|
||||
var one = new Quaternion();
|
||||
|
||||
assertEquals(zero, one.log());
|
||||
|
||||
var i = new Quaternion(0, 1, 0, 0);
|
||||
assertEquals(i.times(Math.PI / 2), i.log());
|
||||
|
||||
var j = new Quaternion(0, 0, 1, 0);
|
||||
assertEquals(j.times(Math.PI / 2), j.log());
|
||||
|
||||
var k = new Quaternion(0, 0, 0, 1);
|
||||
assertEquals(k.times(Math.PI / 2), k.log());
|
||||
assertEquals(i.times(-Math.PI), one.times(-1).log());
|
||||
|
||||
var ln_half = Math.log(0.5);
|
||||
assertEquals(new Quaternion(ln_half, -Math.PI, 0, 0), one.times(-0.5).log());
|
||||
}
|
||||
|
||||
@Test
|
||||
void testLogarithmIsInverseOfExponential() {
|
||||
var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
|
||||
|
||||
// These operations are order-dependent: ln(exp(q)) is congruent
|
||||
// but not necessarily equal to exp(ln(q)) due to the multi-valued nature of the complex
|
||||
// logarithm.
|
||||
|
||||
var q_log_exp = q.log().exp();
|
||||
|
||||
assertEquals(q, q_log_exp);
|
||||
|
||||
var start = new Quaternion(1, 2, 3, 4);
|
||||
var expect = new Quaternion(5, 6, 7, 8);
|
||||
|
||||
var twist = start.log(expect);
|
||||
var actual = start.exp(twist);
|
||||
|
||||
assertEquals(expect, actual);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDotProduct() {
|
||||
var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
|
||||
var p = new Quaternion(5.5, 6.6, 7.7, 8.8);
|
||||
|
||||
assertEquals(
|
||||
q.getW() * p.getW() + q.getX() * p.getX() + q.getY() * p.getY() + q.getZ() * p.getZ(),
|
||||
q.dot(p));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDotProductAsEquality() {
|
||||
var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
|
||||
var q_conj = q.conjugate();
|
||||
|
||||
assertAll(() -> assertEquals(q, q), () -> assertNotEquals(q, q_conj));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -44,6 +44,54 @@ TEST(QuaternionTest, Init) {
|
||||
q3.Z() * q3.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Addition) {
|
||||
Quaternion q{0.1, 0.2, 0.3, 0.4};
|
||||
Quaternion p{0.5, 0.6, 0.7, 0.8};
|
||||
|
||||
auto sum = q + p;
|
||||
|
||||
EXPECT_DOUBLE_EQ(q.W() + p.W(), sum.W());
|
||||
EXPECT_DOUBLE_EQ(q.X() + p.X(), sum.X());
|
||||
EXPECT_DOUBLE_EQ(q.Y() + p.Y(), sum.Y());
|
||||
EXPECT_DOUBLE_EQ(q.Z() + p.Z(), sum.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Subtraction) {
|
||||
Quaternion q{0.1, 0.2, 0.3, 0.4};
|
||||
Quaternion p{0.5, 0.6, 0.7, 0.8};
|
||||
|
||||
auto difference = q - p;
|
||||
|
||||
EXPECT_DOUBLE_EQ(q.W() - p.W(), difference.W());
|
||||
EXPECT_DOUBLE_EQ(q.X() - p.X(), difference.X());
|
||||
EXPECT_DOUBLE_EQ(q.Y() - p.Y(), difference.Y());
|
||||
EXPECT_DOUBLE_EQ(q.Z() - p.Z(), difference.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, ScalarMultiplication) {
|
||||
Quaternion q{0.1, 0.2, 0.3, 0.4};
|
||||
auto scalar = 2;
|
||||
|
||||
auto product = q * scalar;
|
||||
|
||||
EXPECT_DOUBLE_EQ(q.W() * scalar, product.W());
|
||||
EXPECT_DOUBLE_EQ(q.X() * scalar, product.X());
|
||||
EXPECT_DOUBLE_EQ(q.Y() * scalar, product.Y());
|
||||
EXPECT_DOUBLE_EQ(q.Z() * scalar, product.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, ScalarDivision) {
|
||||
Quaternion q{0.1, 0.2, 0.3, 0.4};
|
||||
auto scalar = 2;
|
||||
|
||||
auto product = q / scalar;
|
||||
|
||||
EXPECT_DOUBLE_EQ(q.W() / scalar, product.W());
|
||||
EXPECT_DOUBLE_EQ(q.X() / scalar, product.X());
|
||||
EXPECT_DOUBLE_EQ(q.Y() / scalar, product.Y());
|
||||
EXPECT_DOUBLE_EQ(q.Z() / scalar, product.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Multiply) {
|
||||
// 90° CCW rotations around each axis
|
||||
double c = units::math::cos(90_deg / 2.0);
|
||||
@@ -71,13 +119,104 @@ TEST(QuaternionTest, Multiply) {
|
||||
EXPECT_NEAR(0.0, actual.Z(), 1e-9);
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Conjugate) {
|
||||
Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
|
||||
0.48507125007266594};
|
||||
auto conj = q.Conjugate();
|
||||
|
||||
EXPECT_DOUBLE_EQ(q.W(), conj.W());
|
||||
EXPECT_DOUBLE_EQ(-q.X(), conj.X());
|
||||
EXPECT_DOUBLE_EQ(-q.Y(), conj.Y());
|
||||
EXPECT_DOUBLE_EQ(-q.Z(), conj.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Inverse) {
|
||||
Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
|
||||
0.48507125007266594};
|
||||
auto norm = q.Norm();
|
||||
|
||||
auto inv = q.Inverse();
|
||||
|
||||
EXPECT_DOUBLE_EQ(q.W(), inv.W());
|
||||
EXPECT_DOUBLE_EQ(-q.X(), inv.X());
|
||||
EXPECT_DOUBLE_EQ(-q.Y(), inv.Y());
|
||||
EXPECT_DOUBLE_EQ(-q.Z(), inv.Z());
|
||||
EXPECT_DOUBLE_EQ(q.W() / (norm * norm), inv.W());
|
||||
EXPECT_DOUBLE_EQ(-q.X() / (norm * norm), inv.X());
|
||||
EXPECT_DOUBLE_EQ(-q.Y() / (norm * norm), inv.Y());
|
||||
EXPECT_DOUBLE_EQ(-q.Z() / (norm * norm), inv.Z());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Norm) {
|
||||
Quaternion q{3, 4, 12, 84};
|
||||
auto norm = q.Norm();
|
||||
|
||||
EXPECT_NEAR(85, norm, 1e-9);
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Exponential) {
|
||||
Quaternion q{1.1, 2.2, 3.3, 4.4};
|
||||
Quaternion expect{2.81211398529184, -0.392521193481878, -0.588781790222817,
|
||||
-0.785042386963756};
|
||||
|
||||
auto q_exp = q.Exp();
|
||||
|
||||
EXPECT_EQ(expect, q_exp);
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, Logarithm) {
|
||||
Quaternion q{1.1, 2.2, 3.3, 4.4};
|
||||
Quaternion expect{1.7959088706354, 0.515190292664085, 0.772785438996128,
|
||||
1.03038058532817};
|
||||
|
||||
auto q_log = q.Log();
|
||||
|
||||
EXPECT_EQ(expect, q_log);
|
||||
|
||||
Quaternion zero{0, 0, 0, 0};
|
||||
Quaternion one{1, 0, 0, 0};
|
||||
Quaternion i{0, 1, 0, 0};
|
||||
Quaternion j{0, 0, 1, 0};
|
||||
Quaternion k{0, 0, 0, 1};
|
||||
Quaternion ln_half{std::log(0.5), -std::numbers::pi, 0, 0};
|
||||
|
||||
EXPECT_EQ(zero, one.Log());
|
||||
EXPECT_EQ(i * std::numbers::pi / 2, i.Log());
|
||||
EXPECT_EQ(j * std::numbers::pi / 2, j.Log());
|
||||
EXPECT_EQ(k * std::numbers::pi / 2, k.Log());
|
||||
|
||||
EXPECT_EQ(i * -std::numbers::pi, (one * -1).Log());
|
||||
EXPECT_EQ(ln_half, (one * -0.5).Log());
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, LogarithmAndExponentialInverse) {
|
||||
Quaternion q{1.1, 2.2, 3.3, 4.4};
|
||||
|
||||
// These operations are order-dependent: ln(exp(q)) is congruent but not
|
||||
// necessarily equal to exp(ln(q)) due to the multi-valued nature of the
|
||||
// complex logarithm.
|
||||
|
||||
auto q_log_exp = q.Log().Exp();
|
||||
|
||||
EXPECT_EQ(q, q_log_exp);
|
||||
|
||||
Quaternion start{1, 2, 3, 4};
|
||||
Quaternion expect{5, 6, 7, 8};
|
||||
|
||||
auto twist = start.Log(expect);
|
||||
auto actual = start.Exp(twist);
|
||||
|
||||
EXPECT_EQ(expect, actual);
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, DotProduct) {
|
||||
Quaternion q{1.1, 2.2, 3.3, 4.4};
|
||||
Quaternion p{5.5, 6.6, 7.7, 8.8};
|
||||
|
||||
EXPECT_NEAR(q.W() * p.W() + q.X() * p.X() + q.Y() * p.Y() + q.Z() * p.Z(),
|
||||
q.Dot(p), 1e-9);
|
||||
}
|
||||
|
||||
TEST(QuaternionTest, DotProductAsEquality) {
|
||||
Quaternion q{1.1, 2.2, 3.3, 4.4};
|
||||
auto q_conj = q.Conjugate();
|
||||
|
||||
EXPECT_NEAR(q.Dot(q), q.Norm() * q.Norm(), 1e-9);
|
||||
EXPECT_GT(std::abs(q.Dot(q_conj) - q.Norm() * q_conj.Norm()), 1e-9);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user