[wpimath] Clean up Java Quaternion class (#4399)

Vector.norm() and Vector.dot() were added to make the implementation
simpler and match the C++ version more closely.
This commit is contained in:
Tyler Veness
2022-09-04 09:45:02 -07:00
committed by GitHub
parent f18dd1905d
commit 20b5bed1cb
5 changed files with 75 additions and 27 deletions

View File

@@ -16,11 +16,14 @@ Quaternion Quaternion::operator*(const Quaternion& other) const {
const auto& r2 = other.m_r;
const auto& v2 = other.m_v;
// v₁ x v₂
Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
v2(0) * v1(2) - v1(0) * v2(2),
v1(0) * v2(1) - v2(0) * v1(1)};
// v = r₁v₂ + r₂v₁ + v₁ x v₂
Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
}