mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +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
@@ -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