Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make Java Quaternion use doubles instead of Vector (#5525)

Browse Source 
This avoids allocation overhead on construction. times() was also
rewritten to not allocate any temporary objects.

Getter calls in the C++ Quaternion class were modified for parity.
This commit is contained in:
Tyler Veness
2023-08-11 23:27:29 -07:00
committed by GitHub
parent 38c198fa64
commit e8b5d44752
3 changed files with 59 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/main/native/cpp/geometry/Quaternion.cpp
View File

@@ -26,15 +26,18 @@ Quaternion Quaternion::operator*(const Quaternion& other) const {
// 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)};
return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂
r1 * r2 - v1.dot(v2),
// v = r₁v₂ + r₂v₁ + v₁ x v₂
v(0), v(1), v(2)};
}
bool Quaternion::operator==(const Quaternion& other) const {
return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
return std::abs(W() * other.W() + m_v.dot(other.m_v)) > 1.0 - 1E-9;
}
Quaternion Quaternion::Inverse() const {
return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
return Quaternion{W(), -X(), -Y(), -Z()};
}
Quaternion Quaternion::Normalize() const {